YouBotJoint.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/YouBotJoint.hpp>
namespace youbot
{

YouBotJoint::YouBotJoint(const unsigned int jointNo, const std::string& configFilePath)
{
  // Bouml preserved body begin 000412F1
  this->storage.jointNumber = jointNo;
  timeTillNextMailboxUpdate = 1; //ms
  mailboxMsgRetries = 100;
  this->storage.inverseMovementDirection = false;
  this->storage.lowerLimit = 0;
  this->storage.upperLimit = 0;
  this->storage.areLimitsActive = false;
  std::stringstream jointNameStream;
  jointNameStream << "Joint " << this->storage.jointNumber << " ";
  this->storage.jointNumberStr = jointNameStream.str();
  ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));

  // Bouml preserved body end 000412F1
}

YouBotJoint::~YouBotJoint()
{
  // Bouml preserved body begin 00041371
  // Bouml preserved body end 00041371
}

void YouBotJoint::setConfigurationParameter(const JointParameter& parameter)
{
  // Bouml preserved body begin 00074271
  throw std::runtime_error("Please use YouBotJointParameters");
  // Bouml preserved body end 00074271
}

void YouBotJoint::getConfigurationParameter(JointParameter& parameter)
{
  // Bouml preserved body begin 0005CE71
  throw std::runtime_error("Please use YouBotJointParameters");
  // Bouml preserved body end 0005CE71
}

