YouBotGripper.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public 
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/
#include <youbot_driver/youbot/YouBotGripper.hpp>
namespace youbot
{

YouBotGripper::YouBotGripper(const unsigned int jointNo, const std::string& configFilePath)
{
  // Bouml preserved body begin 0005EFF1
  this->jointNumber = jointNo;
  this->mailboxMsgRetries = 200;
  this->timeTillNextMailboxUpdate = 1; //ms

  ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));
  bar1.reset(new YouBotGripperBar(0, jointNo, configFilePath));
  bar2.reset(new YouBotGripperBar(1, jointNo, configFilePath));

  // Bouml preserved body end 0005EFF1
}

YouBotGripper::~YouBotGripper()
{
  // Bouml preserved body begin 0005F071
  // Bouml preserved body end 0005F071
}

void YouBotGripper::getConfigurationParameter(GripperParameter& parameter) const
{
  // Bouml preserved body begin 0005FBF1
  throw std::runtime_error("Please use YouBotGripperParameter");
  // Bouml preserved body end 0005FBF1
}

void YouBotGripper::setConfigurationParameter(const GripperParameter& parameter)
{
  // Bouml preserved body begin 0005FA71
  throw std::runtime_error("Please use YouBotGripperParameter");
  // Bouml preserved body end 0005FA71
}

void YouBotGripper::getConfigurationParameter(GripperFirmwareVersion& parameter) const
{
  // Bouml preserved body begin 000BEF71

  YouBotSlaveMailboxMsg message;
  parameter.getYouBotMailboxMsg(message);

  bool unvalid = true;
  unsigned int retry = 0;

  ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber);
  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

  do
  {
    if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber))
    {
      unvalid = false;
    }
    else
    {
      SLEEP_MILLISEC(timeTillNextMailboxUpdate);
      retry++;
    }
  } while (retry < mailboxMsgRetries && unvalid);

  if (unvalid)
  {
    this->parseMailboxStatusFlags(message);
    throw std::runtime_error("Unable to get firmware version of the gripper");
    return;
  }

  char versionString[8] = {0};
  //  versionString[0] = message.stctInput.replyAddress;
  versionString[0] = message.stctInput.moduleAddress;
  versionString[1] = message.stctInput.status;
  versionString[2] = message.stctInput.commandNumber;
  versionString[3] = message.stctInput.value >> 24;
  versionString[4] = message.stctInput.value >> 16;
  versionString[5] = message.stctInput.value >> 8;
  versionString[6] = message.stctInput.value & 0xff;

  // LOG(warning) <<"version: "<< versionString;
  int controllerType = 0;
  float firmwareVersion = 0;
  sscanf(versionString, "%dV%f", &controllerType, &firmwareVersion); //KR842V20

  parameter.setParameter(controllerType, firmwareVersion);

  return;
  // Bouml preserved body end 000BEF71
}

