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/YouBotGripperBar.hpp" 00052 namespace youbot { 00053 00054 YouBotGripperBar::YouBotGripperBar(const unsigned int barNo, const unsigned int jointNo, const std::string& configFilePath) { 00055 // Bouml preserved body begin 000E0371 00056 this->jointNumber = jointNo; 00057 this->mailboxMsgRetries = 200; 00058 this->timeTillNextMailboxUpdate = 1; //ms 00059 this->barNo = barNo; 00060 this->maxTravelDistance = 0.0115 * meter; 00061 this->maxEncoderValue = 67000; 00062 this->barSpacingOffset = 0 * meter; 00063 00064 ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath)); 00065 // Bouml preserved body end 000E0371 00066 } 00067 00068 YouBotGripperBar::~YouBotGripperBar() { 00069 // Bouml preserved body begin 000E03F1 00070 // Bouml preserved body end 000E03F1 00071 } 00072 00073 void YouBotGripperBar::setConfigurationParameter(const MaxEncoderValue& parameter) { 00074 // Bouml preserved body begin 00061E71 00075 this->maxEncoderValue = parameter.value; 00076 // Bouml preserved body end 00061E71 00077 } 00078 00079 void YouBotGripperBar::getConfigurationParameter(MaxEncoderValue& parameter) const { 00080 // Bouml preserved body begin 000D7871 00081 parameter.value = this->maxEncoderValue; 00082 // Bouml preserved body end 000D7871 00083 } 00084 00085 void YouBotGripperBar::getConfigurationParameter(MaxTravelDistance& parameter) const { 00086 // Bouml preserved body begin 000D77F1 00087 parameter.value = this->maxTravelDistance; 00088 // Bouml preserved body end 000D77F1 00089 } 00090 00091 void YouBotGripperBar::setConfigurationParameter(const MaxTravelDistance& parameter) { 00092 // Bouml preserved body begin 00061DF1 00093 this->maxTravelDistance = parameter.value; 00094 // Bouml preserved body end 00061DF1 00095 } 00096 00097 void YouBotGripperBar::setConfigurationParameter(const BarSpacingOffset& parameter) { 00098 // Bouml preserved body begin 00061871 00099 this->barSpacingOffset = parameter.value; 00100 // Bouml preserved body end 00061871 00101 } 00102 00103 void YouBotGripperBar::getConfigurationParameter(BarSpacingOffset& parameter) const { 00104 // Bouml preserved body begin 000D7771 00105 parameter.value = this->barSpacingOffset; 00106 // Bouml preserved body end 000D7771 00107 } 00108 00109 void YouBotGripperBar::setConfigurationParameter(const GripperBarName& parameter) { 00110 // Bouml preserved body begin 0010A271 00111 this->name = parameter.value; 00112 // Bouml preserved body end 0010A271 00113 } 00114 00115 void YouBotGripperBar::getConfigurationParameter(GripperBarName& parameter) const { 00116 // Bouml preserved body begin 0010A1F1 00117 parameter.value = this->name; 00118 // Bouml preserved body end 0010A1F1 00119 } 00120 00121 void YouBotGripperBar::getConfigurationParameter(YouBotGripperParameter& parameter) const { 00122 // Bouml preserved body begin 000E05F1 00123 00124 if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) { 00125 00126 YouBotSlaveMailboxMsg message; 00127 parameter.getYouBotMailboxMsg(message); 00128 message.stctOutput.commandNumber = GAP; 00129 message.stctOutput.moduleAddress = GRIPPER; 00130 message.stctOutput.motorNumber = this->barNo; 00131 message.parameterName = parameter.getName(); 00132 00133 if (retrieveValueFromMotorContoller(message)) { 00134 parameter.setYouBotMailboxMsg(message); 00135 } else { 00136 throw JointParameterException("Unable to get parameter: " + parameter.getName() + " from the gripper"); 00137 } 00138 }else{ 00139 throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of the gripper"); 00140 } 00141 // Bouml preserved body end 000E05F1 00142 } 00143 00144 void YouBotGripperBar::setConfigurationParameter(const YouBotGripperParameter& parameter) { 00145 // Bouml preserved body begin 000E0671 00146 if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) { 00147 00148 YouBotSlaveMailboxMsg message; 00149 parameter.getYouBotMailboxMsg(message); 00150 message.stctOutput.commandNumber = SAP; 00151 message.stctOutput.moduleAddress = GRIPPER; 00152 message.stctOutput.motorNumber = this->barNo; 00153 message.parameterName = parameter.getName(); 00154 00155 if (!setValueToMotorContoller(message)) { 00156 throw JointParameterException("Unable to set parameter: " + parameter.getName() + " to the gripper"); 00157 } 00158 }else{ 00159 throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of the gripper"); 00160 } 00161 // Bouml preserved body end 000E0671 00162 } 00163 00164 void YouBotGripperBar::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter) const { 00165 // Bouml preserved body begin 000E0A71 00166 if (!retrieveValueFromMotorContoller(parameter)) { 00167 throw JointParameterException("Unable to get parameter from the gripper"); 00168 } 00169 this->parseMailboxStatusFlags(parameter); 00170 // Bouml preserved body end 000E0A71 00171 } 00172 00173 void YouBotGripperBar::setData(const GripperBarEncoderSetpoint& encoderSetpoint) { 00174 // Bouml preserved body begin 000E0CF1 00175 YouBotSlaveMailboxMsg message; 00176 message.stctOutput.moduleAddress = GRIPPER; 00177 message.stctOutput.commandNumber = MVP; 00178 message.stctOutput.typeNumber = 0; //move gripper absolute 00179 message.stctOutput.motorNumber = this->barNo; 00180 message.stctOutput.value = encoderSetpoint.barEncoder * -1; 00181 00182 setValueToMotorContoller(message); 00183 00184 // Bouml preserved body end 000E0CF1 00185 } 00186 00187 void YouBotGripperBar::getData(GripperSensedVelocity& barVelocity) const { 00188 // Bouml preserved body begin 000E0DF1 00189 YouBotSlaveMailboxMsg message; 00190 message.stctOutput.moduleAddress = GRIPPER; 00191 message.stctOutput.commandNumber = GAP; 00192 message.stctOutput.typeNumber = 3; //actual velocity 00193 message.stctOutput.value = 0; 00194 00195 message.stctOutput.motorNumber = this->barNo; 00196 00197 retrieveValueFromMotorContoller(message); 00198 //std::cout << message.stctInput.value << std::endl; 00199 00200 barVelocity.barVelocity = message.stctInput.value; 00201 00202 00203 // Bouml preserved body end 000E0DF1 00204 } 00205 00206 void YouBotGripperBar::getData(GripperSensedBarPosition& barPosition) const { 00207 // Bouml preserved body begin 000F9171 00208 int valueBar = 0; 00209 ActualPosition actualPoseBar; 00210 this->getConfigurationParameter(actualPoseBar); 00211 actualPoseBar.getParameter(valueBar); 00212 00213 barPosition.barPosition = (((double) valueBar / this->maxEncoderValue) * this->maxTravelDistance) + this->barSpacingOffset; 00214 00215 // Bouml preserved body end 000F9171 00216 } 00217 00218 void YouBotGripperBar::setData(GripperBarPositionSetPoint& barPosition) { 00219 // Bouml preserved body begin 000F91F1 00220 00221 if (barPosition.barPosition > (this->maxTravelDistance + this->barSpacingOffset) || barPosition.barPosition < this->barSpacingOffset) { 00222 std::stringstream errorMessageStream; 00223 errorMessageStream << "The bar position is not allowed to be less than "<< this->barSpacingOffset <<" or higher than " << (this->maxTravelDistance + this->barSpacingOffset) << ". You set " << barPosition.barPosition; 00224 throw std::out_of_range(errorMessageStream.str()); 00225 } 00226 00227 quantity<si::length> setpoint;; 00228 setpoint = (barPosition.barPosition - this->barSpacingOffset); 00229 00230 GripperBarEncoderSetpoint setpointBar; 00231 setpointBar.barEncoder = setpoint / this->maxTravelDistance * this->maxEncoderValue; 00232 this->setData(setpointBar); 00233 00234 // Bouml preserved body end 000F91F1 00235 } 00236 00237 void YouBotGripperBar::parseGripperErrorFlags(const unsigned int& errosFlags) { 00238 // Bouml preserved body begin 00103CF1 00239 if (errosFlags & STALL_GUARD_STATUS) { 00240 // LOG(warning) << "Gripper " << "stallguard2 threshold reached"; 00241 } 00242 if (errosFlags & GRIPPER_OVER_TEMPERATURE) { 00243 LOG(error) << "Gripper " << "over temperature"; 00244 } 00245 if (errosFlags & PRE_WARNING_OVER_TEMPERATURE) { 00246 LOG(warning) << "Gripper " << "pre warning over temperature"; 00247 } 00248 if (errosFlags & SHORT_TO_GROUND_A) { 00249 LOG(error) << "Gripper " << "short to ground A"; 00250 } 00251 if (errosFlags & SHORT_TO_GROUND_B) { 00252 LOG(error) << "Gripper " << "short to ground B"; 00253 } 00254 if (errosFlags & OPEN_LOAD_A) { 00255 LOG(warning) << "Gripper " << "open load A"; 00256 } 00257 if (errosFlags & OPEN_LOAD_B) { 00258 LOG(warning) << "Gripper " << "open load B"; 00259 } 00260 if (errosFlags & STAND_STILL) { 00261 // LOG(info) << "Gripper " << "stand still"; 00262 } 00263 if ( !(errosFlags & STAND_STILL) && (errosFlags & STALL_GUARD_STATUS) ) { 00264 LOG(info) << "Gripper " << "motor stall"; 00265 } 00266 // Bouml preserved body end 00103CF1 00267 } 00268 00269 void YouBotGripperBar::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg) const { 00270 // Bouml preserved body begin 000E0E71 00271 std::stringstream errorMessageStream; 00272 errorMessageStream << "Joint " << this->jointNumber << ": "; 00273 std::string errorMessage; 00274 errorMessage = errorMessageStream.str(); 00275 00276 00277 switch(mailboxMsg.stctInput.status){ 00278 case NO_ERROR: 00279 break; 00280 case INVALID_COMMAND: 00281 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << " is an invalid command!" ; 00282 // throw JointParameterException(errorMessage + "invalid command"); 00283 break; 00284 case WRONG_TYPE: 00285 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!"; 00286 // throw JointParameterException(errorMessage + "wrong type"); 00287 break; 00288 case INVALID_VALUE: 00289 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!"; 00290 // throw JointParameterException(errorMessage + "invalid value"); 00291 break; 00292 case CONFIGURATION_EEPROM_LOCKED: 00293 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Configuration EEPROM locked"; 00294 // throw JointParameterException(errorMessage + "configuration EEPROM locked"); 00295 break; 00296 case COMMAND_NOT_AVAILABLE: 00297 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << "Command is not available!"; 00298 // throw JointParameterException(errorMessage + "command not available"); 00299 break; 00300 } 00301 00302 00303 // Bouml preserved body end 000E0E71 00304 } 00305 00306 bool YouBotGripperBar::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg) const { 00307 // Bouml preserved body begin 000E0EF1 00308 00309 YouBotSlaveMailboxMsg mailboxMsgBuffer; 00310 mailboxMsgBuffer = mailboxMsg; 00311 bool unvalid = true; 00312 unsigned int retry = 0; 00313 00314 ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber); 00315 // LOG(trace) << "set Output CommandNumber " << (int) mailboxMsgBuffer.stctOutput.commandNumber 00316 // << " moduleAddress " << (int) mailboxMsgBuffer.stctOutput.moduleAddress 00317 // << " motorNumber " << (int) mailboxMsgBuffer.stctOutput.motorNumber 00318 // << " typeNumber " << (int) mailboxMsgBuffer.stctOutput.typeNumber 00319 // << " value " << mailboxMsgBuffer.stctOutput.value; 00320 00321 SLEEP_MILLISEC(timeTillNextMailboxUpdate); 00322 00323 do { 00324 00325 00326 if (ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber) && 00327 mailboxMsgBuffer.stctInput.status == NO_ERROR) { 00328 unvalid = false; 00329 } else { 00330 SLEEP_MILLISEC(timeTillNextMailboxUpdate); 00331 retry++; 00332 } 00333 // LOG(trace) << "set Input CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber 00334 // << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress 00335 // << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress 00336 // << " status " << (int) mailboxMsgBuffer.stctInput.status 00337 // << " value " << mailboxMsgBuffer.stctInput.value; 00338 } while (retry < mailboxMsgRetries && unvalid); 00339 00340 if (unvalid) { 00341 this->parseMailboxStatusFlags(mailboxMsgBuffer); 00342 return false; 00343 } else { 00344 return true; 00345 } 00346 00347 // Bouml preserved body end 000E0EF1 00348 } 00349 00350 bool YouBotGripperBar::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message) const { 00351 // Bouml preserved body begin 000E0F71 00352 00353 bool unvalid = true; 00354 unsigned int retry = 0; 00355 00356 ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber); 00357 // LOG(trace) << "get Output CommandNumber " << (int) message.stctOutput.commandNumber 00358 // << " moduleAddress " << (int) message.stctOutput.moduleAddress 00359 // << " motorNumber " << (int) message.stctOutput.motorNumber 00360 // << " typeNumber " << (int) message.stctOutput.typeNumber 00361 // << " value " << message.stctOutput.value 00362 // << " No " << this->jointNumber; 00363 00364 SLEEP_MILLISEC(timeTillNextMailboxUpdate); 00365 00366 do { 00367 00368 00369 if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber) && 00370 message.stctInput.status == NO_ERROR) { 00371 unvalid = false; 00372 } else { 00373 SLEEP_MILLISEC(timeTillNextMailboxUpdate); 00374 retry++; 00375 } 00376 // LOG(trace) << "get input CommandNumber " << (int) message.stctInput.commandNumber 00377 // << " moduleAddress " << (int) message.stctInput.moduleAddress 00378 // << " replyAddress " << (int) message.stctInput.replyAddress 00379 // << " status " << (int) message.stctInput.status 00380 // << " value " << message.stctInput.value 00381 // << " No " << this->jointNumber; 00382 00383 } while (retry < mailboxMsgRetries && unvalid); 00384 00385 if (unvalid) { 00386 this->parseMailboxStatusFlags(message); 00387 return false; 00388 } else { 00389 return true; 00390 } 00391 00392 // Bouml preserved body end 000E0F71 00393 } 00394 00395 00396 } // namespace youbot