void YouBotJoint::getConfigurationParameter(YouBotJointParameterReadOnly& parameter)
{
  // Bouml preserved body begin 00071FF1

  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER)
  {

    YouBotSlaveMailboxMsg message;
    parameter.getYouBotMailboxMsg(message, GAP, storage);

    message.parameterName = parameter.getName();
    if (retrieveValueFromMotorContoller(message))
    {
      parameter.setYouBotMailboxMsg(message, storage);
    }
    else
    {
      throw JointParameterException(
          "Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
    }
  }
  else
  {
    throw JointParameterException(
        "Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
  }
  // Bouml preserved body end 00071FF1
}

void YouBotJoint::getConfigurationParameter(YouBotJointParameter& parameter)
{
  // Bouml preserved body begin 0005BCF1
  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER)
  {

    YouBotSlaveMailboxMsg message;
    parameter.getYouBotMailboxMsg(message, GAP, storage);

    message.parameterName = parameter.getName();
    if (retrieveValueFromMotorContoller(message))
    {
      parameter.setYouBotMailboxMsg(message, storage);
    }
    else
    {
      throw JointParameterException(
          "Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
    }
  }
  else
  {
    throw JointParameterException(
        "Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
  }
  // Bouml preserved body end 0005BCF1
}

void YouBotJoint::setConfigurationParameter(const YouBotJointParameter& parameter)
{
  // Bouml preserved body begin 0005BC71
  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER)
  {

    YouBotSlaveMailboxMsg message;
    parameter.getYouBotMailboxMsg(message, SAP, storage);

    message.parameterName = parameter.getName();
    if (!setValueToMotorContoller(message))
    {
      throw JointParameterException(
          "Unable to set parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
    }
  }
  else
  {
    throw JointParameterException(
        "Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
  }
  // Bouml preserved body end 0005BC71
}

void YouBotJoint::getConfigurationParameter(JointName& parameter)
{
  // Bouml preserved body begin 000740F1
  parameter.value = this->storage.jointName;
  // Bouml preserved body end 000740F1
}

void YouBotJoint::setConfigurationParameter(const JointName& parameter)
{
  // Bouml preserved body begin 0005CDF1
  this->storage.jointName = parameter.value;
  // Bouml preserved body end 0005CDF1
}

void YouBotJoint::getConfigurationParameter(GearRatio& parameter)
{
  // Bouml preserved body begin 00074171
  parameter.setParameter(this->storage.gearRatio);
  // Bouml preserved body end 00074171
}

void YouBotJoint::setConfigurationParameter(const GearRatio& parameter)
{
  // Bouml preserved body begin 00073FF1
  if (parameter.value == 0)
  {
    throw std::out_of_range("A Gear Ratio of zero is not allowed");
  }
  this->storage.gearRatio = parameter.value;
  // Bouml preserved body end 00073FF1
}

void YouBotJoint::getConfigurationParameter(EncoderTicksPerRound& parameter)
{
  // Bouml preserved body begin 000741F1
  parameter.setParameter(this->storage.encoderTicksPerRound);
  // Bouml preserved body end 000741F1
}

void YouBotJoint::setConfigurationParameter(const EncoderTicksPerRound& parameter)
{
  // Bouml preserved body begin 00074071
  if (parameter.value == 0)
  {
    throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
  }
  this->storage.encoderTicksPerRound = parameter.value;
  // Bouml preserved body end 00074071
}

void YouBotJoint::setConfigurationParameter(const CalibrateJoint& parameter)
{
  // Bouml preserved body begin 000623F1
  if (parameter.doCalibration)
  {
    LOG(info) << "Calibrate Joint: " << this->storage.jointName;

    int calibrationVel = 0; //rpm
    //YouBotSlaveMsg messageBuffer;
    messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
    if (parameter.calibrationDirection == POSITIV)
    {
      calibrationVel = 1.0 / storage.gearRatio;
    }
    else if (parameter.calibrationDirection == NEGATIV)
    {
      calibrationVel = -1.0 / storage.gearRatio;
    }
    else
    {
      throw std::runtime_error("No calibration direction for joint: " + this->storage.jointName);
    }

    if (this->storage.inverseMovementDirection == true)
    {
      calibrationVel *= -1;
    }

    JointSensedCurrent sensedCurrent;

    messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
    messageBuffer.stctOutput.value = calibrationVel;
    ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);

    sensedCurrent.current = 0;
    //turn till a max current is reached
    while (abs(sensedCurrent.current) < abs(parameter.maxCurrent))
    {
      SLEEP_MILLISEC(timeTillNextMailboxUpdate);
      this->getData(sensedCurrent);
    }

    //stop movement
    messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
    messageBuffer.stctOutput.value = 0;
    //   LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
    ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);

    //set encoder reference position
    SLEEP_MILLISEC(500);
    messageBuffer.stctOutput.controllerMode = SET_POSITION_TO_REFERENCE;
    messageBuffer.stctOutput.value = 0;
    ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);

    //switch to position controll
    SLEEP_MILLISEC(100);
    messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
    messageBuffer.stctOutput.value = 0;
    //   LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
    ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);

    //     LOG(info) << "Calibration finished for joint: " << this->storage.jointName;
  }

  // Bouml preserved body end 000623F1
}

void YouBotJoint::setConfigurationParameter(const InverseMovementDirection& parameter)
{
  // Bouml preserved body begin 000624F1
  this->storage.inverseMovementDirection = parameter.value;
  // Bouml preserved body end 000624F1
}

void YouBotJoint::getConfigurationParameter(InverseMovementDirection& parameter)
{
  // Bouml preserved body begin 000C9671
  parameter.setParameter(this->storage.inverseMovementDirection);
  // Bouml preserved body end 000C9671
}

void YouBotJoint::setConfigurationParameter(const JointLimits& parameter)
{
  // Bouml preserved body begin 000D4371

  this->storage.lowerLimit = parameter.lowerLimit;
  this->storage.upperLimit = parameter.upperLimit;
  this->storage.areLimitsActive = parameter.areLimitsActive;
  MotorAcceleration acc;
  quantity<angular_acceleration> accValue;

  if (this->storage.areLimitsActive)
  {
    this->getConfigurationParameter(acc);
    acc.getParameter(accValue);
    this->limitMonitor.reset(new JointLimitMonitor(this->storage, accValue));
    ethercatMaster->registerJointLimitMonitor(this->limitMonitor.get(), this->storage.jointNumber);
  }
  else
  {
    this->limitMonitor.reset(NULL);
  }
  //ethercatMaster->setJointLimits(parameter.lowerLimit, parameter.upperLimit, storage.inverseMovementDirection, parameter.areLimitsActive, this->storage.jointNumber);

  // Bouml preserved body end 000D4371
}