void YouBotGripper::setConfigurationParameter(const CalibrateGripper& parameter)
{
  // Bouml preserved body begin 00048271

  char index = 16; // Parameter 0 to 15 of bank 2 are password protected
  YouBotSlaveMailboxMsg IsCalibratedReadMessage;
  IsCalibratedReadMessage.stctOutput.moduleAddress = GRIPPER;
  IsCalibratedReadMessage.stctOutput.commandNumber = GGP;
  IsCalibratedReadMessage.stctOutput.typeNumber = index;
  IsCalibratedReadMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
  IsCalibratedReadMessage.stctOutput.value = 0;
  IsCalibratedReadMessage.stctInput.value = 0;

  YouBotSlaveMailboxMsg IsCalibratedSetMessage;
  IsCalibratedSetMessage.stctOutput.moduleAddress = GRIPPER;
  IsCalibratedSetMessage.stctOutput.commandNumber = SGP;
  IsCalibratedSetMessage.stctOutput.typeNumber = index;
  IsCalibratedSetMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
  IsCalibratedSetMessage.stctOutput.value = 1;

  bool doCalibration = true;
  if (parameter.value == false)
  {
    if (!retrieveValueFromMotorContoller(IsCalibratedReadMessage))
    {
      IsCalibratedReadMessage.stctInput.value = 0;
    }

    if (IsCalibratedReadMessage.stctInput.value == 1)
    {
      doCalibration = false;
    }
  }

  if (doCalibration)
  {
    LOG(info) << "Calibrate Gripper";

    YouBotSlaveMailboxMsg message;

    unsigned int maxenc = 0;
    MaxEncoderValue maxencoder;
    bar1->getConfigurationParameter(maxencoder);
    maxencoder.getParameter(maxenc);

    message.stctOutput.moduleAddress = GRIPPER;
    message.stctOutput.commandNumber = MVP;
    message.stctOutput.typeNumber = 1; //move gripper relative
    message.stctOutput.value = -maxenc;
    message.stctOutput.motorNumber = 0; //move bar 0
    setValueToMotorContoller(message);
    message.stctOutput.motorNumber = 1; //move bar 1
    setValueToMotorContoller(message);

    //open gripper
    TargetPositionReached bar1TargetReched;
    TargetPositionReached bar2TargetReched;
    bool targetReachedBar1 = false;
    bool targetReachedBar2 = false;

    for (int i = 0; i < 40; i++)
    {
      SLEEP_MILLISEC(100);
      bar1->getConfigurationParameter(bar1TargetReched);
      bar1TargetReched.getParameter(targetReachedBar1);
      bar2->getConfigurationParameter(bar2TargetReched);
      bar2TargetReched.getParameter(targetReachedBar2);
      if (targetReachedBar1 && targetReachedBar2)
      {
        break;
      }
    }

    //close gripper
    message.stctOutput.moduleAddress = GRIPPER;
    message.stctOutput.commandNumber = MVP;
    message.stctOutput.typeNumber = 1; //move gripper relative
    message.stctOutput.value = maxenc;
    message.stctOutput.motorNumber = 0; //move bar 0
    setValueToMotorContoller(message);
    message.stctOutput.motorNumber = 1; //move bar 1
    setValueToMotorContoller(message);

    targetReachedBar1 = false;
    targetReachedBar2 = false;

    for (int i = 0; i < 40; i++)
    {
      SLEEP_MILLISEC(100);
      bar1->getConfigurationParameter(bar1TargetReched);
      bar1TargetReched.getParameter(targetReachedBar1);
      bar2->getConfigurationParameter(bar2TargetReched);
      bar2TargetReched.getParameter(targetReachedBar2);
      if (targetReachedBar1 && targetReachedBar2)
        break;
    }

    //stop Gripper motor
    /*
     message.stctOutput.moduleAddress = GRIPPER;
     message.stctOutput.commandNumber = MST;
     message.stctOutput.value = 0;
     message.stctOutput.motorNumber = 0; //move bar 0
     setValueToMotorContoller(message);
     message.stctOutput.motorNumber = 1; //move bar 1
     setValueToMotorContoller(message);
     */

    // set pose to zero as reference
    ActualPosition actualPose;
    actualPose.setParameter(0);
    bar1->setConfigurationParameter(actualPose);
    bar2->setConfigurationParameter(actualPose);

    // set a flag in the user variable to remember that it is calibrated
    this->setValueToMotorContoller(IsCalibratedSetMessage);
  }

  // Bouml preserved body end 00048271
}

void YouBotGripper::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter) const
{
  // Bouml preserved body begin 000DE971
  if (!retrieveValueFromMotorContoller(parameter))
  {
    throw JointParameterException("Unable to get parameter from the gripper");
  }
  this->parseMailboxStatusFlags(parameter);
  // Bouml preserved body end 000DE971
}

