YouBotGripper.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/YouBotGripper.hpp"
00052 namespace youbot {
00053 
00054 YouBotGripper::YouBotGripper(const unsigned int jointNo, const std::string& configFilePath) {
00055   // Bouml preserved body begin 0005EFF1
00056     this->jointNumber = jointNo;
00057     this->mailboxMsgRetries = 200;
00058     this->timeTillNextMailboxUpdate = 1; //ms
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   // Bouml preserved body end 0005EFF1
00065 }
00066 
00067 YouBotGripper::~YouBotGripper() {
00068   // Bouml preserved body begin 0005F071
00069   // Bouml preserved body end 0005F071
00070 }
00071 
00072 void YouBotGripper::getConfigurationParameter(GripperParameter& parameter) const {
00073   // Bouml preserved body begin 0005FBF1
00074     throw std::runtime_error("Please use YouBotGripperParameter");
00075   // Bouml preserved body end 0005FBF1
00076 }
00077 
00078 void YouBotGripper::setConfigurationParameter(const GripperParameter& parameter) {
00079   // Bouml preserved body begin 0005FA71
00080     throw std::runtime_error("Please use YouBotGripperParameter");
00081   // Bouml preserved body end 0005FA71
00082 }
00083 
00084 void YouBotGripper::getConfigurationParameter(GripperFirmwareVersion& parameter) const {
00085   // Bouml preserved body begin 000BEF71
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     //  versionString[0] = message.stctInput.replyAddress;
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     // LOG(warning) <<"version: "<< versionString;
00122     int controllerType = 0;
00123     float firmwareVersion = 0;
00124     sscanf(versionString, "%dV%f", &controllerType, &firmwareVersion); //KR842V20
00125 
00126 
00127     parameter.setParameter(controllerType, firmwareVersion);
00128 
00129     return;
00130   // Bouml preserved body end 000BEF71
00131 }
00132 
00133 void YouBotGripper::setConfigurationParameter(const CalibrateGripper& parameter) {
00134   // Bouml preserved body begin 00048271
00135 
00136     char index = 16; // Parameter 0 to 15 of bank 2 are password protected
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; //move gripper relative
00177       message.stctOutput.value = -maxenc;
00178       message.stctOutput.motorNumber = 0; //move bar 0
00179       setValueToMotorContoller(message);
00180       message.stctOutput.motorNumber = 1; //move bar 1
00181       setValueToMotorContoller(message);
00182 
00183       //open gripper
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       //close gripper
00201       message.stctOutput.moduleAddress = GRIPPER;
00202       message.stctOutput.commandNumber = MVP;
00203       message.stctOutput.typeNumber = 1; //move gripper relative
00204       message.stctOutput.value = maxenc;
00205       message.stctOutput.motorNumber = 0; //move bar 0
00206       setValueToMotorContoller(message);
00207       message.stctOutput.motorNumber = 1; //move bar 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       //stop Gripper motor
00224       /*
00225       message.stctOutput.moduleAddress = GRIPPER;
00226       message.stctOutput.commandNumber = MST;
00227       message.stctOutput.value = 0;
00228       message.stctOutput.motorNumber = 0; //move bar 0
00229       setValueToMotorContoller(message);
00230       message.stctOutput.motorNumber = 1; //move bar 1
00231       setValueToMotorContoller(message);
00232 */
00233       
00234       // set pose to zero as reference
00235       ActualPosition actualPose;
00236       actualPose.setParameter(0);
00237       bar1->setConfigurationParameter(actualPose);
00238       bar2->setConfigurationParameter(actualPose);
00239       
00240        // set a flag in the user variable to remember that it is calibrated
00241       this->setValueToMotorContoller(IsCalibratedSetMessage);
00242     }
00243 
00244   // Bouml preserved body end 00048271
00245 }
00246 
00247 void YouBotGripper::getConfigurationParameter(YouBotSlaveMailboxMsg& parameter) const {
00248   // Bouml preserved body begin 000DE971
00249     if (!retrieveValueFromMotorContoller(parameter)) {
00250       throw JointParameterException("Unable to get parameter from the gripper");
00251     }
00252     this->parseMailboxStatusFlags(parameter);
00253   // Bouml preserved body end 000DE971
00254 }
00255 
00256 void YouBotGripper::getData(const GripperData& data) const {
00257   // Bouml preserved body begin 0005FB71
00258     LOG(info) << "Nothing to do";
00259   // Bouml preserved body end 0005FB71
00260 }
00261 
00262 void YouBotGripper::setData(const GripperData& data) {
00263   // Bouml preserved body begin 0005FAF1
00264     LOG(info) << "Nothing to do";
00265   // Bouml preserved body end 0005FAF1
00266 }
00267 
00268 void YouBotGripper::getData(OneDOFGripperData& data) const {
00269   // Bouml preserved body begin 000483F1
00270     LOG(info) << "Nothing to do";
00271   // Bouml preserved body end 000483F1
00272 }
00273 
00274 void YouBotGripper::setData(const OneDOFGripperData& data) {
00275   // Bouml preserved body begin 00048371
00276     LOG(info) << "Nothing to do";
00277   // Bouml preserved body end 00048371
00278 }
00279 
00280 void YouBotGripper::setData(const GripperBarSpacingSetPoint& barSpacing) {
00281   // Bouml preserved body begin 0005F8F1
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   // Bouml preserved body end 0005F8F1
00293 }
00294 
00295 void YouBotGripper::getData(GripperSensedBarSpacing& barSpacing) const {
00296   // Bouml preserved body begin 0005F971
00297     GripperSensedBarPosition bar1Position;
00298     GripperSensedBarPosition bar2Position;
00299     bar1->getData(bar1Position);
00300     bar2->getData(bar2Position);
00301     
00302     barSpacing.barSpacing = bar1Position.barPosition + bar2Position.barPosition;
00303 
00304   // Bouml preserved body end 0005F971
00305 }
00306 
00307 void YouBotGripper::open() {
00308   // Bouml preserved body begin 000E3BF1
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   // Bouml preserved body end 000E3BF1
00328 }
00329 
00330 void YouBotGripper::close() {
00331   // Bouml preserved body begin 00103D71
00332     GripperBarEncoderSetpoint setpointBar1;
00333     GripperBarEncoderSetpoint setpointBar2;
00334     setpointBar1.barEncoder = 0;
00335     setpointBar2.barEncoder = 0;
00336 
00337     bar1->setData(setpointBar1);
00338     bar2->setData(setpointBar2);
00339   // Bouml preserved body end 00103D71
00340 }
00341 
00342 YouBotGripperBar& YouBotGripper::getGripperBar1() {
00343   // Bouml preserved body begin 000E0FF1
00344     if (bar1 == NULL)
00345       throw std::runtime_error("gripper bar 1 is missing");
00346 
00347     return *bar1;
00348   // Bouml preserved body end 000E0FF1
00349 }
00350 
00351 YouBotGripperBar& YouBotGripper::getGripperBar2() {
00352   // Bouml preserved body begin 000E1071
00353     if (bar2 == NULL)
00354       throw std::runtime_error("gripper bar 2 is missing");
00355 
00356     return *bar2;
00357   // Bouml preserved body end 000E1071
00358 }
00359 
00360 void YouBotGripper::parseMailboxStatusFlags(const YouBotSlaveMailboxMsg& mailboxMsg) const {
00361   // Bouml preserved body begin 00075C71
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         //    throw JointParameterException(errorMessage + "invalid command");
00374         break;
00375       case WRONG_TYPE:
00376         LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
00377         //    throw JointParameterException(errorMessage + "wrong type");
00378         break;
00379       case INVALID_VALUE:
00380         LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!";
00381         //    throw JointParameterException(errorMessage + "invalid value");
00382         break;
00383       case CONFIGURATION_EEPROM_LOCKED:
00384         LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Configuration EEPROM locked";
00385         //    throw JointParameterException(errorMessage + "configuration EEPROM locked");
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         //    throw JointParameterException(errorMessage + "command not available");
00390         break;
00391     }
00392 
00393 
00394   // Bouml preserved body end 00075C71
00395 }
00396 
00397 bool YouBotGripper::setValueToMotorContoller(const YouBotSlaveMailboxMsg& mailboxMsg) const {
00398   // Bouml preserved body begin 0005EF71
00399 
00400     YouBotSlaveMailboxMsg mailboxMsgBuffer;
00401     mailboxMsgBuffer = mailboxMsg;
00402     bool unvalid = true;
00403     unsigned int retry = 0;
00404 
00405     ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber);
00406     //    LOG(trace) << "set Output CommandNumber " << (int) mailboxMsgBuffer.stctOutput.commandNumber
00407     //                  << " moduleAddress " << (int) mailboxMsgBuffer.stctOutput.moduleAddress
00408     //                  << " motorNumber " << (int) mailboxMsgBuffer.stctOutput.motorNumber
00409     //                  << " typeNumber " << (int) mailboxMsgBuffer.stctOutput.typeNumber
00410     //                  << " value " << mailboxMsgBuffer.stctOutput.value;
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       //      LOG(trace) << "set Input CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber
00425       //                  << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress
00426       //                  << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress
00427       //                  << " status " << (int) mailboxMsgBuffer.stctInput.status
00428       //                  << " value " << mailboxMsgBuffer.stctInput.value;
00429     } while (retry < mailboxMsgRetries && unvalid);
00430 
00431     if (unvalid) {
00432       this->parseMailboxStatusFlags(mailboxMsgBuffer);
00433       return false;
00434     } else {
00435       return true;
00436     }
00437 
00438   // Bouml preserved body end 0005EF71
00439 }
00440 
00441 bool YouBotGripper::retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg& message) const {
00442   // Bouml preserved body begin 0005EEF1
00443 
00444     bool unvalid = true;
00445     unsigned int retry = 0;
00446 
00447     ethercatMaster->setMailboxMsgBuffer(message, this->jointNumber);
00448     //     LOG(trace) << "get Output CommandNumber " << (int) message.stctOutput.commandNumber
00449     //                  << " moduleAddress " << (int) message.stctOutput.moduleAddress
00450     //                  << " motorNumber " << (int) message.stctOutput.motorNumber
00451     //                  << " typeNumber " << (int) message.stctOutput.typeNumber
00452     //                  << " value " << message.stctOutput.value
00453     //                  << " No " << this->jointNumber;
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       //      LOG(trace) << "get input CommandNumber " << (int) message.stctInput.commandNumber
00468       //                 << " moduleAddress " << (int) message.stctInput.moduleAddress
00469       //                 << " replyAddress " << (int) message.stctInput.replyAddress
00470       //                 << " status " << (int) message.stctInput.status
00471       //                 << " value " << message.stctInput.value
00472       //                 << " No " << this->jointNumber;
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   // Bouml preserved body end 0005EEF1
00484 }
00485 
00486 
00487 } // namespace youbot
Generated by  doxygen 1.6.3