void YouBotJoint::getConfigurationParameter(JointLimits& parameter)
{
  // Bouml preserved body begin 000C95F1
  parameter.setParameter(this->storage.lowerLimit, this->storage.upperLimit, this->storage.areLimitsActive);
  // Bouml preserved body end 000C95F1
}

void YouBotJoint::getConfigurationParameter(JointLimitsRadian& parameter)
{
  // Bouml preserved body begin 000D43F1
  quantity<plane_angle> lowlimit = ((double)this->storage.lowerLimit / storage.encoderTicksPerRound) * storage.gearRatio
      * (2.0 * M_PI) * radian;
  quantity<plane_angle> uplimit = ((double)this->storage.upperLimit / storage.encoderTicksPerRound) * storage.gearRatio
      * (2.0 * M_PI) * radian;
  parameter.setParameter(lowlimit, uplimit, this->storage.areLimitsActive);
  // Bouml preserved body end 000D43F1
}

void YouBotJoint::setConfigurationParameter(const InitializeJoint& parameter)
{
  // Bouml preserved body begin 000973F1
  if (parameter.value)
  {
    messageBuffer.stctOutput.controllerMode = INITIALIZE;
    messageBuffer.stctOutput.value = 0;

    ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  }
  // Bouml preserved body end 000973F1
}