void YouBotGripper::getData(const GripperData& data) const
{
  // Bouml preserved body begin 0005FB71
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 0005FB71
}

void YouBotGripper::setData(const GripperData& data)
{
  // Bouml preserved body begin 0005FAF1
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 0005FAF1
}

void YouBotGripper::getData(OneDOFGripperData& data) const
{
  // Bouml preserved body begin 000483F1
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 000483F1
}

void YouBotGripper::setData(const OneDOFGripperData& data)
{
  // Bouml preserved body begin 00048371
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 00048371
}

void YouBotGripper::setData(const GripperBarSpacingSetPoint& barSpacing)
{
  // Bouml preserved body begin 0005F8F1

  GripperBarPositionSetPoint setpointBar1;
  GripperBarPositionSetPoint setpointBar2;

  setpointBar1.barPosition = barSpacing.barSpacing / 2.0;
  setpointBar2.barPosition = barSpacing.barSpacing / 2.0;

  bar1->setData(setpointBar1);
  bar2->setData(setpointBar2);

  // Bouml preserved body end 0005F8F1
}

void YouBotGripper::getData(GripperSensedBarSpacing& barSpacing) const
{
  // Bouml preserved body begin 0005F971
  GripperSensedBarPosition bar1Position;
  GripperSensedBarPosition bar2Position;
  bar1->getData(bar1Position);
  bar2->getData(bar2Position);

  barSpacing.barSpacing = bar1Position.barPosition + bar2Position.barPosition;

  // Bouml preserved body end 0005F971
}

void YouBotGripper::open()
{
  // Bouml preserved body begin 000E3BF1

  MaxEncoderValue maxEnc;
  unsigned int bar1MaxEncoderValue = 0;
  unsigned int bar2MaxEncoderValue = 0;

  bar1->getConfigurationParameter(maxEnc);
  maxEnc.getParameter(bar1MaxEncoderValue);
  bar2->getConfigurationParameter(maxEnc);
  maxEnc.getParameter(bar2MaxEncoderValue);

  GripperBarEncoderSetpoint setpointBar1;
  GripperBarEncoderSetpoint setpointBar2;
  setpointBar1.barEncoder = bar1MaxEncoderValue;
  setpointBar2.barEncoder = bar2MaxEncoderValue;

  bar1->setData(setpointBar1);
  bar2->setData(setpointBar2);

  // Bouml preserved body end 000E3BF1
}

void YouBotGripper::close()
{
  // Bouml preserved body begin 00103D71
  GripperBarEncoderSetpoint setpointBar1;
  GripperBarEncoderSetpoint setpointBar2;
  setpointBar1.barEncoder = 0;
  setpointBar2.barEncoder = 0;

  bar1->setData(setpointBar1);
  bar2->setData(setpointBar2);
  // Bouml preserved body end 00103D71
}

YouBotGripperBar& YouBotGripper::getGripperBar1()
{
  // Bouml preserved body begin 000E0FF1
  if (bar1 == NULL)
    throw std::runtime_error("gripper bar 1 is missing");

  return *bar1;
  // Bouml preserved body end 000E0FF1
}

YouBotGripperBar& YouBotGripper::getGripperBar2()
{
  // Bouml preserved body begin 000E1071
  if (bar2 == NULL)
    throw std::runtime_error("gripper bar 2 is missing");

  return *bar2;
  // Bouml preserved body end 000E1071
}

