YouBotJoint.cpp

Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011
00004  * All rights reserved.
00005  *
00006  * Hochschule Bonn-Rhein-Sieg
00007  * University of Applied Sciences
00008  * Computer Science Department
00009  *
00010  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00011  *
00012  * Author:
00013  * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
00014  * Supervised by:
00015  * Gerhard K. Kraetzschmar
00016  *
00017  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00018  *
00019  * This sofware is published under a dual-license: GNU Lesser General Public 
00020  * License LGPL 2.1 and BSD license. The dual-license implies that users of this
00021  * code may choose which terms they prefer.
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
00034  *       contributors may be used to endorse or promote products derived from
00035  *       this software without specific prior written permission.
00036  *
00037  * This program is free software: you can redistribute it and/or modify
00038  * it under the terms of the GNU Lesser General Public License LGPL as
00039  * published by the Free Software Foundation, either version 2.1 of the
00040  * License, or (at your option) any later version or the BSD license.
00041  *
00042  * This program is distributed in the hope that it will be useful,
00043  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00044  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00045  * GNU Lesser General Public License LGPL and the BSD license for more details.
00046  *
00047  * You should have received a copy of the GNU Lesser General Public
00048  * License LGPL and BSD license along with this program.
00049  *
00050  ****************************************************************/
00051 #include "youbot/YouBotJoint.hpp"
00052 namespace youbot {
00053 
00054 YouBotJoint::YouBotJoint(const unsigned int jointNo, const std::string& configFilePath) {
00055   // Bouml preserved body begin 000412F1
00056     this->storage.jointNumber = jointNo;
00057     timeTillNextMailboxUpdate = 1; //ms
00058     mailboxMsgRetries = 100;
00059     this->storage.inverseMovementDirection = false;
00060     this->storage.lowerLimit = 0;
00061     this->storage.upperLimit = 0;
00062     this->storage.areLimitsActive = false;
00063     std::stringstream jointNameStream;
00064     jointNameStream << "Joint " << this->storage.jointNumber << " ";
00065     this->storage.jointNumberStr = jointNameStream.str();
00066     ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));
00067     
00068   // Bouml preserved body end 000412F1
00069 }
00070 
00071 YouBotJoint::~YouBotJoint() {
00072   // Bouml preserved body begin 00041371
00073   // Bouml preserved body end 00041371
00074 }
00075 
00076 void YouBotJoint::setConfigurationParameter(const JointParameter& parameter) {
00077   // Bouml preserved body begin 00074271
00078   throw std::runtime_error("Please use YouBotJointParameters");
00079   // Bouml preserved body end 00074271
00080 }
00081 
00082 void YouBotJoint::getConfigurationParameter(JointParameter& parameter) {
00083   // Bouml preserved body begin 0005CE71
00084   throw std::runtime_error("Please use YouBotJointParameters");
00085   // Bouml preserved body end 0005CE71
00086 }
00087 
00088 void YouBotJoint::getConfigurationParameter(YouBotJointParameterReadOnly& parameter) {
00089   // Bouml preserved body begin 00071FF1
00090 
00091     if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
00092 
00093       YouBotSlaveMailboxMsg message;
00094       parameter.getYouBotMailboxMsg(message, GAP, storage);
00095 
00096       message.parameterName = parameter.getName();
00097       if (retrieveValueFromMotorContoller(message)) {
00098         parameter.setYouBotMailboxMsg(message, storage);
00099       } else {
00100         throw JointParameterException("Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
00101       }
00102     }else{
00103       throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
00104     }
00105   // Bouml preserved body end 00071FF1
00106 }
00107 
00108 void YouBotJoint::getConfigurationParameter(YouBotJointParameter& parameter) {
00109   // Bouml preserved body begin 0005BCF1
00110     if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
00111 
00112       YouBotSlaveMailboxMsg message;
00113       parameter.getYouBotMailboxMsg(message, GAP, storage);
00114 
00115       message.parameterName = parameter.getName();
00116       if (retrieveValueFromMotorContoller(message)) {
00117         parameter.setYouBotMailboxMsg(message, storage);
00118       } else {
00119         throw JointParameterException("Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
00120       }
00121     }else{
00122       throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
00123     }
00124   // Bouml preserved body end 0005BCF1
00125 }
00126 
00127 void YouBotJoint::setConfigurationParameter(const YouBotJointParameter& parameter) {
00128   // Bouml preserved body begin 0005BC71
00129     if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
00130 
00131       YouBotSlaveMailboxMsg message;
00132       parameter.getYouBotMailboxMsg(message, SAP, storage);
00133 
00134       message.parameterName = parameter.getName();
00135       if (!setValueToMotorContoller(message)) {
00136         throw JointParameterException("Unable to set parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
00137       }
00138     }else{
00139       throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
00140     }
00141   // Bouml preserved body end 0005BC71
00142 }
00143 
00144 void YouBotJoint::getConfigurationParameter(JointName& parameter) {
00145   // Bouml preserved body begin 000740F1
00146     parameter.value = this->storage.jointName;
00147   // Bouml preserved body end 000740F1
00148 }
00149 
00150 void YouBotJoint::setConfigurationParameter(const JointName& parameter) {
00151   // Bouml preserved body begin 0005CDF1
00152     this->storage.jointName = parameter.value;
00153   // Bouml preserved body end 0005CDF1
00154 }
00155 
00156 void YouBotJoint::getConfigurationParameter(GearRatio& parameter) {
00157   // Bouml preserved body begin 00074171
00158     parameter.setParameter(this->storage.gearRatio);
00159   // Bouml preserved body end 00074171
00160 }
00161 
00162 void YouBotJoint::setConfigurationParameter(const GearRatio& parameter) {
00163   // Bouml preserved body begin 00073FF1
00164     if (parameter.value == 0) {
00165       throw std::out_of_range("A Gear Ratio of zero is not allowed");
00166     }
00167     this->storage.gearRatio = parameter.value;
00168   // Bouml preserved body end 00073FF1
00169 }
00170 
00171 void YouBotJoint::getConfigurationParameter(EncoderTicksPerRound& parameter) {
00172   // Bouml preserved body begin 000741F1
00173     parameter.setParameter(this->storage.encoderTicksPerRound);
00174   // Bouml preserved body end 000741F1
00175 }
00176 
00177 void YouBotJoint::setConfigurationParameter(const EncoderTicksPerRound& parameter) {
00178   // Bouml preserved body begin 00074071
00179     if (parameter.value == 0) {
00180       throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
00181     }
00182     this->storage.encoderTicksPerRound = parameter.value;
00183   // Bouml preserved body end 00074071
00184 }
00185 
00186 void YouBotJoint::setConfigurationParameter(const CalibrateJoint& parameter) {
00187   // Bouml preserved body begin 000623F1
00188     if (parameter.doCalibration) {
00189       LOG(info) << "Calibrate Joint: " << this->storage.jointName;
00190 
00191       int calibrationVel = 0; //rpm
00192       //YouBotSlaveMsg messageBuffer;
00193       messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
00194       if (parameter.calibrationDirection == POSITIV) {
00195         calibrationVel = 1.0 / storage.gearRatio;
00196       } else if (parameter.calibrationDirection == NEGATIV) {
00197         calibrationVel = -1.0 / storage.gearRatio;
00198       } else {
00199         throw std::runtime_error("No calibration direction for joint: " + this->storage.jointName);
00200       }
00201 
00202       if (this->storage.inverseMovementDirection == true) {
00203         calibrationVel *= -1;
00204       }
00205 
00206       JointSensedCurrent sensedCurrent;
00207 
00208       messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
00209       messageBuffer.stctOutput.value = calibrationVel;
00210       ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00211 
00212       sensedCurrent.current = 0;
00213       //turn till a max current is reached
00214       while (abs(sensedCurrent.current) < abs(parameter.maxCurrent)) {
00215         SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00216         this->getData(sensedCurrent);
00217       }
00218 
00219       //stop movement
00220       messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
00221       messageBuffer.stctOutput.value = 0;
00222       //   LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
00223       ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00224 
00225       //set encoder reference position
00226       SLEEP_MILLISEC(500);
00227       messageBuffer.stctOutput.controllerMode = SET_POSITION_TO_REFERENCE;
00228       messageBuffer.stctOutput.value = 0;
00229       ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00230 
00231       //switch to position controll
00232       SLEEP_MILLISEC(100);
00233       messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
00234       messageBuffer.stctOutput.value = 0;
00235       //   LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
00236       ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00237 
00238       //     LOG(info) << "Calibration finished for joint: " << this->storage.jointName;
00239     }
00240 
00241   // Bouml preserved body end 000623F1
00242 }
00243 
00244 void YouBotJoint::setConfigurationParameter(const InverseMovementDirection& parameter) {
00245   // Bouml preserved body begin 000624F1
00246     this->storage.inverseMovementDirection = parameter.value;
00247   // Bouml preserved body end 000624F1
00248 }
00249 
00250 void YouBotJoint::getConfigurationParameter(InverseMovementDirection& parameter) {
00251   // Bouml preserved body begin 000C9671
00252     parameter.setParameter(this->storage.inverseMovementDirection);
00253   // Bouml preserved body end 000C9671
00254 }
00255 
00256 void YouBotJoint::setConfigurationParameter(const JointLimits& parameter) {
00257   // Bouml preserved body begin 000D4371
00258 
00259     this->storage.lowerLimit = parameter.lowerLimit;
00260     this->storage.upperLimit = parameter.upperLimit;
00261     this->storage.areLimitsActive = parameter.areLimitsActive;
00262     MotorAcceleration acc;
00263     quantity<angular_acceleration> accValue;
00264     
00265 
00266     
00267     if(this->storage.areLimitsActive){
00268       this->getConfigurationParameter(acc);
00269       acc.getParameter(accValue);
00270       this->limitMonitor.reset(new JointLimitMonitor(this->storage, accValue));
00271       ethercatMaster->registerJointLimitMonitor(this->limitMonitor.get(), this->storage.jointNumber);
00272     }else{
00273       this->limitMonitor.reset(NULL);
00274     }
00275    //ethercatMaster->setJointLimits(parameter.lowerLimit, parameter.upperLimit, storage.inverseMovementDirection, parameter.areLimitsActive, this->storage.jointNumber);
00276 
00277   // Bouml preserved body end 000D4371
00278 }
00279 
00280 void YouBotJoint::getConfigurationParameter(JointLimits& parameter) {
00281   // Bouml preserved body begin 000C95F1
00282     parameter.setParameter(this->storage.lowerLimit, this->storage.upperLimit, this->storage.areLimitsActive);
00283   // Bouml preserved body end 000C95F1
00284 }
00285 
00286 void YouBotJoint::getConfigurationParameter(JointLimitsRadian& parameter) {
00287   // Bouml preserved body begin 000D43F1
00288     quantity<plane_angle> lowlimit = ((double) this->storage.lowerLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00289     quantity<plane_angle> uplimit = ((double) this->storage.upperLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00290     parameter.setParameter(lowlimit, uplimit, this->storage.areLimitsActive);
00291   // Bouml preserved body end 000D43F1
00292 }
00293 
00294 /// commutation method for firmware 1.48 and below
00295 void YouBotJoint::setConfigurationParameter(const InitializeJoint& parameter) {
00296   // Bouml preserved body begin 000973F1
00297     if (parameter.value) {
00298       messageBuffer.stctOutput.controllerMode = INITIALIZE;
00299       messageBuffer.stctOutput.value = 0;
00300 
00301       ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00302     }
00303   // Bouml preserved body end 000973F1
00304 }
00305 
00306 void YouBotJoint::getConfigurationParameter(FirmwareVersion& parameter) {
00307   // Bouml preserved body begin 0009AA71
00308 
00309     YouBotSlaveMailboxMsg message;
00310     parameter.getYouBotMailboxMsg(message, GAP, storage);
00311 
00312     bool unvalid = true;
00313     unsigned int retry = 0;
00314 
00315     ethercatMaster->setMailboxMsgBuffer(message, this->storage.jointNumber);
00316 
00317     SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00318 
00319     do {
00320       if( ethercatMaster->getMailboxMsgBuffer(message, this->storage.jointNumber) ) {
00321         unvalid = false;
00322       } else {
00323         SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00324         retry++;
00325       }
00326     } while (retry < mailboxMsgRetries && unvalid);
00327 
00328     if (unvalid) {
00329       this->parseMailboxStatusFlags(message);
00330       throw std::runtime_error("Unable to get firmware version for joint: " + this->storage.jointName);
00331       return;
00332     } 
00333     
00334     char versionString[9] = {0}; 
00335     versionString[0] = message.stctInput.replyAddress;
00336     versionString[1] = message.stctInput.moduleAddress;
00337     versionString[2] = message.stctInput.status;
00338     versionString[3] = message.stctInput.commandNumber;
00339     versionString[4] = message.stctInput.value >> 24;
00340     versionString[5] = message.stctInput.value >> 16;
00341     versionString[6] = message.stctInput.value >> 8;
00342     versionString[7] = message.stctInput.value & 0xff;
00343 
00344     int controllerType = 0;
00345     char firmwareVersion[9] = {0};
00346     sscanf (versionString,"%dV%s",&controllerType, firmwareVersion);
00347     std::string version(firmwareVersion);
00348     size_t founddot = version.find(".");
00349     while(founddot != std::string::npos){
00350       version.erase(founddot,1);
00351       founddot = version.find(".");
00352     }
00353     parameter.setParameter(controllerType, version);
00354     return;
00355   // Bouml preserved body end 0009AA71
00356 }
00357 
00358 ///this method should be only used if you know what you are doing
00359 void YouBotJoint::setConfigurationParameter(const YouBotSlaveMailboxMsg& parameter) {
00360   // Bouml preserved body begin 000A9D71
00361    if (!setValueToMotorContoller(parameter)) {
00362      throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
00363    }
00364   // Bouml preserved body end 000A9D71
00365 }
00366 
00367 ///this method should be only used if you know what you are doing
00368 void YouBotJoint::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter) {
00369   // Bouml preserved body begin 000A9DF1
00370    if (!retrieveValueFromMotorContoller(parameter)) {
00371      throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
00372    }
00373    this->parseMailboxStatusFlags(parameter);
00374   // Bouml preserved body end 000A9DF1
00375 }
00376 
00377 void YouBotJoint::getConfigurationParameter(TorqueConstant& parameter) {
00378   // Bouml preserved body begin 000D9571
00379    parameter.setParameter(this->storage.torqueConstant);
00380   // Bouml preserved body end 000D9571
00381 }
00382 
00383 void YouBotJoint::setConfigurationParameter(const TorqueConstant& parameter) {
00384   // Bouml preserved body begin 000C7171
00385    parameter.getParameter(this->storage.torqueConstant);
00386   // Bouml preserved body end 000C7171
00387 }
00388 
00389 ///stores the joint parameter permanent in the EEPROM of the motor contoller
00390 ///Attentions: The EEPROM has only a finite number of program-erase cycles
00391 void YouBotJoint::storeConfigurationParameterPermanent(const YouBotJointParameter& parameter) {
00392   // Bouml preserved body begin 000919F1
00393     if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
00394 
00395       this->setConfigurationParameter(parameter);
00396 
00397       YouBotSlaveMailboxMsg message;
00398       parameter.getYouBotMailboxMsg(message, STAP, storage);
00399 
00400       if (!setValueToMotorContoller(message)) {
00401         throw JointParameterException("Unable to store parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
00402       }
00403     }else{
00404       throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
00405     }
00406   // Bouml preserved body end 000919F1
00407 }
00408 
00409 /// Restores the joint parameter from the EEPROM.
00410 void YouBotJoint::restoreConfigurationParameter(YouBotJointParameter& parameter) {
00411   // Bouml preserved body begin 00091A71
00412     if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
00413 
00414       YouBotSlaveMailboxMsg message;
00415       parameter.getYouBotMailboxMsg(message, RSAP, storage);
00416 
00417       if (!setValueToMotorContoller(message)) {
00418         throw JointParameterException("Unable to restore parameter: " + parameter.getName() + " at joint: " + this->storage.jointName);
00419       }
00420 
00421       this->getConfigurationParameter(parameter);
00422     }else{
00423       throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
00424     }
00425   // Bouml preserved body end 00091A71
00426 }
00427 
00428 void YouBotJoint::setData(const JointDataSetpoint& data) {
00429   // Bouml preserved body begin 000413F1
00430     LOG(info) << "Nothing to do";
00431   // Bouml preserved body end 000413F1
00432 }
00433 
00434 void YouBotJoint::getData(JointData& data) {
00435   // Bouml preserved body begin 00041471
00436     LOG(info) << "Nothing to do";
00437   // Bouml preserved body end 00041471
00438 }
00439 
00440 ///commands a position or angle to one joint
00441 ///@param data the to command position
00442 void YouBotJoint::setData(const JointAngleSetpoint& data) {
00443   // Bouml preserved body begin 0003C1F1
00444 
00445     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00446       throw EtherCATConnectionException("No EtherCAT connection");
00447     }
00448     
00449     //YouBotSlaveMsg messageBuffer;
00450     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00451     this->parseYouBotErrorFlags(messageBuffer);
00452     
00453     if (storage.gearRatio == 0) {
00454       throw std::out_of_range("A Gear Ratio of zero is not allowed");
00455     }
00456     
00457     if (storage.encoderTicksPerRound == 0) {
00458       throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
00459     }
00460 
00461     if (this->limitMonitor != 0)
00462      this->limitMonitor->checkLimitsPositionControl(data.angle);
00463     /*
00464     if(storage.areLimitsActive){
00465 
00466       quantity<plane_angle> lowLimit = ((double) this->storage.lowerLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00467       quantity<plane_angle> upLimit = ((double) this->storage.upperLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00468       
00469       if (storage.inverseMovementDirection) {
00470         upLimit = ((double) -(this->storage.lowerLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00471         lowLimit = ((double) -(this->storage.upperLimit) / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00472       }
00473       
00474 
00475       if (!((data.angle < upLimit) && (data.angle > lowLimit))) {
00476         std::stringstream errorMessageStream;
00477         errorMessageStream << "The setpoint angle is out of range. The valid range is between " << lowLimit << " and " << upLimit << " and it is: " << data.angle;
00478         //    LOG(trace) << "abs_value: " << abs(data.angle) << " abslow " << abs(lowLimit) << " absupper " << abs(upLimit);
00479         throw std::out_of_range(errorMessageStream.str());
00480       }
00481     }
00482     */
00483     messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
00484     messageBuffer.stctOutput.value = (int32) round((data.angle.value() * ((double) storage.encoderTicksPerRound / (2.0 * M_PI))) / storage.gearRatio);
00485 
00486 
00487     if (storage.inverseMovementDirection) {
00488       messageBuffer.stctOutput.value *= -1;
00489     }
00490     //   LOG(trace) << "value: " << data.angle << " gear " << gearRatio << " encoderperRound " << encoderTicksPerRound << " encPos " << messageBuffer.stctOutput.value << " joint " << this->storage.jointNumber;
00491     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00492   // Bouml preserved body end 0003C1F1
00493 }
00494 
00495 ///commands a encoder value (position) to one joint
00496 ///@param data the to command encoder value
00497 void YouBotJoint::setData(const JointEncoderSetpoint& data) {
00498   // Bouml preserved body begin 000C2371
00499 
00500     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00501       throw EtherCATConnectionException("No EtherCAT connection");
00502     }
00503     
00504     //YouBotSlaveMsg messageBuffer;
00505     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00506     this->parseYouBotErrorFlags(messageBuffer);
00507     
00508     if (this->limitMonitor != 0)
00509      this->limitMonitor->checkLimitsEncoderPosition(data.encoderTicks);
00510   
00511     /*
00512     if(storage.areLimitsActive){
00513 
00514       if (!((data.encoderTicks < this->storage.upperLimit) && (data.encoderTicks > this->storage.lowerLimit))) {
00515         std::stringstream errorMessageStream;
00516         errorMessageStream << "The setpoint angle is out of range. The valid range is between " << this->storage.lowerLimit << " and " << this->storage.upperLimit << " and it is: " << data.encoderTicks;
00517         //    LOG(trace) << "abs_value: " << abs(data.angle) << " abslow " << abs(lowLimit) << " absupper " << abs(upLimit);
00518         throw std::out_of_range(errorMessageStream.str());
00519       }
00520     }
00521   */
00522     messageBuffer.stctOutput.controllerMode = POSITION_CONTROL;
00523     messageBuffer.stctOutput.value = data.encoderTicks;
00524     
00525     if (storage.inverseMovementDirection) {
00526       messageBuffer.stctOutput.value *= -1;
00527     }
00528     
00529      ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00530   // Bouml preserved body end 000C2371
00531 }
00532 
00533 ///gets the position or angle of one joint which have been calculated from the actual encoder value 
00534 ///@param data returns the angle by reference
00535 void YouBotJoint::getData(JointSensedAngle& data) {
00536   // Bouml preserved body begin 0003DCF1
00537     //YouBotSlaveMsg messageBuffer;
00538     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00539       throw EtherCATConnectionException("No EtherCAT connection");
00540     }
00541   
00542     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00543     this->parseYouBotErrorFlags(messageBuffer);
00544 
00545     if (storage.gearRatio == 0) {
00546       throw std::out_of_range("A Gear Ratio of zero is not allowed");
00547     }
00548     if (storage.encoderTicksPerRound == 0) {
00549       throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
00550     }
00551     //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
00552     data.angle = ((double) messageBuffer.stctInput.actualPosition / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00553 
00554     if (storage.inverseMovementDirection) {
00555       data.angle = -data.angle;
00556     }
00557   // Bouml preserved body end 0003DCF1
00558 }
00559 
00560 ///commands a velocity to one joint
00561 ///@param data the to command velocity
00562 void YouBotJoint::setData(const JointVelocitySetpoint& data) {
00563   // Bouml preserved body begin 0003C371
00564     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00565       throw EtherCATConnectionException("No EtherCAT connection");
00566     }
00567     
00568     //YouBotSlaveMsg messageBuffer;
00569     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00570     this->parseYouBotErrorFlags(messageBuffer);
00571   
00572     messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
00573 
00574     if (storage.gearRatio == 0) {
00575       throw std::out_of_range("A Gear Ratio of 0 is not allowed");
00576     }
00577 
00578     messageBuffer.stctOutput.value = (int32) round((data.angularVelocity.value() / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
00579     if (storage.inverseMovementDirection) {
00580       messageBuffer.stctOutput.value *= -1;
00581     }
00582 
00583     //  LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
00584     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00585   // Bouml preserved body end 0003C371
00586 }
00587 
00588 ///gets the velocity of one joint
00589 ///@param data returns the velocity by reference
00590 void YouBotJoint::getData(JointSensedVelocity& data) {
00591   // Bouml preserved body begin 0003DD71
00592     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00593       throw EtherCATConnectionException("No EtherCAT connection");
00594     }
00595     
00596     //YouBotSlaveMsg messageBuffer;
00597     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00598     this->parseYouBotErrorFlags(messageBuffer);
00599 
00600     if (storage.gearRatio == 0) {
00601       throw std::out_of_range("A Gear Ratio of 0 is not allowed");
00602     }
00603     double motorRPM = messageBuffer.stctInput.actualVelocity;
00604     //convert RPM of the motor to radian per second of the wheel/joint
00605     data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
00606     
00607     if (storage.inverseMovementDirection) {
00608       data.angularVelocity *= -1;
00609     }
00610   // Bouml preserved body end 0003DD71
00611 }
00612 
00613 ///gets the velocity in round per minute of one joint
00614 ///@param data returns the velocity by reference
00615 void YouBotJoint::getData(JointSensedRoundsPerMinute& data) {
00616   // Bouml preserved body begin 000AEC71
00617     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00618       throw EtherCATConnectionException("No EtherCAT connection");
00619     }
00620     
00621     //YouBotSlaveMsg messageBuffer;
00622     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00623     this->parseYouBotErrorFlags(messageBuffer);
00624 
00625     data.rpm = messageBuffer.stctInput.actualVelocity;
00626     
00627     if (storage.inverseMovementDirection) {
00628       data.rpm *= -1;
00629     }
00630   // Bouml preserved body end 000AEC71
00631 }
00632 
00633 ///sets the velocity in round per minute to one joint
00634 ///@param data the setpoint velocity
00635 void YouBotJoint::setData(const JointRoundsPerMinuteSetpoint& data) {
00636   // Bouml preserved body begin 000AECF1
00637     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00638       throw EtherCATConnectionException("No EtherCAT connection");
00639     }
00640     
00641     //YouBotSlaveMsg messageBuffer;
00642     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00643     this->parseYouBotErrorFlags(messageBuffer);
00644     
00645     messageBuffer.stctOutput.controllerMode = VELOCITY_CONTROL;
00646     messageBuffer.stctOutput.value = data.rpm;
00647     
00648     if (storage.inverseMovementDirection) {
00649       messageBuffer.stctOutput.value *= -1;
00650     }
00651 
00652     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00653   // Bouml preserved body end 000AECF1
00654 }
00655 
00656 ///gets the motor current of one joint which have been measured by a hal sensor
00657 ///@param data returns the actual motor current by reference
00658 void YouBotJoint::getData(JointSensedCurrent& data) {
00659   // Bouml preserved body begin 0003DDF1
00660     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00661       throw EtherCATConnectionException("No EtherCAT connection");
00662     }
00663     
00664     //YouBotSlaveMsg messageBuffer;
00665     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00666     this->parseYouBotErrorFlags(messageBuffer);
00667     //convert mili ampere to ampere
00668     double current = messageBuffer.stctInput.actualCurrent;
00669     data.current =  current / 1000.0 * ampere;
00670     
00671     if (storage.inverseMovementDirection) {
00672       data.current *= -1;
00673     }
00674   // Bouml preserved body end 0003DDF1
00675 }
00676 
00677 ///commands a current to one joint
00678 ///@param data the to command current
00679 void YouBotJoint::setData(const JointCurrentSetpoint& data) {
00680   // Bouml preserved body begin 000955F1
00681     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00682       throw EtherCATConnectionException("No EtherCAT connection");
00683     }
00684     
00685     //YouBotSlaveMsg messageBuffer;
00686     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00687     this->parseYouBotErrorFlags(messageBuffer);
00688     
00689     messageBuffer.stctOutput.controllerMode = CURRENT_MODE;
00690     messageBuffer.stctOutput.value = (int32) (data.current.value() * 1000.0);  //convert from Ampere to milli Ampere
00691     
00692     if (storage.inverseMovementDirection) {
00693       messageBuffer.stctOutput.value *= -1;
00694     }
00695     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00696   // Bouml preserved body end 000955F1
00697 }
00698 
00699 ///gets the encoder ticks of one joint
00700 ///@param data returns the ticks by reference
00701 void YouBotJoint::getData(JointSensedEncoderTicks& data) {
00702   // Bouml preserved body begin 000AB7F1
00703     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00704       throw EtherCATConnectionException("No EtherCAT connection");
00705     }
00706     
00707     //YouBotSlaveMsg messageBuffer;
00708     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00709     this->parseYouBotErrorFlags(messageBuffer);
00710 
00711     //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
00712     data.encoderTicks = messageBuffer.stctInput.actualPosition ;
00713     
00714     if (storage.inverseMovementDirection) {
00715       data.encoderTicks *= -1;
00716     }
00717 
00718   // Bouml preserved body end 000AB7F1
00719 }
00720 
00721 ///sets the output part of a EtherCAT slave message
00722 ///this methode should be only used if you know what you are doing
00723 ///@param data output part of a EtherCAT slave message
00724 void YouBotJoint::setData(const SlaveMessageOutput& data) {
00725   // Bouml preserved body begin 000C5671
00726     //YouBotSlaveMsg messageBuffer;
00727     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00728       throw EtherCATConnectionException("No EtherCAT connection");
00729     }
00730     
00731     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00732     this->parseYouBotErrorFlags(messageBuffer);
00733     
00734     messageBuffer.stctOutput = data;
00735   
00736     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
00737   // Bouml preserved body end 000C5671
00738 }
00739 
00740 ///gets the input and ouput part of a EtherCAT slave message
00741 ///this methode should be only used if you know what you are doing
00742 ///@param data returns the sensor values by reference
00743 void YouBotJoint::getData(YouBotSlaveMsg& data) {
00744   // Bouml preserved body begin 000C56F1
00745     //YouBotSlaveMsg messageBuffer;
00746     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00747       throw EtherCATConnectionException("No EtherCAT connection");
00748     }
00749     
00750     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00751     this->parseYouBotErrorFlags(messageBuffer);
00752 
00753     data = messageBuffer;
00754   // Bouml preserved body end 000C56F1
00755 }
00756 
00757 ///commands a torque to one joint
00758 ///@param data the to command torque
00759 void YouBotJoint::setData(const JointTorqueSetpoint& data) {
00760   // Bouml preserved body begin 000C7071
00761     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00762       throw EtherCATConnectionException("No EtherCAT connection");
00763     }
00764     
00765     JointCurrentSetpoint currentSetpoint;
00766     
00767     if (this->storage.torqueConstant == 0) {
00768       throw std::out_of_range("A torque constant of 0 is not allowed");
00769     }
00770    
00771     currentSetpoint.current = ((data.torque.value()*this->storage.gearRatio)/this->storage.torqueConstant) * ampere;
00772     this->setData(currentSetpoint);
00773   // Bouml preserved body end 000C7071
00774 }
00775 
00776 ///gets the motor torque of one joint which have been calculated from the current
00777 ///@param data returns the actual motor torque by reference
00778 void YouBotJoint::getData(JointSensedTorque& data) {
00779   // Bouml preserved body begin 000C70F1
00780   if(!ethercatMaster->isEtherCATConnectionEstablished()){
00781       throw EtherCATConnectionException("No EtherCAT connection");
00782   }
00783   
00784   JointSensedCurrent sensedCurrent;
00785   this->getData(sensedCurrent);
00786   
00787   if (this->storage.gearRatio == 0) {
00788     throw std::out_of_range("A Gear Ratio of 0 is not allowed");
00789   }
00790   data.torque = ((sensedCurrent.current.value() * this->storage.torqueConstant) / this->storage.gearRatio) * newton_meter;
00791   // Bouml preserved body end 000C70F1
00792 }
00793 
00794 ///gets the target or setpoint position of one joint (only firmware 2.0 or higher)
00795 ///@param data returns the angle by reference
00796 void YouBotJoint::getData(JointAngleSetpoint& data) {
00797   // Bouml preserved body begin 00103EF1
00798     //YouBotSlaveMsg messageBuffer;
00799     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00800       throw EtherCATConnectionException("No EtherCAT connection");
00801     }
00802   
00803     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00804     this->parseYouBotErrorFlags(messageBuffer);
00805 
00806     if (storage.gearRatio == 0) {
00807       throw std::out_of_range("A Gear Ratio of zero is not allowed");
00808     }
00809     if (storage.encoderTicksPerRound == 0) {
00810       throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
00811     }
00812     //  LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
00813     data.angle = ((double) messageBuffer.stctInput.targetPosition / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
00814 
00815     if (storage.inverseMovementDirection) {
00816       data.angle = -data.angle;
00817     }
00818   // Bouml preserved body end 00103EF1
00819 }
00820 
00821 ///gets the target or setpoint velocity of one joint (only firmware 2.0 or higher)
00822 ///@param data returns the velocity by reference
00823 void YouBotJoint::getData(JointVelocitySetpoint& data) {
00824   // Bouml preserved body begin 001002F1
00825     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00826       throw EtherCATConnectionException("No EtherCAT connection");
00827     }
00828     
00829     //YouBotSlaveMsg messageBuffer;
00830     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00831     this->parseYouBotErrorFlags(messageBuffer);
00832 
00833     if (storage.gearRatio == 0) {
00834       throw std::out_of_range("A Gear Ratio of 0 is not allowed");
00835     }
00836     double motorRPM = messageBuffer.stctInput.targetVelocity;
00837     //convert RPM of the motor to radian per second of the wheel/joint
00838     data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
00839     
00840     if (storage.inverseMovementDirection) {
00841       data.angularVelocity *= -1;
00842     }
00843   // Bouml preserved body end 001002F1
00844 }
00845 
00846 ///gets the motor current target or setpoint of one joint (only firmware 2.0 or higher)
00847 ///@param data returns the motor current by reference
00848 void YouBotJoint::getData(JointCurrentSetpoint& data) {
00849   // Bouml preserved body begin 00100371
00850     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00851       throw EtherCATConnectionException("No EtherCAT connection");
00852     }
00853     
00854     //YouBotSlaveMsg messageBuffer;
00855     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00856     this->parseYouBotErrorFlags(messageBuffer);
00857     //convert mili ampere to ampere
00858     double current = messageBuffer.stctInput.targetCurrent;
00859     data.current =  current / 1000.0 * ampere;
00860     
00861     if (storage.inverseMovementDirection) {
00862       data.current *= -1;
00863     }
00864   // Bouml preserved body end 00100371
00865 }
00866 
00867 ///gets the ramp generator velocity of one joint (only firmware 2.0 or higher)
00868 ///@param data returns the velocity by reference
00869 void YouBotJoint::getData(JointRampGeneratorVelocity& data) {
00870   // Bouml preserved body begin 001003F1
00871     if(!ethercatMaster->isEtherCATConnectionEstablished()){
00872       throw EtherCATConnectionException("No EtherCAT connection");
00873     }
00874     
00875     //YouBotSlaveMsg messageBuffer;
00876     ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00877     this->parseYouBotErrorFlags(messageBuffer);
00878 
00879     if (storage.gearRatio == 0) {
00880       throw std::out_of_range("A Gear Ratio of 0 is not allowed");
00881     }
00882     double motorRPM = messageBuffer.stctInput.rampGeneratorVelocity;
00883     //convert RPM of the motor to radian per second of the wheel/joint
00884     data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
00885     
00886     if (storage.inverseMovementDirection) {
00887       data.angularVelocity *= -1;
00888     }
00889   // Bouml preserved body end 001003F1
00890 }
00891 
00892 void YouBotJoint::getUserVariable(const unsigned int index, int& data) {
00893   // Bouml preserved body begin 000AD171
00894   
00895   if(index == 0 || index > 55){
00896     throw JointParameterException("User variable index is out of range use 1-55 at: " + this->storage.jointName);
00897   }
00898   //56 is the last userdata at bank 2
00899     YouBotSlaveMailboxMsg message;
00900     message.stctOutput.moduleAddress = DRIVE;
00901     message.stctOutput.commandNumber = GGP;
00902     message.stctOutput.typeNumber = index;
00903     message.stctOutput.motorNumber = USER_VARIABLE_BANK;
00904     message.stctOutput.value = 0;
00905     
00906    if (!retrieveValueFromMotorContoller(message)) {
00907      throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
00908    }
00909    this->parseMailboxStatusFlags(message);
00910    
00911    data = message.stctInput.value;
00912   // Bouml preserved body end 000AD171
00913 }
00914 
00915 void YouBotJoint::setUserVariable(const unsigned int index, const int data) {
00916   // Bouml preserved body begin 000AD1F1
00917   
00918   if(index < 17 || index > 55){
00919     throw JointParameterException("User variable index is out of range use 17-55 at: " + this->storage.jointName);
00920   }
00921   //56 is the last userdata at bank 2
00922     YouBotSlaveMailboxMsg message;
00923     message.stctOutput.moduleAddress = DRIVE;
00924     message.stctOutput.commandNumber = GGP;
00925     message.stctOutput.typeNumber = index;
00926     message.stctOutput.motorNumber = USER_VARIABLE_BANK;
00927     message.stctOutput.value = data;
00928     
00929   if (!setValueToMotorContoller(message)) {
00930      throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
00931    }
00932     this->parseMailboxStatusFlags(message);
00933   // Bouml preserved body end 000AD1F1
00934 }
00935 
00936 /// Returns the status messages for the motor controller. 
00937 void YouBotJoint::getStatus(std::vector<std::string>& statusMessages) {
00938   // Bouml preserved body begin 000AD271
00939   //YouBotSlaveMsg messageBuffer;
00940   ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
00941   
00942   
00943   
00944 
00945     if (messageBuffer.stctInput.errorFlags & OVER_CURRENT) {
00946       statusMessages.push_back(this->storage.jointName + " got over current");
00947     }
00948 
00949     if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE) {
00950       statusMessages.push_back(this->storage.jointName + " got under voltage");
00951     }
00952 
00953     if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE) {
00954       statusMessages.push_back(this->storage.jointName + " got over voltage");
00955     }
00956 
00957     if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE) {
00958       statusMessages.push_back(this->storage.jointName + " got over temperature");
00959     }
00960 
00961     if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED) {
00962       statusMessages.push_back(this->storage.jointName + " is halted");
00963     }
00964 
00965     if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR) {
00966       statusMessages.push_back(this->storage.jointName + " got hall sensor problem");
00967     }
00968 
00969 //    if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
00970 //      statusMessages.push_back(this->storage.jointName + " got encoder problem");
00971 //    }
00972 //
00973 //     if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
00974 //      statusMessages.push_back(this->storage.jointName + " got inizialization problem");
00975 //    }
00976 
00977 //    if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
00978 //      statusMessages.push_back(this->storage.jointName + " has PWM mode active");
00979 //    }
00980 
00981     if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE) {
00982       statusMessages.push_back(this->storage.jointName + " has velocity mode active");
00983     }
00984 
00985     if (messageBuffer.stctInput.errorFlags & POSITION_MODE) {
00986       statusMessages.push_back(this->storage.jointName + " has position mode active");
00987     }
00988 
00989     if (messageBuffer.stctInput.errorFlags & TORQUE_MODE) {
00990       statusMessages.push_back(this->storage.jointName + " has torque mode active");
00991     }
00992 
00993 //    if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
00994 //      statusMessages.push_back(this->storage.jointName + " has emergency stop active");
00995 //    }
00996 //
00997 //    if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
00998 //      statusMessages.push_back(this->storage.jointName + " has freerunning active");
00999 //    }
01000 
01001     if (messageBuffer.stctInput.errorFlags & POSITION_REACHED) {
01002       statusMessages.push_back(this->storage.jointName + " has position reached");
01003     }
01004 
01005     if (messageBuffer.stctInput.errorFlags & INITIALIZED) {
01006       statusMessages.push_back(this->storage.jointName + " is initialized");
01007     }
01008 
01009     if (messageBuffer.stctInput.errorFlags & TIMEOUT) {
01010       statusMessages.push_back(this->storage.jointName + " has a timeout");
01011     }
01012 
01013     if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED) {
01014       statusMessages.push_back(this->storage.jointName + " exceeded I2t");
01015     }
01016   
01017   
01018   // Bouml preserved body end 000AD271
01019 }
01020 
01021 /// Returns the status messages as status flags for the motor controller. The status flag bits are assigned like this:
01022 /// 0:  Overcurrent
01023 /// 1:  Undervoltage
01024 /// 2:  Overvoltage
01025 /// 3:  Overtemperature
01026 /// 4:  Motor halted
01027 /// 5:  Hall error flag
01028 /// 6:  ---
01029 /// 7:  ---
01030 /// 8:  PWM mode active
01031 /// 9:  Velocity mode active
01032 /// 10: Position mode active
01033 /// 11: Torque mode active
01034 /// 12: ---
01035 /// 13: ---
01036 /// 14: Position end flag
01037 /// 15: Module initialized
01038 /// 16: EtherCAT timeout flag
01039 /// 17: I2t exceeded flag
01040 void YouBotJoint::getStatus(unsigned int& statusFlags) {
01041   // Bouml preserved body begin 000AD2F1
01042   //YouBotSlaveMsg messageBuffer;
01043   ethercatMaster->getMsgBuffer(this->storage.jointNumber, messageBuffer);
01044   
01045   statusFlags = messageBuffer.stctInput.errorFlags;
01046   // Bouml preserved body end 000AD2F1
01047 }
01048 
01049 /// set the encoder values of the joint to zero. This postion will be the new reference.
01050 void YouBotJoint::setEncoderToZero() {
01051   // Bouml preserved body begin 000AED71
01052     if(!ethercatMaster->isEtherCATConnectionEstablished()){
01053       throw EtherCATConnectionException("No EtherCAT connection");
01054     }
01055 
01056     YouBotSlaveMailboxMsg message;
01057     message.stctOutput.commandNumber = SAP;
01058     message.stctOutput.moduleAddress = DRIVE;
01059     message.stctOutput.typeNumber = 1; //actual Position
01060     message.stctOutput.value = 0;
01061 
01062     if (!setValueToMotorContoller(message)) {
01063       throw JointParameterException("Unable to set the encoders to zero at joint: " + this->storage.jointName);
01064     }
01065 
01066     //YouBotSlaveMsg messageBuffer;
01067     // messageBuffer.stctOutput.controllerMode = SET_POSITION_TO_REFERENCE;
01068     // messageBuffer.stctOutput.value = 0;
01069 
01070     //ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
01071 
01072 
01073   // Bouml preserved body end 000AED71
01074 }
01075 
01076 void YouBotJoint::noMoreAction() {
01077   // Bouml preserved body begin 000664F1
01078     if(!ethercatMaster->isEtherCATConnectionEstablished()){
01079       throw EtherCATConnectionException("No EtherCAT connection");
01080     }
01081     
01082     //YouBotSlaveMsg messageBuffer;
01083     messageBuffer.stctOutput.controllerMode = NO_MORE_ACTION;
01084     messageBuffer.stctOutput.value = 0;
01085 
01086     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
01087   // Bouml preserved body end 000664F1
01088 }
01089 
01090 void YouBotJoint::stopJoint() {
01091   // Bouml preserved body begin 00066471
01092     if(!ethercatMaster->isEtherCATConnectionEstablished()){
01093       throw EtherCATConnectionException("No EtherCAT connection");
01094     }
01095     
01096     //YouBotSlaveMsg messageBuffer;
01097     messageBuffer.stctOutput.controllerMode = MOTOR_STOP;
01098     messageBuffer.stctOutput.value = 0;
01099 
01100     ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
01101   // Bouml preserved body end 00066471
01102 }
01103 
01104 unsigned int YouBotJoint::getJointNumber() {
01105   // Bouml preserved body begin 000EA2F1
01106   return this->storage.jointNumber;
01107   // Bouml preserved body end 000EA2F1
01108 }
01109 
01110 void YouBotJoint::parseYouBotErrorFlags(const YouBotSlaveMsg& messageBuffer) {
01111   // Bouml preserved body begin 00044AF1
01112 
01113     if (messageBuffer.stctInput.errorFlags & OVER_CURRENT) {
01114       LOG(warning) << this->storage.jointName << " over current";
01115       //    throw JointErrorException(this->storage.jointName + "got over current");
01116     }
01117 
01118     if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE) {
01119       LOG(warning) << this->storage.jointName << " under voltage";
01120       //    throw JointErrorException(this->storage.jointName + "got under voltage");
01121     }
01122 
01123     if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE) {
01124       LOG(warning) << this->storage.jointName << " over voltage";
01125       //   throw JointErrorException(this->storage.jointName + "got over voltage");
01126     }
01127 
01128     if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE) {
01129       LOG(warning) << this->storage.jointName << " over temperature";
01130       //   throw JointErrorException(this->storage.jointName + "got over temperature");
01131     }
01132 
01133     if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED) {
01134       //   LOG(info) << this->storage.jointName << " is halted";
01135       //   throw JointErrorException(this->storage.jointName + "is halted");
01136     }
01137 
01138     if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR) {
01139       LOG(warning) << this->storage.jointName << " hall sensor problem";
01140       //   throw JointErrorException(this->storage.jointName + "got hall sensor problem");
01141     }
01142 
01143 //    if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
01144 //      LOG(warning) << this->storage.jointName << " encoder problem";
01145 //      //   throw JointErrorException(this->storage.jointName + "got encoder problem");
01146 //    }
01147 //
01148 //     if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
01149 //      LOG(warning) << this->storage.jointName << " initialization problem";
01150 //      //   throw JointErrorException(this->storage.jointName + "got motor winding problem");
01151 //    }
01152 
01153 //    if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
01154    //   LOG(info) << this->storage.jointName << " has PWM mode active";
01155       //   throw JointErrorException(this->storage.jointName + "the cycle time is violated");
01156 //    }
01157 
01158     if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE) {
01159    //   LOG(info) << this->storage.jointName << " has velocity mode active";
01160       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01161     }
01162 
01163     if (messageBuffer.stctInput.errorFlags & POSITION_MODE) {
01164    //   LOG(info) << this->storage.jointName << " has position mode active";
01165       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01166     }
01167 
01168     if (messageBuffer.stctInput.errorFlags & TORQUE_MODE) {
01169    //   LOG(info) << this->storage.jointName << " has torque mode active";
01170       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01171     }
01172 
01173 //    if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
01174 //      LOG(info) << this->storage.jointName << " emergency stop active";
01175 //      //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01176 //    }
01177 //
01178 //    if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
01179 //   //   LOG(info) << this->storage.jointName << " has freerunning active";
01180 //      //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01181 //    }
01182 
01183     if (messageBuffer.stctInput.errorFlags & POSITION_REACHED) {
01184   //    LOG(info) << this->storage.jointName << " has position reached";
01185       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01186     }
01187 
01188     if (!(messageBuffer.stctInput.errorFlags & INITIALIZED)) {
01189    //   LOG(warning) << this->storage.jointName << " not initialized";
01190       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01191     }
01192 
01193     if (messageBuffer.stctInput.errorFlags & TIMEOUT) {
01194       LOG(warning) << this->storage.jointName << " exceeded timeout";
01195       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01196     }
01197 
01198     if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED) {
01199       LOG(warning) << this->storage.jointName << " exceeded I2t";
01200       //   throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
01201     }
01202 
01203   // Bouml preserved body end 00044AF1
01204 }
01205 
01206 void YouBotJoint::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg) {
01207   // Bouml preserved body begin 00075BF1
01208 
01209     switch(mailboxMsg.stctInput.status){
01210       case NO_ERROR:
01211         break;
01212       case INVALID_COMMAND:
01213         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << " is an invalid command!" ;
01214       //    throw JointParameterException(this->storage.jointName + "invalid command");
01215         break;
01216       case WRONG_TYPE:
01217         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
01218       //    throw JointParameterException(this->storage.jointName + "wrong type");
01219         break;
01220       case INVALID_VALUE:
01221         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!";
01222       //    throw JointParameterException(this->storage.jointName + "invalid value");
01223         break;
01224       case CONFIGURATION_EEPROM_LOCKED:
01225         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Configuration EEPROM locked";
01226       //    throw JointParameterException(this->storage.jointName + "configuration EEPROM locked");
01227         break;
01228       case COMMAND_NOT_AVAILABLE:
01229         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Command is not available!";
01230       //    throw JointParameterException(this->storage.jointName + "command not available");
01231         break;
01232       case REPLY_WRITE_PROTECTED:
01233         LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Permissions denied!";
01234       //    throw JointParameterException(this->storage.jointName + "command not available");
01235         break;
01236     }
01237    
01238 
01239   // Bouml preserved body end 00075BF1
01240 }
01241 
01242 bool YouBotJoint::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message) {
01243   // Bouml preserved body begin 0005BD71
01244 
01245     bool unvalid = true;
01246     unsigned int retry = 0;
01247 
01248     ethercatMaster->setMailboxMsgBuffer(message, this->storage.jointNumber);
01249 
01250     SLEEP_MILLISEC(timeTillNextMailboxUpdate);
01251 
01252     do {
01253       ethercatMaster->getMailboxMsgBuffer(message, this->storage.jointNumber);
01254     /*     LOG(trace) << "CommandNumber " << (int) message.stctInput.commandNumber
01255                  << " moduleAddress " << (int) message.stctInput.moduleAddress
01256                  << " replyAddress " << (int) message.stctInput.replyAddress
01257                  << " status " << (int) message.stctInput.status
01258                  << " value " << message.stctInput.value; */
01259        
01260       if (message.stctOutput.commandNumber == message.stctInput.commandNumber &&
01261               message.stctInput.status == NO_ERROR) {
01262         unvalid = false;
01263       } else {
01264         SLEEP_MILLISEC(timeTillNextMailboxUpdate);
01265         retry++;
01266       }
01267     } while (retry < mailboxMsgRetries && unvalid);
01268 
01269     if (unvalid) {
01270       this->parseMailboxStatusFlags(message);
01271       return false;
01272     } else {
01273       return true;
01274     }
01275 
01276   // Bouml preserved body end 0005BD71
01277 }
01278 
01279 bool YouBotJoint::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg) {
01280   // Bouml preserved body begin 00054AF1
01281 
01282     YouBotSlaveMailboxMsg mailboxMsgBuffer;
01283     mailboxMsgBuffer = mailboxMsg;
01284     bool unvalid = true;
01285     unsigned int retry = 0;
01286 
01287     ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->storage.jointNumber);
01288 
01289     SLEEP_MILLISEC(timeTillNextMailboxUpdate);
01290 
01291     do {
01292       ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->storage.jointNumber);
01293       /*    LOG(trace) << "CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber
01294                   << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress
01295                   << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress
01296                   << " status " << (int) mailboxMsgBuffer.stctInput.status
01297                   << " value " << mailboxMsgBuffer.stctInput.value;
01298        */
01299       if (mailboxMsgBuffer.stctOutput.commandNumber == mailboxMsgBuffer.stctInput.commandNumber &&
01300               mailboxMsgBuffer.stctOutput.value == mailboxMsgBuffer.stctInput.value &&
01301               mailboxMsgBuffer.stctInput.status == NO_ERROR) {
01302         unvalid = false;
01303       } else {
01304         SLEEP_MILLISEC(timeTillNextMailboxUpdate);
01305         retry++;
01306       }
01307     } while (retry < mailboxMsgRetries && unvalid);
01308 
01309     if (unvalid) {
01310       this->parseMailboxStatusFlags(mailboxMsgBuffer);
01311       return false;
01312     } else {
01313       return true;
01314     }
01315 
01316   // Bouml preserved body end 00054AF1
01317 }
01318 
01319 
01320 } // namespace youbot
Generated by  doxygen 1.6.3