void YouBotJoint::getConfigurationParameter(FirmwareVersion& parameter)
{
  // Bouml preserved body begin 0009AA71

  YouBotSlaveMailboxMsg message;
  parameter.getYouBotMailboxMsg(message, GAP, storage);

  bool unvalid = true;
  unsigned int retry = 0;

  ethercatMaster->setMailboxMsgBuffer(message, this->storage.jointNumber);

  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

  do
  {
    if (ethercatMaster->getMailboxMsgBuffer(message, this->storage.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 for joint: " + this->storage.jointName);
    return;
  }

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

  int controllerType = 0;
  char firmwareVersion[9] = {0};
  sscanf(versionString, "%dV%s", &controllerType, firmwareVersion);
  std::string version(firmwareVersion);
  size_t founddot = version.find(".");
  while (founddot != std::string::npos)
  {
    version.erase(founddot, 1);
    founddot = version.find(".");
  }
  parameter.setParameter(controllerType, version);
  return;
  // Bouml preserved body end 0009AA71
}

void YouBotJoint::setConfigurationParameter(const YouBotSlaveMailboxMsg& parameter)
{
  // Bouml preserved body begin 000A9D71
  if (!setValueToMotorContoller(parameter))
  {
    throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
  }
  // Bouml preserved body end 000A9D71
}

void YouBotJoint::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter)
{
  // Bouml preserved body begin 000A9DF1
  if (!retrieveValueFromMotorContoller(parameter))
  {
    throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
  }
  this->parseMailboxStatusFlags(parameter);
  // Bouml preserved body end 000A9DF1
}

void YouBotJoint::getConfigurationParameter(TorqueConstant& parameter)
{
  // Bouml preserved body begin 000D9571
  parameter.setParameter(this->storage.torqueConstant);
  // Bouml preserved body end 000D9571
}

void YouBotJoint::setConfigurationParameter(const TorqueConstant& parameter)
{
  // Bouml preserved body begin 000C7171
  parameter.getParameter(this->storage.torqueConstant);
  // Bouml preserved body end 000C7171
}

void YouBotJoint::storeConfigurationParameterPermanent(const YouBotJointParameter& parameter)
{
  // Bouml preserved body begin 000919F1
  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER)
  {

    this->setConfigurationParameter(parameter);

    YouBotSlaveMailboxMsg message;
    parameter.getYouBotMailboxMsg(message, STAP, storage);

    if (!setValueToMotorContoller(message))
    {
      throw JointParameterException(
          "Unable to store parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
    }
  }
  else
  {
    throw JointParameterException(
        "Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
  }
  // Bouml preserved body end 000919F1
}

void YouBotJoint::restoreConfigurationParameter(YouBotJointParameter& parameter)
{
  // Bouml preserved body begin 00091A71
  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER)
  {

    YouBotSlaveMailboxMsg message;
    parameter.getYouBotMailboxMsg(message, RSAP, storage);

    if (!setValueToMotorContoller(message))
    {
      throw JointParameterException(
          "Unable to restore parameter: " + parameter.getName() + " at joint: " + this->storage.jointName);
    }

    this->getConfigurationParameter(parameter);
  }
  else
  {
    throw JointParameterException(
        "Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
  }
  // Bouml preserved body end 00091A71
}

void YouBotJoint::setData(const JointDataSetpoint& data)
{
  // Bouml preserved body begin 000413F1
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 000413F1
}

void YouBotJoint::getData(JointData& data)
{
  // Bouml preserved body begin 00041471
  LOG(info) << "Nothing to do";
  // Bouml preserved body end 00041471
}

void YouBotJoint::setData(const JointAngleSetpoint& data)
{
  // Bouml preserved body begin 0003C1F1

  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of zero is not allowed");
  }

  if (storage.encoderTicksPerRound == 0)
  {
    throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
  }

  if (this->limitMonitor != 0)
    this->limitMonitor->checkLimitsPositionControl(data.angle);

  messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
  messageBuffer.stctOutput.value = (int32)round(
      (data.angle.value() * ((double)storage.encoderTicksPerRound / (2.0 * M_PI))) / storage.gearRatio);

  if (storage.inverseMovementDirection)
  {
    messageBuffer.stctOutput.value *= -1;
  }
  //   LOG(trace) << "value: " << data.angle << " gear " << gearRatio << " encoderperRound " << encoderTicksPerRound << " encPos " << messageBuffer.stctOutput.value << " joint " << this->storage.jointNumber;
  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 0003C1F1
}

void YouBotJoint::setData(const JointEncoderSetpoint& data)
{
  // Bouml preserved body begin 000C2371

  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (this->limitMonitor != 0)
    this->limitMonitor->checkLimitsEncoderPosition(data.encoderTicks);

  messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
  messageBuffer.stctOutput.value = data.encoderTicks;

  if (storage.inverseMovementDirection)
  {
    messageBuffer.stctOutput.value *= -1;
  }

  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 000C2371
}

void YouBotJoint::getData(JointSensedAngle& data)
{
  // Bouml preserved body begin 0003DCF1
  //YouBotSlaveMsg messageBuffer;
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of zero is not allowed");
  }
  if (storage.encoderTicksPerRound == 0)
  {
    throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
  }
  //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
  data.angle = ((double)messageBuffer.stctInput.actualPosition / storage.encoderTicksPerRound) * storage.gearRatio
      * (2.0 * M_PI) * radian;

  if (storage.inverseMovementDirection)
  {
    data.angle = -data.angle;
  }
  // Bouml preserved body end 0003DCF1
}

void YouBotJoint::setData(const JointVelocitySetpoint& data)
{
  // Bouml preserved body begin 0003C371
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of 0 is not allowed");
  }

  messageBuffer.stctOutput.value = (int32)round(
      (data.angularVelocity.value() / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
  if (storage.inverseMovementDirection)
  {
    messageBuffer.stctOutput.value *= -1;
  }

  //  LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 0003C371
}

void YouBotJoint::getData(JointSensedVelocity& data)
{
  // Bouml preserved body begin 0003DD71
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of 0 is not allowed");
  }
  double motorRPM = messageBuffer.stctInput.actualVelocity;
  //convert RPM of the motor to radian per second of the wheel/joint
  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;

  if (storage.inverseMovementDirection)
  {
    data.angularVelocity *= -1;
  }
  // Bouml preserved body end 0003DD71
}

