YouBotGripper.cpp
Go to the documentation of this file.
1 /****************************************************************
2  *
3  * Copyright (c) 2011
4  * All rights reserved.
5  *
6  * Hochschule Bonn-Rhein-Sieg
7  * University of Applied Sciences
8  * Computer Science Department
9  *
10  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11  *
12  * Author:
13  * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
14  * Supervised by:
15  * Gerhard K. Kraetzschmar
16  *
17  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18  *
19  * This sofware is published under a dual-license: GNU Lesser General Public
20  * License LGPL 2.1 and BSD license. The dual-license implies that users of this
21  * code may choose which terms they prefer.
22  *
23  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24  *
25  * Redistribution and use in source and binary forms, with or without
26  * modification, are permitted provided that the following conditions are met:
27  *
28  * * Redistributions of source code must retain the above copyright
29  * notice, this list of conditions and the following disclaimer.
30  * * Redistributions in binary form must reproduce the above copyright
31  * notice, this list of conditions and the following disclaimer in the
32  * documentation and/or other materials provided with the distribution.
33  * * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
34  * contributors may be used to endorse or promote products derived from
35  * this software without specific prior written permission.
36  *
37  * This program is free software: you can redistribute it and/or modify
38  * it under the terms of the GNU Lesser General Public License LGPL as
39  * published by the Free Software Foundation, either version 2.1 of the
40  * License, or (at your option) any later version or the BSD license.
41  *
42  * This program is distributed in the hope that it will be useful,
43  * but WITHOUT ANY WARRANTY; without even the implied warranty of
44  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
45  * GNU Lesser General Public License LGPL and the BSD license for more details.
46  *
47  * You should have received a copy of the GNU Lesser General Public
48  * License LGPL and BSD license along with this program.
49  *
50  ****************************************************************/
53 namespace youbot {
54 
55 YouBotGripper::YouBotGripper(const unsigned int jointNo, const std::string& configFilePath) {
56  // Bouml preserved body begin 0005EFF1
57  this->jointNumber = jointNo;
58  this->mailboxMsgRetries = 200;
59  this->timeTillNextMailboxUpdate = 1; //ms
60 
61  ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));
62  bar1.reset(new YouBotGripperBar(0, jointNo, configFilePath));
63  bar2.reset(new YouBotGripperBar(1, jointNo, configFilePath));
64 
65  // Bouml preserved body end 0005EFF1
66 }
67 
69  // Bouml preserved body begin 0005F071
70  // Bouml preserved body end 0005F071
71 }
72 
74  // Bouml preserved body begin 0005FBF1
75  throw std::runtime_error("Please use YouBotGripperParameter");
76  // Bouml preserved body end 0005FBF1
77 }
78 
80  // Bouml preserved body begin 0005FA71
81  throw std::runtime_error("Please use YouBotGripperParameter");
82  // Bouml preserved body end 0005FA71
83 }
84 
86  // Bouml preserved body begin 000BEF71
87 
88  YouBotSlaveMailboxMsg message;
89  parameter.getYouBotMailboxMsg(message);
90 
91  bool unvalid = true;
92  unsigned int retry = 0;
93 
96 
97  do {
98  if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber)) {
99  unvalid = false;
100  } else {
102  retry++;
103  }
104  } while (retry < mailboxMsgRetries && unvalid);
105 
106  if (unvalid) {
107  this->parseMailboxStatusFlags(message);
108  throw std::runtime_error("Unable to get firmware version of the gripper");
109  return;
110  }
111 
112  char versionString[8] = {0};
113  // versionString[0] = message.stctInput.replyAddress;
114  versionString[0] = message.stctInput.moduleAddress;
115  versionString[1] = message.stctInput.status;
116  versionString[2] = message.stctInput.commandNumber;
117  versionString[3] = message.stctInput.value >> 24;
118  versionString[4] = message.stctInput.value >> 16;
119  versionString[5] = message.stctInput.value >> 8;
120  versionString[6] = message.stctInput.value & 0xff;
121 
122  // LOG(warning) <<"version: "<< versionString;
123  int controllerType = 0;
124  float firmwareVersion = 0;
125  sscanf(versionString, "%dV%f", &controllerType, &firmwareVersion); //KR842V20
126 
127 
128  parameter.setParameter(controllerType, firmwareVersion);
129 
130  return;
131  // Bouml preserved body end 000BEF71
132 }
133 
135  // Bouml preserved body begin 00048271
136 
137  char index = 16; // Parameter 0 to 15 of bank 2 are password protected
138  YouBotSlaveMailboxMsg IsCalibratedReadMessage;
139  IsCalibratedReadMessage.stctOutput.moduleAddress = GRIPPER;
140  IsCalibratedReadMessage.stctOutput.commandNumber = GGP;
141  IsCalibratedReadMessage.stctOutput.typeNumber = index;
142  IsCalibratedReadMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
143  IsCalibratedReadMessage.stctOutput.value = 0;
144  IsCalibratedReadMessage.stctInput.value = 0;
145 
146  YouBotSlaveMailboxMsg IsCalibratedSetMessage;
147  IsCalibratedSetMessage.stctOutput.moduleAddress = GRIPPER;
148  IsCalibratedSetMessage.stctOutput.commandNumber = SGP;
149  IsCalibratedSetMessage.stctOutput.typeNumber = index;
150  IsCalibratedSetMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
151  IsCalibratedSetMessage.stctOutput.value = 1;
152 
153 
154  bool doCalibration = true;
155  if (parameter.value == false) {
156  if (!retrieveValueFromMotorContoller(IsCalibratedReadMessage)) {
157  IsCalibratedReadMessage.stctInput.value = 0;
158  }
159 
160  if (IsCalibratedReadMessage.stctInput.value == 1) {
161  doCalibration = false;
162  }
163  }
164 
165  if(doCalibration){
166  LOG(info) << "Calibrate Gripper";
167 
168  YouBotSlaveMailboxMsg message;
169 
170  unsigned int maxenc = 0;
171  MaxEncoderValue maxencoder;
172  bar1->getConfigurationParameter(maxencoder);
173  maxencoder.getParameter(maxenc);
174 
175  message.stctOutput.moduleAddress = GRIPPER;
176  message.stctOutput.commandNumber = MVP;
177  message.stctOutput.typeNumber = 1; //move gripper relative
178  message.stctOutput.value = -maxenc;
179  message.stctOutput.motorNumber = 0; //move bar 0
180  setValueToMotorContoller(message);
181  message.stctOutput.motorNumber = 1; //move bar 1
182  setValueToMotorContoller(message);
183 
184  //open gripper
185  TargetPositionReached bar1TargetReched;
186  TargetPositionReached bar2TargetReched;
187  bool targetReachedBar1 = false;
188  bool targetReachedBar2 = false;
189 
190  for (int i = 0; i < 40; i++) {
191  SLEEP_MILLISEC(100);
192  bar1->getConfigurationParameter(bar1TargetReched);
193  bar1TargetReched.getParameter(targetReachedBar1);
194  bar2->getConfigurationParameter(bar2TargetReched);
195  bar2TargetReched.getParameter(targetReachedBar2);
196  if (targetReachedBar1 && targetReachedBar2){
197  break;
198  }
199  }
200 
201  //close gripper
202  message.stctOutput.moduleAddress = GRIPPER;
203  message.stctOutput.commandNumber = MVP;
204  message.stctOutput.typeNumber = 1; //move gripper relative
205  message.stctOutput.value = maxenc;
206  message.stctOutput.motorNumber = 0; //move bar 0
207  setValueToMotorContoller(message);
208  message.stctOutput.motorNumber = 1; //move bar 1
209  setValueToMotorContoller(message);
210 
211  targetReachedBar1 = false;
212  targetReachedBar2 = false;
213 
214  for (int i = 0; i < 40; i++) {
215  SLEEP_MILLISEC(100);
216  bar1->getConfigurationParameter(bar1TargetReched);
217  bar1TargetReched.getParameter(targetReachedBar1);
218  bar2->getConfigurationParameter(bar2TargetReched);
219  bar2TargetReched.getParameter(targetReachedBar2);
220  if (targetReachedBar1 && targetReachedBar2)
221  break;
222  }
223 
224  //stop Gripper motor
225  /*
226  message.stctOutput.moduleAddress = GRIPPER;
227  message.stctOutput.commandNumber = MST;
228  message.stctOutput.value = 0;
229  message.stctOutput.motorNumber = 0; //move bar 0
230  setValueToMotorContoller(message);
231  message.stctOutput.motorNumber = 1; //move bar 1
232  setValueToMotorContoller(message);
233 */
234 
235  // set pose to zero as reference
236  ActualPosition actualPose;
237  actualPose.setParameter(0);
238  bar1->setConfigurationParameter(actualPose);
239  bar2->setConfigurationParameter(actualPose);
240 
241  // set a flag in the user variable to remember that it is calibrated
242  this->setValueToMotorContoller(IsCalibratedSetMessage);
243  }
244 
245  // Bouml preserved body end 00048271
246 }
247 
249  // Bouml preserved body begin 000DE971
250  if (!retrieveValueFromMotorContoller(parameter)) {
251  throw JointParameterException("Unable to get parameter from the gripper");
252  }
253  this->parseMailboxStatusFlags(parameter);
254  // Bouml preserved body end 000DE971
255 }
256 
257 void YouBotGripper::getData(const GripperData& data) const {
258  // Bouml preserved body begin 0005FB71
259  LOG(info) << "Nothing to do";
260  // Bouml preserved body end 0005FB71
261 }
262 
264  // Bouml preserved body begin 0005FAF1
265  LOG(info) << "Nothing to do";
266  // Bouml preserved body end 0005FAF1
267 }
268 
270  // Bouml preserved body begin 000483F1
271  LOG(info) << "Nothing to do";
272  // Bouml preserved body end 000483F1
273 }
274 
276  // Bouml preserved body begin 00048371
277  LOG(info) << "Nothing to do";
278  // Bouml preserved body end 00048371
279 }
280 
282  // Bouml preserved body begin 0005F8F1
283 
284  GripperBarPositionSetPoint setpointBar1;
285  GripperBarPositionSetPoint setpointBar2;
286 
287  setpointBar1.barPosition = barSpacing.barSpacing/2.0;
288  setpointBar2.barPosition = barSpacing.barSpacing/2.0;
289 
290  bar1->setData(setpointBar1);
291  bar2->setData(setpointBar2);
292 
293  // Bouml preserved body end 0005F8F1
294 }
295 
297  // Bouml preserved body begin 0005F971
298  GripperSensedBarPosition bar1Position;
299  GripperSensedBarPosition bar2Position;
300  bar1->getData(bar1Position);
301  bar2->getData(bar2Position);
302 
303  barSpacing.barSpacing = bar1Position.barPosition + bar2Position.barPosition;
304 
305  // Bouml preserved body end 0005F971
306 }
307 
309  // Bouml preserved body begin 000E3BF1
310 
311  MaxEncoderValue maxEnc;
312  unsigned int bar1MaxEncoderValue = 0;
313  unsigned int bar2MaxEncoderValue = 0;
314 
315  bar1->getConfigurationParameter(maxEnc);
316  maxEnc.getParameter(bar1MaxEncoderValue);
317  bar2->getConfigurationParameter(maxEnc);
318  maxEnc.getParameter(bar2MaxEncoderValue);
319 
320  GripperBarEncoderSetpoint setpointBar1;
321  GripperBarEncoderSetpoint setpointBar2;
322  setpointBar1.barEncoder = bar1MaxEncoderValue;
323  setpointBar2.barEncoder = bar2MaxEncoderValue;
324 
325  bar1->setData(setpointBar1);
326  bar2->setData(setpointBar2);
327 
328  // Bouml preserved body end 000E3BF1
329 }
330 
332  // Bouml preserved body begin 00103D71
333  GripperBarEncoderSetpoint setpointBar1;
334  GripperBarEncoderSetpoint setpointBar2;
335  setpointBar1.barEncoder = 0;
336  setpointBar2.barEncoder = 0;
337 
338  bar1->setData(setpointBar1);
339  bar2->setData(setpointBar2);
340  // Bouml preserved body end 00103D71
341 }
342 
344  // Bouml preserved body begin 000E0FF1
345  if (bar1 == NULL)
346  throw std::runtime_error("gripper bar 1 is missing");
347 
348  return *bar1;
349  // Bouml preserved body end 000E0FF1
350 }
351 
353  // Bouml preserved body begin 000E1071
354  if (bar2 == NULL)
355  throw std::runtime_error("gripper bar 2 is missing");
356 
357  return *bar2;
358  // Bouml preserved body end 000E1071
359 }
360 
362  // Bouml preserved body begin 00075C71
363  std::stringstream errorMessageStream;
364  errorMessageStream << "Joint " << this->jointNumber << ": ";
365  std::string errorMessage;
366  errorMessage = errorMessageStream.str();
367 
368 
369  switch (mailboxMsg.stctInput.status) {
370  case MAILBOX_SUCCESS:
371  break;
372  case INVALID_COMMAND:
373  LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << " is an invalid command!";
374  // throw JointParameterException(errorMessage + "invalid command");
375  break;
376  case WRONG_TYPE:
377  LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
378  // throw JointParameterException(errorMessage + "wrong type");
379  break;
380  case INVALID_VALUE:
381  LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!";
382  // throw JointParameterException(errorMessage + "invalid value");
383  break;
385  LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << " Configuration EEPROM locked";
386  // throw JointParameterException(errorMessage + "configuration EEPROM locked");
387  break;
389  LOG(error) << errorMessage << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << "Command is not available!";
390  // throw JointParameterException(errorMessage + "command not available");
391  break;
392  }
393 
394 
395  // Bouml preserved body end 00075C71
396 }
397 
399  // Bouml preserved body begin 0005EF71
400 
401  YouBotSlaveMailboxMsg mailboxMsgBuffer;
402  mailboxMsgBuffer = mailboxMsg;
403  bool unvalid = true;
404  unsigned int retry = 0;
405 
406  ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber);
407  // LOG(trace) << "set Output CommandNumber " << (int) mailboxMsgBuffer.stctOutput.commandNumber
408  // << " moduleAddress " << (int) mailboxMsgBuffer.stctOutput.moduleAddress
409  // << " motorNumber " << (int) mailboxMsgBuffer.stctOutput.motorNumber
410  // << " typeNumber " << (int) mailboxMsgBuffer.stctOutput.typeNumber
411  // << " value " << mailboxMsgBuffer.stctOutput.value;
412 
414 
415  do {
416 
417 
418  if (ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->jointNumber) &&
419  mailboxMsgBuffer.stctInput.status == MAILBOX_SUCCESS) {
420  unvalid = false;
421  } else {
423  retry++;
424  }
425  // LOG(trace) << "set Input CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber
426  // << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress
427  // << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress
428  // << " status " << (int) mailboxMsgBuffer.stctInput.status
429  // << " value " << mailboxMsgBuffer.stctInput.value;
430  } while (retry < mailboxMsgRetries && unvalid);
431 
432  if (unvalid) {
433  this->parseMailboxStatusFlags(mailboxMsgBuffer);
434  return false;
435  } else {
436  return true;
437  }
438 
439  // Bouml preserved body end 0005EF71
440 }
441 
443  // Bouml preserved body begin 0005EEF1
444 
445  bool unvalid = true;
446  unsigned int retry = 0;
447 
449  // LOG(trace) << "get Output CommandNumber " << (int) message.stctOutput.commandNumber
450  // << " moduleAddress " << (int) message.stctOutput.moduleAddress
451  // << " motorNumber " << (int) message.stctOutput.motorNumber
452  // << " typeNumber " << (int) message.stctOutput.typeNumber
453  // << " value " << message.stctOutput.value
454  // << " No " << this->jointNumber;
455 
457 
458  do {
459 
460 
461  if (ethercatMaster->getMailboxMsgBuffer(message, this->jointNumber) &&
462  message.stctInput.status == MAILBOX_SUCCESS) {
463  unvalid = false;
464  } else {
466  retry++;
467  }
468  // LOG(trace) << "get input CommandNumber " << (int) message.stctInput.commandNumber
469  // << " moduleAddress " << (int) message.stctInput.moduleAddress
470  // << " replyAddress " << (int) message.stctInput.replyAddress
471  // << " status " << (int) message.stctInput.status
472  // << " value " << message.stctInput.value
473  // << " No " << this->jointNumber;
474 
475  } while (retry < mailboxMsgRetries && unvalid);
476 
477  if (unvalid) {
478  this->parseMailboxStatusFlags(message);
479  return false;
480  } else {
481  return true;
482  }
483 
484  // Bouml preserved body end 0005EEF1
485 }
486 
487 
488 } // namespace youbot
Indicates that the actual position equals the target position.
unsigned int timeTillNextMailboxUpdate
The encoder value when the gripper has reached it&#39;s maximum bar spacing position. ...
abstract gripper parameter
YouBotGripper(const unsigned int jointNo, const std::string &configFilePath="../config/")
virtual void setMailboxMsgBuffer(const YouBotSlaveMailboxMsg &msgBuffer, const unsigned int jointNumber)=0
void parseMailboxStatusFlags(const YouBotSlaveMailboxMsg &mailboxMsg) const
Actual position of one gripper bar.
bool retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg &message) const
void setParameter(const int parameter)
void setParameter(const int controllerType, const double firmwareVersion)
The sensed bar spacing for a one DOF gripper.
The encoder setpoint for one bar.
virtual void setData(const GripperData &data)
YouBotGripperBar & getGripperBar1()
Setpoint length of the bar spacing for a one DOF gripper.
#define LOG(level)
Definition: Logger.hpp:102
virtual void setConfigurationParameter(const GripperParameter &parameter)
boost::scoped_ptr< YouBotGripperBar > bar2
EthercatMasterInterface * ethercatMaster
Joint parameter exception.
Definition: Exceptions.hpp:104
unsigned int mailboxMsgRetries
void getParameter(unsigned int &parameter) const
YouBotGripperBar & getGripperBar2()
EtherCAT mailbox message of the youBot slaves.
One bar of the youBot gripper.
#define SLEEP_MILLISEC(millisec)
Definition: Time.hpp:60
boost::scoped_ptr< YouBotGripperBar > bar1
the firmware version of the gripper
void getParameter(bool &parameter) const
abstract class of gripper data
Definition: GripperData.hpp:59
void getYouBotMailboxMsg(YouBotSlaveMailboxMsg &message) const
quantity< si::length > barSpacing
#define USER_VARIABLE_BANK
The bar position for a one gripper bar.
static EthercatMasterInterface & getInstance(const std::string configFile="youbot-ethercat.cfg", const std::string configFilePath="../config/", const bool ethercatMasterWithThread=true)
abstract class of data for gripper with one degree of freedom
The sensed bar position for a one gripper bar.
virtual void getConfigurationParameter(GripperParameter &parameter) const
virtual void getData(const GripperData &data) const
virtual bool getMailboxMsgBuffer(YouBotSlaveMailboxMsg &mailboxMsg, const unsigned int jointNumber)=0
bool setValueToMotorContoller(const YouBotSlaveMailboxMsg &mailboxMsg) const


youbot_driver
Author(s): Jan Paulus
autogenerated on Mon Jun 10 2019 15:46:25