void YouBotGripper::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg) const
{
  // Bouml preserved body begin 00075C71
  std::stringstream errorMessageStream;
  errorMessageStream << "Joint " << this->jointNumber << ": ";
  std::string errorMessage;
  errorMessage = errorMessageStream.str();

  switch (mailboxMsg.stctInput.status)
  {
    case NO_ERROR:
      break;
    case INVALID_COMMAND:
      LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: "
          << mailboxMsg.stctOutput.commandNumber << " is an invalid command!";
      //    throw JointParameterException(errorMessage + "invalid command");
      break;
    case WRONG_TYPE:
      LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
      //    throw JointParameterException(errorMessage + "wrong type");
      break;
    case INVALID_VALUE:
      LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Value: "
          << mailboxMsg.stctOutput.value << " is a invalid value!";
      //    throw JointParameterException(errorMessage + "invalid value");
      break;
    case CONFIGURATION_EEPROM_LOCKED:
      LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Configuration EEPROM locked";
      //    throw JointParameterException(errorMessage + "configuration EEPROM locked");
      break;
    case COMMAND_NOT_AVAILABLE:
      LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: "
          << mailboxMsg.stctOutput.commandNumber << "Command is not available!";
      //    throw JointParameterException(errorMessage + "command not available");
      break;
  }

  // Bouml preserved body end 00075C71
}

bool YouBotGripper::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg) const
{
  // Bouml preserved body begin 0005EF71

  YouBotSlaveMailboxMsg mailboxMsgBuffer;
  mailboxMsgBuffer = mailboxMsg;
  bool unvalid = true;
  unsigned int retry = 0;

  ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber);
  //    LOG(trace) << "set Output CommandNumber " << (int) mailboxMsgBuffer.stctOutput.commandNumber
  //                  << " moduleAddress " << (int) mailboxMsgBuffer.stctOutput.moduleAddress
  //                  << " motorNumber " << (int) mailboxMsgBuffer.stctOutput.motorNumber
  //                  << " typeNumber " << (int) mailboxMsgBuffer.stctOutput.typeNumber
  //                  << " value " << mailboxMsgBuffer.stctOutput.value;

  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

  do
  {

    if (ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber)
        && mailboxMsgBuffer.stctInput.status == NO_ERROR)
    {
      unvalid = false;
    }
    else
    {
      SLEEP_MILLISEC(timeTillNextMailboxUpdate);
      retry++;
    }
    //      LOG(trace) << "set Input CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber
    //                  << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress
    //                  << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress
    //                  << " status " << (int) mailboxMsgBuffer.stctInput.status
    //                  << " value " << mailboxMsgBuffer.stctInput.value;
  } while (retry < mailboxMsgRetries && unvalid);

  if (unvalid)
  {
    this->parseMailboxStatusFlags(mailboxMsgBuffer);
    return false;
  }
  else
  {
    return true;
  }

  // Bouml preserved body end 0005EF71
}

bool YouBotGripper::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message) const
{
  // Bouml preserved body begin 0005EEF1

  bool unvalid = true;
  unsigned int retry = 0;

  ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber);
  //     LOG(trace) << "get Output CommandNumber " << (int) message.stctOutput.commandNumber
  //                  << " moduleAddress " << (int) message.stctOutput.moduleAddress
  //                  << " motorNumber " << (int) message.stctOutput.motorNumber
  //                  << " typeNumber " << (int) message.stctOutput.typeNumber
  //                  << " value " << message.stctOutput.value
  //                  << " No " << this->jointNumber;

  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

  do
  {

    if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber) && message.stctInput.status == NO_ERROR)
    {
      unvalid = false;
    }
    else
    {
      SLEEP_MILLISEC(timeTillNextMailboxUpdate);
      retry++;
    }
    //      LOG(trace) << "get input CommandNumber " << (int) message.stctInput.commandNumber
    //                 << " moduleAddress " << (int) message.stctInput.moduleAddress
    //                 << " replyAddress " << (int) message.stctInput.replyAddress
    //                 << " status " << (int) message.stctInput.status
    //                 << " value " << message.stctInput.value
    //                 << " No " << this->jointNumber;

  } while (retry < mailboxMsgRetries && unvalid);

  if (unvalid)
  {
    this->parseMailboxStatusFlags(message);
    return false;
  }
  else
  {
    return true;
  }

  // Bouml preserved body end 0005EEF1
}

} // namespace youbot