void YouBotJoint::getData(JointSensedRoundsPerMinute& data)
{
  // Bouml preserved body begin 000AEC71
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  data.rpm = messageBuffer.stctInput.actualVelocity;

  if (storage.inverseMovementDirection)
  {
    data.rpm *= -1;
  }
  // Bouml preserved body end 000AEC71
}

void YouBotJoint::setData(const JointRoundsPerMinuteSetpoint& data)
{
  // Bouml preserved body begin 000AECF1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
  messageBuffer.stctOutput.value = data.rpm;

  if (storage.inverseMovementDirection)
  {
    messageBuffer.stctOutput.value *= -1;
  }

  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 000AECF1
}

void YouBotJoint::getData(JointSensedCurrent& data)
{
  // Bouml preserved body begin 0003DDF1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);
  //convert mili ampere to ampere
  double current = messageBuffer.stctInput.actualCurrent;
  data.current = current / 1000.0 * ampere;

  if (storage.inverseMovementDirection)
  {
    data.current *= -1;
  }
  // Bouml preserved body end 0003DDF1
}

void YouBotJoint::setData(const JointCurrentSetpoint& data)
{
  // Bouml preserved body begin 000955F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  messageBuffer.stctOutput.controllerMode = CURRENT_MODE;
  messageBuffer.stctOutput.value = (int32)(data.current.value() * 1000.0);  //convert from Ampere to milli Ampere

  if (storage.inverseMovementDirection)
  {
    messageBuffer.stctOutput.value *= -1;
  }
  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 000955F1
}

void YouBotJoint::getData(JointSensedEncoderTicks& data)
{
  // Bouml preserved body begin 000AB7F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
  data.encoderTicks = messageBuffer.stctInput.actualPosition;

  if (storage.inverseMovementDirection)
  {
    data.encoderTicks *= -1;
  }

  // Bouml preserved body end 000AB7F1
}

void YouBotJoint::setData(const SlaveMessageOutput& data)
{
  // Bouml preserved body begin 000C5671
  //YouBotSlaveMsg messageBuffer;
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  messageBuffer.stctOutput = data;

  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 000C5671
}

void YouBotJoint::getData(YouBotSlaveMsg& data)
{
  // Bouml preserved body begin 000C56F1
  //YouBotSlaveMsg messageBuffer;
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  data = messageBuffer;
  // Bouml preserved body end 000C56F1
}

void YouBotJoint::setData(const JointTorqueSetpoint& data)
{
  // Bouml preserved body begin 000C7071
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  JointCurrentSetpoint currentSetpoint;

  if (this->storage.torqueConstant == 0)
  {
    throw std::out_of_range("A torque constant of 0 is not allowed");
  }

  currentSetpoint.current = ((data.torque.value() * this->storage.gearRatio) / this->storage.torqueConstant) * ampere;
  this->setData(currentSetpoint);
  // Bouml preserved body end 000C7071
}

void YouBotJoint::getData(JointSensedTorque& data)
{
  // Bouml preserved body begin 000C70F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  JointSensedCurrent sensedCurrent;
  this->getData(sensedCurrent);

  if (this->storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of 0 is not allowed");
  }
  data.torque = ((sensedCurrent.current.value() * this->storage.torqueConstant) / this->storage.gearRatio)
      * newton_meter;
  // Bouml preserved body end 000C70F1
}

void YouBotJoint::getData(JointAngleSetpoint& data)
{
  // Bouml preserved body begin 00103EF1
  //YouBotSlaveMsg messageBuffer;
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of zero is not allowed");
  }
  if (storage.encoderTicksPerRound == 0)
  {
    throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
  }
  //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
  data.angle = ((double)messageBuffer.stctInput.targetPosition / storage.encoderTicksPerRound) * storage.gearRatio
      * (2.0 * M_PI) * radian;

  if (storage.inverseMovementDirection)
  {
    data.angle = -data.angle;
  }
  // Bouml preserved body end 00103EF1
}

