YouBotGripper.cpp
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #include "youbot/YouBotGripper.hpp"
00052 namespace youbot {
00053
00054 YouBotGripper::YouBotGripper(const unsigned int jointNo, const std::string& configFilePath) {
00055
00056 this->jointNumber = jointNo;
00057 this->mailboxMsgRetries = 200;
00058 this->timeTillNextMailboxUpdate = 1;
00059
00060 ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));
00061 bar1.reset(new YouBotGripperBar(0, jointNo, configFilePath));
00062 bar2.reset(new YouBotGripperBar(1, jointNo, configFilePath));
00063
00064
00065 }
00066
00067 YouBotGripper::~YouBotGripper() {
00068
00069
00070 }
00071
00072 void YouBotGripper::getConfigurationParameter(GripperParameter& parameter) const {
00073
00074 throw std::runtime_error("Please use YouBotGripperParameter");
00075
00076 }
00077
00078 void YouBotGripper::setConfigurationParameter(const GripperParameter& parameter) {
00079
00080 throw std::runtime_error("Please use YouBotGripperParameter");
00081
00082 }
00083
00084 void YouBotGripper::getConfigurationParameter(GripperFirmwareVersion& parameter) const {
00085
00086
00087 YouBotSlaveMailboxMsg message;
00088 parameter.getYouBotMailboxMsg(message);
00089
00090 bool unvalid = true;
00091 unsigned int retry = 0;
00092
00093 ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber);
00094 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00095
00096 do {
00097 if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber)) {
00098 unvalid = false;
00099 } else {
00100 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00101 retry++;
00102 }
00103 } while (retry < mailboxMsgRetries && unvalid);
00104
00105 if (unvalid) {
00106 this->parseMailboxStatusFlags(message);
00107 throw std::runtime_error("Unable to get firmware version of the gripper");
00108 return;
00109 }
00110
00111 char versionString[8] = {0};
00112
00113 versionString[0] = message.stctInput.moduleAddress;
00114 versionString[1] = message.stctInput.status;
00115 versionString[2] = message.stctInput.commandNumber;
00116 versionString[3] = message.stctInput.value >> 24;
00117 versionString[4] = message.stctInput.value >> 16;
00118 versionString[5] = message.stctInput.value >> 8;
00119 versionString[6] = message.stctInput.value & 0xff;
00120
00121
00122 int controllerType = 0;
00123 float firmwareVersion = 0;
00124 sscanf(versionString, "%dV%f", &controllerType, &firmwareVersion);
00125
00126
00127 parameter.setParameter(controllerType, firmwareVersion);
00128
00129 return;
00130
00131 }
00132
00133 void YouBotGripper::setConfigurationParameter(const CalibrateGripper& parameter) {
00134
00135
00136 char index = 16;
00137 YouBotSlaveMailboxMsg IsCalibratedReadMessage;
00138 IsCalibratedReadMessage.stctOutput.moduleAddress = GRIPPER;
00139 IsCalibratedReadMessage.stctOutput.commandNumber = GGP;
00140 IsCalibratedReadMessage.stctOutput.typeNumber = index;
00141 IsCalibratedReadMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
00142 IsCalibratedReadMessage.stctOutput.value = 0;
00143 IsCalibratedReadMessage.stctInput.value = 0;
00144
00145 YouBotSlaveMailboxMsg IsCalibratedSetMessage;
00146 IsCalibratedSetMessage.stctOutput.moduleAddress = GRIPPER;
00147 IsCalibratedSetMessage.stctOutput.commandNumber = SGP;
00148 IsCalibratedSetMessage.stctOutput.typeNumber = index;
00149 IsCalibratedSetMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
00150 IsCalibratedSetMessage.stctOutput.value = 1;
00151
00152
00153 bool doCalibration = true;
00154 if (parameter.value == false) {
00155 if (!retrieveValueFromMotorContoller(IsCalibratedReadMessage)) {
00156 IsCalibratedReadMessage.stctInput.value = 0;
00157 }
00158
00159 if (IsCalibratedReadMessage.stctInput.value == 1) {
00160 doCalibration = false;
00161 }
00162 }
00163
00164 if(doCalibration){
00165 LOG(info) << "Calibrate Gripper";
00166
00167 YouBotSlaveMailboxMsg message;
00168
00169 unsigned int maxenc = 0;
00170 MaxEncoderValue maxencoder;
00171 bar1->getConfigurationParameter(maxencoder);
00172 maxencoder.getParameter(maxenc);
00173
00174 message.stctOutput.moduleAddress = GRIPPER;
00175 message.stctOutput.commandNumber = MVP;
00176 message.stctOutput.typeNumber = 1;
00177 message.stctOutput.value = -maxenc;
00178 message.stctOutput.motorNumber = 0;
00179 setValueToMotorContoller(message);
00180 message.stctOutput.motorNumber = 1;
00181 setValueToMotorContoller(message);
00182
00183
00184 TargetPositionReached bar1TargetReched;
00185 TargetPositionReached bar2TargetReched;
00186 bool targetReachedBar1 = false;
00187 bool targetReachedBar2 = false;
00188
00189 for (int i = 0; i < 40; i++) {
00190 SLEEP_MILLISEC(100);
00191 bar1->getConfigurationParameter(bar1TargetReched);
00192 bar1TargetReched.getParameter(targetReachedBar1);
00193 bar2->getConfigurationParameter(bar2TargetReched);
00194 bar2TargetReched.getParameter(targetReachedBar2);
00195 if (targetReachedBar1 && targetReachedBar2){
00196 break;
00197 }
00198 }
00199
00200
00201 message.stctOutput.moduleAddress = GRIPPER;
00202 message.stctOutput.commandNumber = MVP;
00203 message.stctOutput.typeNumber = 1;
00204 message.stctOutput.value = maxenc;
00205 message.stctOutput.motorNumber = 0;
00206 setValueToMotorContoller(message);
00207 message.stctOutput.motorNumber = 1;
00208 setValueToMotorContoller(message);
00209
00210 targetReachedBar1 = false;
00211 targetReachedBar2 = false;
00212
00213 for (int i = 0; i < 40; i++) {
00214 SLEEP_MILLISEC(100);
00215 bar1->getConfigurationParameter(bar1TargetReched);
00216 bar1TargetReched.getParameter(targetReachedBar1);
00217 bar2->getConfigurationParameter(bar2TargetReched);
00218 bar2TargetReched.getParameter(targetReachedBar2);
00219 if (targetReachedBar1 && targetReachedBar2)
00220 break;
00221 }
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235 ActualPosition actualPose;
00236 actualPose.setParameter(0);
00237 bar1->setConfigurationParameter(actualPose);
00238 bar2->setConfigurationParameter(actualPose);
00239
00240
00241 this->setValueToMotorContoller(IsCalibratedSetMessage);
00242 }
00243
00244
00245 }
00246
00247 void YouBotGripper::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter) const {
00248
00249 if (!retrieveValueFromMotorContoller(parameter)) {
00250 throw JointParameterException("Unable to get parameter from the gripper");
00251 }
00252 this->parseMailboxStatusFlags(parameter);
00253
00254 }
00255
00256 void YouBotGripper::getData(const GripperData& data) const {
00257
00258 LOG(info) << "Nothing to do";
00259
00260 }
00261
00262 void YouBotGripper::setData(const GripperData& data) {
00263
00264 LOG(info) << "Nothing to do";
00265
00266 }
00267
00268 void YouBotGripper::getData(OneDOFGripperData& data) const {
00269
00270 LOG(info) << "Nothing to do";
00271
00272 }
00273
00274 void YouBotGripper::setData(const OneDOFGripperData& data) {
00275
00276 LOG(info) << "Nothing to do";
00277
00278 }
00279
00280 void YouBotGripper::setData(const GripperBarSpacingSetPoint& barSpacing) {
00281
00282
00283 GripperBarPositionSetPoint setpointBar1;
00284 GripperBarPositionSetPoint setpointBar2;
00285
00286 setpointBar1.barPosition = barSpacing.barSpacing/2.0;
00287 setpointBar2.barPosition = barSpacing.barSpacing/2.0;
00288
00289 bar1->setData(setpointBar1);
00290 bar2->setData(setpointBar2);
00291
00292
00293 }
00294
00295 void YouBotGripper::getData(GripperSensedBarSpacing& barSpacing) const {
00296
00297 GripperSensedBarPosition bar1Position;
00298 GripperSensedBarPosition bar2Position;
00299 bar1->getData(bar1Position);
00300 bar2->getData(bar2Position);
00301
00302 barSpacing.barSpacing = bar1Position.barPosition + bar2Position.barPosition;
00303
00304
00305 }
00306
00307 void YouBotGripper::open() {
00308
00309
00310 MaxEncoderValue maxEnc;
00311 unsigned int bar1MaxEncoderValue = 0;
00312 unsigned int bar2MaxEncoderValue = 0;
00313
00314 bar1->getConfigurationParameter(maxEnc);
00315 maxEnc.getParameter(bar1MaxEncoderValue);
00316 bar2->getConfigurationParameter(maxEnc);
00317 maxEnc.getParameter(bar2MaxEncoderValue);
00318
00319 GripperBarEncoderSetpoint setpointBar1;
00320 GripperBarEncoderSetpoint setpointBar2;
00321 setpointBar1.barEncoder = bar1MaxEncoderValue;
00322 setpointBar2.barEncoder = bar2MaxEncoderValue;
00323
00324 bar1->setData(setpointBar1);
00325 bar2->setData(setpointBar2);
00326
00327
00328 }
00329
00330 void YouBotGripper::close() {
00331
00332 GripperBarEncoderSetpoint setpointBar1;
00333 GripperBarEncoderSetpoint setpointBar2;
00334 setpointBar1.barEncoder = 0;
00335 setpointBar2.barEncoder = 0;
00336
00337 bar1->setData(setpointBar1);
00338 bar2->setData(setpointBar2);
00339
00340 }
00341
00342 YouBotGripperBar& YouBotGripper::getGripperBar1() {
00343
00344 if (bar1 == NULL)
00345 throw std::runtime_error("gripper bar 1 is missing");
00346
00347 return *bar1;
00348
00349 }
00350
00351 YouBotGripperBar& YouBotGripper::getGripperBar2() {
00352
00353 if (bar2 == NULL)
00354 throw std::runtime_error("gripper bar 2 is missing");
00355
00356 return *bar2;
00357
00358 }
00359
00360 void YouBotGripper::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg) const {
00361
00362 std::stringstream errorMessageStream;
00363 errorMessageStream << "Joint " << this->jointNumber << ": ";
00364 std::string errorMessage;
00365 errorMessage = errorMessageStream.str();
00366
00367
00368 switch (mailboxMsg.stctInput.status) {
00369 case NO_ERROR:
00370 break;
00371 case INVALID_COMMAND:
00372 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << " is an invalid command!";
00373
00374 break;
00375 case WRONG_TYPE:
00376 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
00377
00378 break;
00379 case INVALID_VALUE:
00380 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!";
00381
00382 break;
00383 case CONFIGURATION_EEPROM_LOCKED:
00384 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Configuration EEPROM locked";
00385
00386 break;
00387 case COMMAND_NOT_AVAILABLE:
00388 LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << "Command is not available!";
00389
00390 break;
00391 }
00392
00393
00394
00395 }
00396
00397 bool YouBotGripper::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg) const {
00398
00399
00400 YouBotSlaveMailboxMsg mailboxMsgBuffer;
00401 mailboxMsgBuffer = mailboxMsg;
00402 bool unvalid = true;
00403 unsigned int retry = 0;
00404
00405 ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber);
00406
00407
00408
00409
00410
00411
00412 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00413
00414 do {
00415
00416
00417 if (ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber) &&
00418 mailboxMsgBuffer.stctInput.status == NO_ERROR) {
00419 unvalid = false;
00420 } else {
00421 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00422 retry++;
00423 }
00424
00425
00426
00427
00428
00429 } while (retry < mailboxMsgRetries && unvalid);
00430
00431 if (unvalid) {
00432 this->parseMailboxStatusFlags(mailboxMsgBuffer);
00433 return false;
00434 } else {
00435 return true;
00436 }
00437
00438
00439 }
00440
00441 bool YouBotGripper::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message) const {
00442
00443
00444 bool unvalid = true;
00445 unsigned int retry = 0;
00446
00447 ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber);
00448
00449
00450
00451
00452
00453
00454
00455 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00456
00457 do {
00458
00459
00460 if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber) &&
00461 message.stctInput.status == NO_ERROR) {
00462 unvalid = false;
00463 } else {
00464 SLEEP_MILLISEC(timeTillNextMailboxUpdate);
00465 retry++;
00466 }
00467
00468
00469
00470
00471
00472
00473
00474 } while (retry < mailboxMsgRetries && unvalid);
00475
00476 if (unvalid) {
00477 this->parseMailboxStatusFlags(message);
00478 return false;
00479 } else {
00480 return true;
00481 }
00482
00483
00484 }
00485
00486
00487 }