void YouBotJoint::getData(JointVelocitySetpoint& data)
{
  // Bouml preserved body begin 001002F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of 0 is not allowed");
  }
  double motorRPM = messageBuffer.stctInput.targetVelocity;
  //convert RPM of the motor to radian per second of the wheel/joint
  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;

  if (storage.inverseMovementDirection)
  {
    data.angularVelocity *= -1;
  }
  // Bouml preserved body end 001002F1
}

void YouBotJoint::getData(JointCurrentSetpoint& data)
{
  // Bouml preserved body begin 00100371
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);
  //convert mili ampere to ampere
  double current = messageBuffer.stctInput.targetCurrent;
  data.current = current / 1000.0 * ampere;

  if (storage.inverseMovementDirection)
  {
    data.current *= -1;
  }
  // Bouml preserved body end 00100371
}

void YouBotJoint::getData(JointRampGeneratorVelocity& data)
{
  // Bouml preserved body begin 001003F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
  this->parseYouBotErrorFlags(messageBuffer);

  if (storage.gearRatio == 0)
  {
    throw std::out_of_range("A Gear Ratio of 0 is not allowed");
  }
  double motorRPM = messageBuffer.stctInput.rampGeneratorVelocity;
  //convert RPM of the motor to radian per second of the wheel/joint
  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;

  if (storage.inverseMovementDirection)
  {
    data.angularVelocity *= -1;
  }
  // Bouml preserved body end 001003F1
}

void YouBotJoint::getUserVariable(const unsigned int index, int& data)
{
  // Bouml preserved body begin 000AD171

  if (index == 0 || index > 55)
  {
    throw JointParameterException("User variable index is out of range use 1-55 at: " + this->storage.jointName);
  }
  //56 is the last userdata at bank 2
  YouBotSlaveMailboxMsg message;
  message.stctOutput.moduleAddress = DRIVE;
  message.stctOutput.commandNumber = GGP;
  message.stctOutput.typeNumber = index;
  message.stctOutput.motorNumber = USER_VARIABLE_BANK;
  message.stctOutput.value = 0;

  if (!retrieveValueFromMotorContoller(message))
  {
    throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
  }
  this->parseMailboxStatusFlags(message);

  data = message.stctInput.value;
  // Bouml preserved body end 000AD171
}

void YouBotJoint::setUserVariable(const unsigned int index, const int data)
{
  // Bouml preserved body begin 000AD1F1

  if (index < 17 || index > 55)
  {
    throw JointParameterException("User variable index is out of range use 17-55 at: " + this->storage.jointName);
  }
  //56 is the last userdata at bank 2
  YouBotSlaveMailboxMsg message;
  message.stctOutput.moduleAddress = DRIVE;
  message.stctOutput.commandNumber = GGP;
  message.stctOutput.typeNumber = index;
  message.stctOutput.motorNumber = USER_VARIABLE_BANK;
  message.stctOutput.value = data;

  if (!setValueToMotorContoller(message))
  {
    throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
  }
  this->parseMailboxStatusFlags(message);
  // Bouml preserved body end 000AD1F1
}

void YouBotJoint::getStatus(std::vector<std::string>& statusMessages)
{
  // Bouml preserved body begin 000AD271
  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);

  if (messageBuffer.stctInput.errorFlags & OVER_CURRENT)
  {
    statusMessages.push_back(this->storage.jointName + " got over current");
  }

  if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE)
  {
    statusMessages.push_back(this->storage.jointName + " got under voltage");
  }

  if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE)
  {
    statusMessages.push_back(this->storage.jointName + " got over voltage");
  }

  if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE)
  {
    statusMessages.push_back(this->storage.jointName + " got over temperature");
  }

  if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED)
  {
    statusMessages.push_back(this->storage.jointName + " is halted");
  }

  if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR)
  {
    statusMessages.push_back(this->storage.jointName + " got hall sensor problem");
  }

//    if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
//      statusMessages.push_back(this->storage.jointName + " got encoder problem");
//    }
//
//     if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
//      statusMessages.push_back(this->storage.jointName + " got inizialization problem");
//    }

//    if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
//      statusMessages.push_back(this->storage.jointName + " has PWM mode active");
//    }

  if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE)
  {
    statusMessages.push_back(this->storage.jointName + " has velocity mode active");
  }

  if (messageBuffer.stctInput.errorFlags & POSITION_MODE)
  {
    statusMessages.push_back(this->storage.jointName + " has position mode active");
  }

  if (messageBuffer.stctInput.errorFlags & TORQUE_MODE)
  {
    statusMessages.push_back(this->storage.jointName + " has torque mode active");
  }

//    if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
//      statusMessages.push_back(this->storage.jointName + " has emergency stop active");
//    }
//
//    if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
//      statusMessages.push_back(this->storage.jointName + " has freerunning active");
//    }

  if (messageBuffer.stctInput.errorFlags & POSITION_REACHED)
  {
    statusMessages.push_back(this->storage.jointName + " has position reached");
  }

  if (messageBuffer.stctInput.errorFlags & INITIALIZED)
  {
    statusMessages.push_back(this->storage.jointName + " is initialized");
  }

  if (messageBuffer.stctInput.errorFlags & TIMEOUT)
  {
    statusMessages.push_back(this->storage.jointName + " has a timeout");
  }

  if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED)
  {
    statusMessages.push_back(this->storage.jointName + " exceeded I2t");
  }

  // Bouml preserved body end 000AD271
}

void YouBotJoint::getStatus(unsigned int& statusFlags)
{
  // Bouml preserved body begin 000AD2F1
  //YouBotSlaveMsg messageBuffer;
  ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);

  statusFlags = messageBuffer.stctInput.errorFlags;
  // Bouml preserved body end 000AD2F1
}

void YouBotJoint::setEncoderToZero()
{
  // Bouml preserved body begin 000AED71
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  YouBotSlaveMailboxMsg message;
  message.stctOutput.commandNumber = SAP;
  message.stctOutput.moduleAddress = DRIVE;
  message.stctOutput.typeNumber = 1; //actual Position
  message.stctOutput.value = 0;

  if (!setValueToMotorContoller(message))
  {
    throw JointParameterException("Unable to set the encoders to zero at joint: " + this->storage.jointName);
  }

  //YouBotSlaveMsg messageBuffer;
  // messageBuffer.stctOutput.controllerMode = SET_POSITION_TO_REFERENCE;
  // messageBuffer.stctOutput.value = 0;

  //ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);

  // Bouml preserved body end 000AED71
}

void YouBotJoint::noMoreAction()
{
  // Bouml preserved body begin 000664F1
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  messageBuffer.stctOutput.controllerMode = NO_MORE_ACTION;
  messageBuffer.stctOutput.value = 0;

  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 000664F1
}

void YouBotJoint::stopJoint()
{
  // Bouml preserved body begin 00066471
  if (!ethercatMaster->isEtherCATConnectionEstablished())
  {
    throw EtherCATConnectionException("No EtherCAT connection");
  }

  //YouBotSlaveMsg messageBuffer;
  messageBuffer.stctOutput.controllerMode = MOTOR_STOP;
  messageBuffer.stctOutput.value = 0;

  ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
  // Bouml preserved body end 00066471
}

unsigned int YouBotJoint::getJointNumber()
{
  // Bouml preserved body begin 000EA2F1
  return this->storage.jointNumber;
  // Bouml preserved body end 000EA2F1
}

void YouBotJoint::parseYouBotErrorFlags(const YouBotSlaveMsg& messageBuffer)
{
  // Bouml preserved body begin 00044AF1

  if (messageBuffer.stctInput.errorFlags & OVER_CURRENT)
  {
    LOG(warning) << this->storage.jointName << " over current";
    //    throw JointErrorException(this->storage.jointName + "got over current");
  }

  if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE)
  {
    LOG(warning) << this->storage.jointName << " under voltage";
    //    throw JointErrorException(this->storage.jointName + "got under voltage");
  }

  if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE)
  {
    LOG(warning) << this->storage.jointName << " over voltage";
    //   throw JointErrorException(this->storage.jointName + "got over voltage");
  }

  if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE)
  {
    LOG(warning) << this->storage.jointName << " over temperature";
    //   throw JointErrorException(this->storage.jointName + "got over temperature");
  }

  if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED)
  {
    //   LOG(info) << this->storage.jointName << " is halted";
    //   throw JointErrorException(this->storage.jointName + "is halted");
  }

  if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR)
  {
    LOG(warning) << this->storage.jointName << " hall sensor problem";
    //   throw JointErrorException(this->storage.jointName + "got hall sensor problem");
  }

//    if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
//      LOG(warning) << this->storage.jointName << " encoder problem";
//      //   throw JointErrorException(this->storage.jointName + "got encoder problem");
//    }
//
//     if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
//      LOG(warning) << this->storage.jointName << " initialization problem";
//      //   throw JointErrorException(this->storage.jointName + "got motor winding problem");
//    }

//    if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
  //   LOG(info) << this->storage.jointName << " has PWM mode active";
  //   throw JointErrorException(this->storage.jointName + "the cycle time is violated");
//    }

  if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE)
  {
    //   LOG(info) << this->storage.jointName << " has velocity mode active";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  if (messageBuffer.stctInput.errorFlags & POSITION_MODE)
  {
    //   LOG(info) << this->storage.jointName << " has position mode active";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  if (messageBuffer.stctInput.errorFlags & TORQUE_MODE)
  {
    //   LOG(info) << this->storage.jointName << " has torque mode active";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

//    if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
//      LOG(info) << this->storage.jointName << " emergency stop active";
//      //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
//    }
//
//    if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
//   //   LOG(info) << this->storage.jointName << " has freerunning active";
//      //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
//    }

  if (messageBuffer.stctInput.errorFlags & POSITION_REACHED)
  {
    //    LOG(info) << this->storage.jointName << " has position reached";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  if (!(messageBuffer.stctInput.errorFlags & INITIALIZED))
  {
    //   LOG(warning) << this->storage.jointName << " not initialized";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  if (messageBuffer.stctInput.errorFlags & TIMEOUT)
  {
    LOG(warning) << this->storage.jointName << " exceeded timeout";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED)
  {
    LOG(warning) << this->storage.jointName << " exceeded I2t";
    //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
  }

  // Bouml preserved body end 00044AF1
}

void YouBotJoint::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg)
{
  // Bouml preserved body begin 00075BF1

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

  // Bouml preserved body end 00075BF1
}

bool YouBotJoint::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message)
{
  // Bouml preserved body begin 0005BD71

  bool unvalid = true;
  unsigned int retry = 0;

  ethercatMaster->setMailboxMsgBuffer(message, this->storage.jointNumber);

  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

  do
  {
    ethercatMaster->getMailboxMsgBuffer(message, this->storage.jointNumber);
    /*     LOG(trace) << "CommandNumber " << (int) message.stctInput.commandNumber
     << " moduleAddress " << (int) message.stctInput.moduleAddress
     << " replyAddress " << (int) message.stctInput.replyAddress
     << " status " << (int) message.stctInput.status
     << " value " << message.stctInput.value; */

    if (message.stctOutput.commandNumber == message.stctInput.commandNumber && message.stctInput.status == NO_ERROR)
    {
      unvalid = false;
    }
    else
    {
      SLEEP_MILLISEC(timeTillNextMailboxUpdate);
      retry++;
    }
  } while (retry < mailboxMsgRetries && unvalid);

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

  // Bouml preserved body end 0005BD71
}

bool YouBotJoint::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg)
{
  // Bouml preserved body begin 00054AF1

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

  ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->storage.jointNumber);

  SLEEP_MILLISEC(timeTillNextMailboxUpdate);

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

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

  // Bouml preserved body end 00054AF1
}

} // namespace youbot