YouBotJoint.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 YouBotJoint::YouBotJoint(const unsigned int jointNo, const std::string& configFilePath) {
56  // Bouml preserved body begin 000412F1
57  this->storage.jointNumber = jointNo;
59  mailboxMsgRetries = 100;
60  this->storage.inverseMovementDirection = false;
61  this->storage.lowerLimit = 0;
62  this->storage.upperLimit = 0;
63  this->storage.areLimitsActive = false;
64  std::stringstream jointNameStream;
65  jointNameStream << "Joint " << this->storage.jointNumber << " ";
66  this->storage.jointNumberStr = jointNameStream.str();
67  ethercatMaster = &(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath));
68 
69  // Bouml preserved body end 000412F1
70 }
71 
73  // Bouml preserved body begin 00041371
74  // Bouml preserved body end 00041371
75 }
76 
78  // Bouml preserved body begin 00074271
79  throw std::runtime_error("Please use YouBotJointParameters");
80  // Bouml preserved body end 00074271
81 }
82 
84  // Bouml preserved body begin 0005CE71
85  throw std::runtime_error("Please use YouBotJointParameters");
86  // Bouml preserved body end 0005CE71
87 }
88 
90  // Bouml preserved body begin 00071FF1
91 
92  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
93 
94  YouBotSlaveMailboxMsg message;
95  parameter.getYouBotMailboxMsg(message, GAP, storage);
96 
97  message.parameterName = parameter.getName();
98  if (retrieveValueFromMotorContoller(message)) {
99  parameter.setYouBotMailboxMsg(message, storage);
100  } else {
101  throw JointParameterException("Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
102  }
103  }else{
104  throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
105  }
106  // Bouml preserved body end 00071FF1
107 }
108 
110  // Bouml preserved body begin 0005BCF1
111  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
112 
113  YouBotSlaveMailboxMsg message;
114  parameter.getYouBotMailboxMsg(message, GAP, storage);
115 
116  message.parameterName = parameter.getName();
117  if (retrieveValueFromMotorContoller(message)) {
118  parameter.setYouBotMailboxMsg(message, storage);
119  } else {
120  throw JointParameterException("Unable to get parameter: " + parameter.getName() + " from joint: " + this->storage.jointName);
121  }
122  }else{
123  throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
124  }
125  // Bouml preserved body end 0005BCF1
126 }
127 
129  // Bouml preserved body begin 0005BC71
130  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
131 
132  YouBotSlaveMailboxMsg message;
133  parameter.getYouBotMailboxMsg(message, SAP, storage);
134 
135  message.parameterName = parameter.getName();
136  if (!setValueToMotorContoller(message)) {
137  throw JointParameterException("Unable to set parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
138  }
139  }else{
140  throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
141  }
142  // Bouml preserved body end 0005BC71
143 }
144 
146  // Bouml preserved body begin 000740F1
147  parameter.value = this->storage.jointName;
148  // Bouml preserved body end 000740F1
149 }
150 
152  // Bouml preserved body begin 0005CDF1
153  this->storage.jointName = parameter.value;
154  // Bouml preserved body end 0005CDF1
155 }
156 
158  // Bouml preserved body begin 00074171
159  parameter.setParameter(this->storage.gearRatio);
160  // Bouml preserved body end 00074171
161 }
162 
164  // Bouml preserved body begin 00073FF1
165  if (parameter.value == 0) {
166  throw std::out_of_range("A Gear Ratio of zero is not allowed");
167  }
168  this->storage.gearRatio = parameter.value;
169  // Bouml preserved body end 00073FF1
170 }
171 
173  // Bouml preserved body begin 000741F1
174  parameter.setParameter(this->storage.encoderTicksPerRound);
175  // Bouml preserved body end 000741F1
176 }
177 
179  // Bouml preserved body begin 00074071
180  if (parameter.value == 0) {
181  throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
182  }
183  this->storage.encoderTicksPerRound = parameter.value;
184  // Bouml preserved body end 00074071
185 }
186 
188  // Bouml preserved body begin 000623F1
189  if (parameter.doCalibration) {
190  LOG(info) << "Calibrate Joint: " << this->storage.jointName;
191 
192  int calibrationVel = 0; //rpm
193  //YouBotSlaveMsg messageBuffer;
195  if (parameter.calibrationDirection == POSITIV) {
196  calibrationVel = 1.0 / storage.gearRatio;
197  } else if (parameter.calibrationDirection == NEGATIV) {
198  calibrationVel = -1.0 / storage.gearRatio;
199  } else {
200  throw std::runtime_error("No calibration direction for joint: " + this->storage.jointName);
201  }
202 
203  if (this->storage.inverseMovementDirection == true) {
204  calibrationVel *= -1;
205  }
206 
207  JointSensedCurrent sensedCurrent;
208 
210  messageBuffer.stctOutput.value = calibrationVel;
212 
213  sensedCurrent.current = 0;
214  //turn till a max current is reached
215  while (abs(sensedCurrent.current) < abs(parameter.maxCurrent)) {
217  this->getData(sensedCurrent);
218  }
219 
220  //stop movement
223  // LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
225 
226  //set encoder reference position
227  SLEEP_MILLISEC(500);
231 
232  //switch to position controll
233  SLEEP_MILLISEC(100);
236  // LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
238 
239  // LOG(info) << "Calibration finished for joint: " << this->storage.jointName;
240  }
241 
242  // Bouml preserved body end 000623F1
243 }
244 
246  // Bouml preserved body begin 000624F1
247  this->storage.inverseMovementDirection = parameter.value;
248  // Bouml preserved body end 000624F1
249 }
250 
252  // Bouml preserved body begin 000C9671
254  // Bouml preserved body end 000C9671
255 }
256 
258  // Bouml preserved body begin 000D4371
259 
260  this->storage.lowerLimit = parameter.lowerLimit;
261  this->storage.upperLimit = parameter.upperLimit;
262  this->storage.areLimitsActive = parameter.areLimitsActive;
263  MotorAcceleration acc;
264  quantity<angular_acceleration> accValue;
265 
266 
267 
268  if(this->storage.areLimitsActive){
269  this->getConfigurationParameter(acc);
270  acc.getParameter(accValue);
271  this->limitMonitor.reset(new JointLimitMonitor(this->storage, accValue));
273  }else{
274  this->limitMonitor.reset(NULL);
275  }
276  //ethercatMaster->setJointLimits(parameter.lowerLimit, parameter.upperLimit, storage.inverseMovementDirection, parameter.areLimitsActive, this->storage.jointNumber);
277 
278  // Bouml preserved body end 000D4371
279 }
280 
282  // Bouml preserved body begin 000C95F1
283  parameter.setParameter(this->storage.lowerLimit, this->storage.upperLimit, this->storage.areLimitsActive);
284  // Bouml preserved body end 000C95F1
285 }
286 
288  // Bouml preserved body begin 000D43F1
289  quantity<plane_angle> lowlimit = ((double) this->storage.lowerLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
290  quantity<plane_angle> uplimit = ((double) this->storage.upperLimit / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
291  parameter.setParameter(lowlimit, uplimit, this->storage.areLimitsActive);
292  // Bouml preserved body end 000D43F1
293 }
294 
297  // Bouml preserved body begin 000973F1
298  if (parameter.value) {
301 
303  }
304  // Bouml preserved body end 000973F1
305 }
306 
308  // Bouml preserved body begin 0009AA71
309 
310  YouBotSlaveMailboxMsg message;
311  parameter.getYouBotMailboxMsg(message, GAP, storage);
312 
313  bool unvalid = true;
314  unsigned int retry = 0;
315 
317 
319 
320  do {
321  if( ethercatMaster->getMailboxMsgBuffer(message, this->storage.jointNumber) ) {
322  unvalid = false;
323  } else {
325  retry++;
326  }
327  } while (retry < mailboxMsgRetries && unvalid);
328 
329  if (unvalid) {
330  this->parseMailboxStatusFlags(message);
331  throw std::runtime_error("Unable to get firmware version for joint: " + this->storage.jointName);
332  return;
333  }
334 
335  char versionString[9] = {0};
336  versionString[0] = message.stctInput.replyAddress;
337  versionString[1] = message.stctInput.moduleAddress;
338  versionString[2] = message.stctInput.status;
339  versionString[3] = message.stctInput.commandNumber;
340  versionString[4] = message.stctInput.value >> 24;
341  versionString[5] = message.stctInput.value >> 16;
342  versionString[6] = message.stctInput.value >> 8;
343  versionString[7] = message.stctInput.value & 0xff;
344 
345  int controllerType = 0;
346  char firmwareVersion[9] = {0};
347  sscanf (versionString,"%dV%s",&controllerType, firmwareVersion);
348  std::string version(firmwareVersion);
349  size_t founddot = version.find(".");
350  while(founddot != std::string::npos){
351  version.erase(founddot,1);
352  founddot = version.find(".");
353  }
354  parameter.setParameter(controllerType, version);
355  return;
356  // Bouml preserved body end 0009AA71
357 }
358 
361  // Bouml preserved body begin 000A9D71
362  if (!setValueToMotorContoller(parameter)) {
363  throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
364  }
365  // Bouml preserved body end 000A9D71
366 }
367 
370  // Bouml preserved body begin 000A9DF1
371  if (!retrieveValueFromMotorContoller(parameter)) {
372  throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
373  }
374  this->parseMailboxStatusFlags(parameter);
375  // Bouml preserved body end 000A9DF1
376 }
377 
379  // Bouml preserved body begin 000D9571
380  parameter.setParameter(this->storage.torqueConstant);
381  // Bouml preserved body end 000D9571
382 }
383 
385  // Bouml preserved body begin 000C7171
386  parameter.getParameter(this->storage.torqueConstant);
387  // Bouml preserved body end 000C7171
388 }
389 
393  // Bouml preserved body begin 000919F1
394  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
395 
396  this->setConfigurationParameter(parameter);
397 
398  YouBotSlaveMailboxMsg message;
399  parameter.getYouBotMailboxMsg(message, STAP, storage);
400 
401  if (!setValueToMotorContoller(message)) {
402  throw JointParameterException("Unable to store parameter: " + parameter.getName() + " to joint: " + this->storage.jointName);
403  }
404  }else{
405  throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
406  }
407  // Bouml preserved body end 000919F1
408 }
409 
412  // Bouml preserved body begin 00091A71
413  if (parameter.getType() == MOTOR_CONTOLLER_PARAMETER) {
414 
415  YouBotSlaveMailboxMsg message;
416  parameter.getYouBotMailboxMsg(message, RSAP, storage);
417 
418  if (!setValueToMotorContoller(message)) {
419  throw JointParameterException("Unable to restore parameter: " + parameter.getName() + " at joint: " + this->storage.jointName);
420  }
421 
422  this->getConfigurationParameter(parameter);
423  }else{
424  throw JointParameterException("Parameter " + parameter.getName() + " is not a motor controller parameter of a joint");
425  }
426  // Bouml preserved body end 00091A71
427 }
428 
430  // Bouml preserved body begin 000413F1
431  LOG(info) << "Nothing to do";
432  // Bouml preserved body end 000413F1
433 }
434 
436  // Bouml preserved body begin 00041471
437  LOG(info) << "Nothing to do";
438  // Bouml preserved body end 00041471
439 }
440 
444  // Bouml preserved body begin 0003C1F1
445 
447  throw EtherCATConnectionException("No EtherCAT connection");
448  }
449 
450  //YouBotSlaveMsg messageBuffer;
453 
454  if (storage.gearRatio == 0) {
455  throw std::out_of_range("A Gear Ratio of zero is not allowed");
456  }
457 
458  if (storage.encoderTicksPerRound == 0) {
459  throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
460  }
461 
462  if (this->limitMonitor != 0)
463  this->limitMonitor->checkLimitsPositionControl(data.angle);
464 
466  messageBuffer.stctOutput.value = (int32) boost::math::round((data.angle.value() * ((double) storage.encoderTicksPerRound / (2.0 * M_PI))) / storage.gearRatio);
467 
468 
471  }
472  // LOG(trace) << "value: " << data.angle << " gear " << gearRatio << " encoderperRound " << encoderTicksPerRound << " encPos " << messageBuffer.stctOutput.value << " joint " << this->storage.jointNumber;
474  // Bouml preserved body end 0003C1F1
475 }
476 
480  // Bouml preserved body begin 000C2371
481 
483  throw EtherCATConnectionException("No EtherCAT connection");
484  }
485 
486  //YouBotSlaveMsg messageBuffer;
489 
490  if (this->limitMonitor != 0)
491  this->limitMonitor->checkLimitsEncoderPosition(data.encoderTicks);
492 
495 
498  }
499 
501  // Bouml preserved body end 000C2371
502 }
503 
507  // Bouml preserved body begin 0003DCF1
508  //YouBotSlaveMsg messageBuffer;
510  throw EtherCATConnectionException("No EtherCAT connection");
511  }
512 
515 
516  if (storage.gearRatio == 0) {
517  throw std::out_of_range("A Gear Ratio of zero is not allowed");
518  }
519  if (storage.encoderTicksPerRound == 0) {
520  throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
521  }
522  // LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
523  data.angle = ((double) messageBuffer.stctInput.actualPosition / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
524 
526  data.angle = -data.angle;
527  }
528  // Bouml preserved body end 0003DCF1
529 }
530 
534  // Bouml preserved body begin 0003C371
536  throw EtherCATConnectionException("No EtherCAT connection");
537  }
538 
539  //YouBotSlaveMsg messageBuffer;
542 
544 
545  if (storage.gearRatio == 0) {
546  throw std::out_of_range("A Gear Ratio of 0 is not allowed");
547  }
548 
549  messageBuffer.stctOutput.value = (int32) boost::math::round((data.angularVelocity.value() / (storage.gearRatio * 2.0 * M_PI)) * 60.0);
552  }
553 
554  // LOG(trace) << "vel [rpm] " << messageBuffer.stctOutput.value << " rad_sec " << data.angularVelocity;
556  // Bouml preserved body end 0003C371
557 }
558 
562  // Bouml preserved body begin 0003DD71
564  throw EtherCATConnectionException("No EtherCAT connection");
565  }
566 
567  //YouBotSlaveMsg messageBuffer;
570 
571  if (storage.gearRatio == 0) {
572  throw std::out_of_range("A Gear Ratio of 0 is not allowed");
573  }
574  double motorRPM = messageBuffer.stctInput.actualVelocity;
575  //convert RPM of the motor to radian per second of the wheel/joint
576  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
577 
579  data.angularVelocity *= -1;
580  }
581  // Bouml preserved body end 0003DD71
582 }
583 
587  // Bouml preserved body begin 000AEC71
589  throw EtherCATConnectionException("No EtherCAT connection");
590  }
591 
592  //YouBotSlaveMsg messageBuffer;
595 
597 
599  data.rpm *= -1;
600  }
601  // Bouml preserved body end 000AEC71
602 }
603 
607  // Bouml preserved body begin 000AECF1
609  throw EtherCATConnectionException("No EtherCAT connection");
610  }
611 
612  //YouBotSlaveMsg messageBuffer;
615 
618 
621  }
622 
624  // Bouml preserved body end 000AECF1
625 }
626 
630  // Bouml preserved body begin 0003DDF1
632  throw EtherCATConnectionException("No EtherCAT connection");
633  }
634 
635  //YouBotSlaveMsg messageBuffer;
638  //convert mili ampere to ampere
639  double current = messageBuffer.stctInput.actualCurrent;
640  data.current = current / 1000.0 * ampere;
641 
643  data.current *= -1;
644  }
645  // Bouml preserved body end 0003DDF1
646 }
647 
651  // Bouml preserved body begin 000955F1
653  throw EtherCATConnectionException("No EtherCAT connection");
654  }
655 
656  //YouBotSlaveMsg messageBuffer;
659 
661  messageBuffer.stctOutput.value = (int32) (data.current.value() * 1000.0); //convert from Ampere to milli Ampere
662 
665  }
667  // Bouml preserved body end 000955F1
668 }
669 
673  // Bouml preserved body begin 000AB7F1
675  throw EtherCATConnectionException("No EtherCAT connection");
676  }
677 
678  //YouBotSlaveMsg messageBuffer;
681 
682  // LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
684 
686  data.encoderTicks *= -1;
687  }
688 
689  // Bouml preserved body end 000AB7F1
690 }
691 
696  // Bouml preserved body begin 000C5671
697  //YouBotSlaveMsg messageBuffer;
699  throw EtherCATConnectionException("No EtherCAT connection");
700  }
701 
704 
705  messageBuffer.stctOutput = data;
706 
708  // Bouml preserved body end 000C5671
709 }
710 
715  // Bouml preserved body begin 000C56F1
716  //YouBotSlaveMsg messageBuffer;
718  throw EtherCATConnectionException("No EtherCAT connection");
719  }
720 
723 
724  data = messageBuffer;
725  // Bouml preserved body end 000C56F1
726 }
727 
731  // Bouml preserved body begin 000C7071
733  throw EtherCATConnectionException("No EtherCAT connection");
734  }
735 
736  JointCurrentSetpoint currentSetpoint;
737 
738  if (this->storage.torqueConstant == 0) {
739  throw std::out_of_range("A torque constant of 0 is not allowed");
740  }
741 
742  currentSetpoint.current = ((data.torque.value()*this->storage.gearRatio)/this->storage.torqueConstant) * ampere;
743  this->setData(currentSetpoint);
744  // Bouml preserved body end 000C7071
745 }
746 
750  // Bouml preserved body begin 000C70F1
752  throw EtherCATConnectionException("No EtherCAT connection");
753  }
754 
755  JointSensedCurrent sensedCurrent;
756  this->getData(sensedCurrent);
757 
758  if (this->storage.gearRatio == 0) {
759  throw std::out_of_range("A Gear Ratio of 0 is not allowed");
760  }
761  data.torque = ((sensedCurrent.current.value() * this->storage.torqueConstant) / this->storage.gearRatio) * newton_meter;
762  // Bouml preserved body end 000C70F1
763 }
764 
768  // Bouml preserved body begin 00103EF1
769  //YouBotSlaveMsg messageBuffer;
771  throw EtherCATConnectionException("No EtherCAT connection");
772  }
773 
776 
777  if (storage.gearRatio == 0) {
778  throw std::out_of_range("A Gear Ratio of zero is not allowed");
779  }
780  if (storage.encoderTicksPerRound == 0) {
781  throw std::out_of_range("Zero Encoder Ticks per Round are not allowed");
782  }
783  // LOG(trace) << "enc: " << messageBuffer.stctInput.actualPosition;
784  data.angle = ((double) messageBuffer.stctInput.targetPosition / storage.encoderTicksPerRound) * storage.gearRatio * (2.0 * M_PI) * radian;
785 
787  data.angle = -data.angle;
788  }
789  // Bouml preserved body end 00103EF1
790 }
791 
795  // Bouml preserved body begin 001002F1
797  throw EtherCATConnectionException("No EtherCAT connection");
798  }
799 
800  //YouBotSlaveMsg messageBuffer;
803 
804  if (storage.gearRatio == 0) {
805  throw std::out_of_range("A Gear Ratio of 0 is not allowed");
806  }
807  double motorRPM = messageBuffer.stctInput.targetVelocity;
808  //convert RPM of the motor to radian per second of the wheel/joint
809  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
810 
812  data.angularVelocity *= -1;
813  }
814  // Bouml preserved body end 001002F1
815 }
816 
820  // Bouml preserved body begin 00100371
822  throw EtherCATConnectionException("No EtherCAT connection");
823  }
824 
825  //YouBotSlaveMsg messageBuffer;
828  //convert mili ampere to ampere
829  double current = messageBuffer.stctInput.targetCurrent;
830  data.current = current / 1000.0 * ampere;
831 
833  data.current *= -1;
834  }
835  // Bouml preserved body end 00100371
836 }
837 
841  // Bouml preserved body begin 001003F1
843  throw EtherCATConnectionException("No EtherCAT connection");
844  }
845 
846  //YouBotSlaveMsg messageBuffer;
849 
850  if (storage.gearRatio == 0) {
851  throw std::out_of_range("A Gear Ratio of 0 is not allowed");
852  }
854  //convert RPM of the motor to radian per second of the wheel/joint
855  data.angularVelocity = ((motorRPM / 60.0) * storage.gearRatio * 2.0 * M_PI) * radian_per_second;
856 
858  data.angularVelocity *= -1;
859  }
860  // Bouml preserved body end 001003F1
861 }
862 
863 void YouBotJoint::getUserVariable(const unsigned int index, int& data) {
864  // Bouml preserved body begin 000AD171
865 
866  if(index == 0 || index > 55){
867  throw JointParameterException("User variable index is out of range use 1-55 at: " + this->storage.jointName);
868  }
869  //56 is the last userdata at bank 2
870  YouBotSlaveMailboxMsg message;
871  message.stctOutput.moduleAddress = DRIVE;
872  message.stctOutput.commandNumber = GGP;
873  message.stctOutput.typeNumber = index;
875  message.stctOutput.value = 0;
876 
877  if (!retrieveValueFromMotorContoller(message)) {
878  throw JointParameterException("Unable to get parameter from joint: " + this->storage.jointName);
879  }
880  this->parseMailboxStatusFlags(message);
881 
882  data = message.stctInput.value;
883  // Bouml preserved body end 000AD171
884 }
885 
886 void YouBotJoint::setUserVariable(const unsigned int index, const int data) {
887  // Bouml preserved body begin 000AD1F1
888 
889  if(index < 17 || index > 55){
890  throw JointParameterException("User variable index is out of range use 17-55 at: " + this->storage.jointName);
891  }
892  //56 is the last userdata at bank 2
893  YouBotSlaveMailboxMsg message;
894  message.stctOutput.moduleAddress = DRIVE;
895  message.stctOutput.commandNumber = GGP;
896  message.stctOutput.typeNumber = index;
898  message.stctOutput.value = data;
899 
900  if (!setValueToMotorContoller(message)) {
901  throw JointParameterException("Unable to set parameter at joint: " + this->storage.jointName);
902  }
903  this->parseMailboxStatusFlags(message);
904  // Bouml preserved body end 000AD1F1
905 }
906 
908 void YouBotJoint::getStatus(std::vector<std::string>& statusMessages) {
909  // Bouml preserved body begin 000AD271
910  //YouBotSlaveMsg messageBuffer;
912 
913 
914 
915 
917  statusMessages.push_back(this->storage.jointName + " got over current");
918  }
919 
921  statusMessages.push_back(this->storage.jointName + " got under voltage");
922  }
923 
925  statusMessages.push_back(this->storage.jointName + " got over voltage");
926  }
927 
929  statusMessages.push_back(this->storage.jointName + " got over temperature");
930  }
931 
933  statusMessages.push_back(this->storage.jointName + " is halted");
934  }
935 
937  statusMessages.push_back(this->storage.jointName + " got hall sensor problem");
938  }
939 
940 // if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
941 // statusMessages.push_back(this->storage.jointName + " got encoder problem");
942 // }
943 //
944 // if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
945 // statusMessages.push_back(this->storage.jointName + " got inizialization problem");
946 // }
947 
948 // if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
949 // statusMessages.push_back(this->storage.jointName + " has PWM mode active");
950 // }
951 
953  statusMessages.push_back(this->storage.jointName + " has velocity mode active");
954  }
955 
957  statusMessages.push_back(this->storage.jointName + " has position mode active");
958  }
959 
961  statusMessages.push_back(this->storage.jointName + " has torque mode active");
962  }
963 
964 // if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
965 // statusMessages.push_back(this->storage.jointName + " has emergency stop active");
966 // }
967 //
968 // if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
969 // statusMessages.push_back(this->storage.jointName + " has freerunning active");
970 // }
971 
973  statusMessages.push_back(this->storage.jointName + " has position reached");
974  }
975 
977  statusMessages.push_back(this->storage.jointName + " is initialized");
978  }
979 
981  statusMessages.push_back(this->storage.jointName + " has a timeout");
982  }
983 
985  statusMessages.push_back(this->storage.jointName + " exceeded I2t");
986  }
987 
988 
989  // Bouml preserved body end 000AD271
990 }
991 
1011 void YouBotJoint::getStatus(unsigned int& statusFlags) {
1012  // Bouml preserved body begin 000AD2F1
1013  //YouBotSlaveMsg messageBuffer;
1015 
1016  statusFlags = messageBuffer.stctInput.errorFlags;
1017  // Bouml preserved body end 000AD2F1
1018 }
1019 
1022  // Bouml preserved body begin 000AED71
1024  throw EtherCATConnectionException("No EtherCAT connection");
1025  }
1026 
1027  YouBotSlaveMailboxMsg message;
1028  message.stctOutput.commandNumber = SAP;
1029  message.stctOutput.moduleAddress = DRIVE;
1030  message.stctOutput.typeNumber = 1; //actual Position
1031  message.stctOutput.value = 0;
1032 
1033  if (!setValueToMotorContoller(message)) {
1034  throw JointParameterException("Unable to set the encoders to zero at joint: " + this->storage.jointName);
1035  }
1036 
1037  //YouBotSlaveMsg messageBuffer;
1038  // messageBuffer.stctOutput.controllerMode = SET_POSITION_TO_REFERENCE;
1039  // messageBuffer.stctOutput.value = 0;
1040 
1041  //ethercatMaster->setMsgBuffer(messageBuffer, this->storage.jointNumber);
1042 
1043 
1044  // Bouml preserved body end 000AED71
1045 }
1046 
1048  // Bouml preserved body begin 000664F1
1050  throw EtherCATConnectionException("No EtherCAT connection");
1051  }
1052 
1053  //YouBotSlaveMsg messageBuffer;
1056 
1058  // Bouml preserved body end 000664F1
1059 }
1060 
1062  // Bouml preserved body begin 00066471
1064  throw EtherCATConnectionException("No EtherCAT connection");
1065  }
1066 
1067  //YouBotSlaveMsg messageBuffer;
1070 
1072  // Bouml preserved body end 00066471
1073 }
1074 
1076  // Bouml preserved body begin 000EA2F1
1077  return this->storage.jointNumber;
1078  // Bouml preserved body end 000EA2F1
1079 }
1080 
1082  // Bouml preserved body begin 00044AF1
1083 
1084  if (messageBuffer.stctInput.errorFlags & OVER_CURRENT) {
1085  LOG(warning) << this->storage.jointName << " over current";
1086  // throw JointErrorException(this->storage.jointName + "got over current");
1087  }
1088 
1089  if (messageBuffer.stctInput.errorFlags & UNDER_VOLTAGE) {
1090  LOG(warning) << this->storage.jointName << " under voltage";
1091  // throw JointErrorException(this->storage.jointName + "got under voltage");
1092  }
1093 
1094  if (messageBuffer.stctInput.errorFlags & OVER_VOLTAGE) {
1095  LOG(warning) << this->storage.jointName << " over voltage";
1096  // throw JointErrorException(this->storage.jointName + "got over voltage");
1097  }
1098 
1099  if (messageBuffer.stctInput.errorFlags & OVER_TEMPERATURE) {
1100  LOG(warning) << this->storage.jointName << " over temperature";
1101  // throw JointErrorException(this->storage.jointName + "got over temperature");
1102  }
1103 
1104  if (messageBuffer.stctInput.errorFlags & MOTOR_HALTED) {
1105  // LOG(info) << this->storage.jointName << " is halted";
1106  // throw JointErrorException(this->storage.jointName + "is halted");
1107  }
1108 
1109  if (messageBuffer.stctInput.errorFlags & HALL_SENSOR_ERROR) {
1110  LOG(warning) << this->storage.jointName << " hall sensor problem";
1111  // throw JointErrorException(this->storage.jointName + "got hall sensor problem");
1112  }
1113 
1114 // if (messageBuffer.stctInput.errorFlags & ENCODER_ERROR) {
1115 // LOG(warning) << this->storage.jointName << " encoder problem";
1116 // // throw JointErrorException(this->storage.jointName + "got encoder problem");
1117 // }
1118 //
1119 // if (messageBuffer.stctInput.errorFlags & INITIALIZATION_ERROR) {
1120 // LOG(warning) << this->storage.jointName << " initialization problem";
1121 // // throw JointErrorException(this->storage.jointName + "got motor winding problem");
1122 // }
1123 
1124 // if (messageBuffer.stctInput.errorFlags & PWM_MODE_ACTIVE) {
1125  // LOG(info) << this->storage.jointName << " has PWM mode active";
1126  // throw JointErrorException(this->storage.jointName + "the cycle time is violated");
1127 // }
1128 
1129  if (messageBuffer.stctInput.errorFlags & VELOCITY_MODE) {
1130  // LOG(info) << this->storage.jointName << " has velocity mode active";
1131  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1132  }
1133 
1134  if (messageBuffer.stctInput.errorFlags & POSITION_MODE) {
1135  // LOG(info) << this->storage.jointName << " has position mode active";
1136  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1137  }
1138 
1139  if (messageBuffer.stctInput.errorFlags & TORQUE_MODE) {
1140  // LOG(info) << this->storage.jointName << " has torque mode active";
1141  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1142  }
1143 
1144 // if (messageBuffer.stctInput.errorFlags & EMERGENCY_STOP) {
1145 // LOG(info) << this->storage.jointName << " emergency stop active";
1146 // // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1147 // }
1148 //
1149 // if (messageBuffer.stctInput.errorFlags & FREERUNNING) {
1150 // // LOG(info) << this->storage.jointName << " has freerunning active";
1151 // // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1152 // }
1153 
1154  if (messageBuffer.stctInput.errorFlags & POSITION_REACHED) {
1155  // LOG(info) << this->storage.jointName << " has position reached";
1156  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1157  }
1158 
1159  if (!(messageBuffer.stctInput.errorFlags & INITIALIZED)) {
1160  // LOG(warning) << this->storage.jointName << " not initialized";
1161  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1162  }
1163 
1164  if (messageBuffer.stctInput.errorFlags & TIMEOUT) {
1165  LOG(warning) << this->storage.jointName << " exceeded timeout";
1166  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1167  }
1168 
1169  if (messageBuffer.stctInput.errorFlags & I2T_EXCEEDED) {
1170  LOG(warning) << this->storage.jointName << " exceeded I2t";
1171  // throw JointErrorException(this->storage.jointName + "need to initialize the sinus commutation");
1172  }
1173 
1174  // Bouml preserved body end 00044AF1
1175 }
1176 
1178  // Bouml preserved body begin 00075BF1
1179 
1180  switch(mailboxMsg.stctInput.status){
1181  case MAILBOX_SUCCESS:
1182  break;
1183  case INVALID_COMMAND:
1184  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << "; Command no: " << mailboxMsg.stctOutput.commandNumber << " is an invalid command!" ;
1185  // throw JointParameterException(this->storage.jointName + "invalid command");
1186  break;
1187  case WRONG_TYPE:
1188  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " has a wrong type!";
1189  // throw JointParameterException(this->storage.jointName + "wrong type");
1190  break;
1191  case INVALID_VALUE:
1192  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " Value: " << mailboxMsg.stctOutput.value << " is a invalid value!";
1193  // throw JointParameterException(this->storage.jointName + "invalid value");
1194  break;
1196  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Configuration EEPROM locked";
1197  // throw JointParameterException(this->storage.jointName + "configuration EEPROM locked");
1198  break;
1199  case COMMAND_NOT_AVAILABLE:
1200  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Command is not available!";
1201  // throw JointParameterException(this->storage.jointName + "command not available");
1202  break;
1203  case REPLY_WRITE_PROTECTED:
1204  LOG(error) << this->storage.jointName << "Parameter name: " << mailboxMsg.parameterName << " - Permissions denied!";
1205  // throw JointParameterException(this->storage.jointName + "command not available");
1206  break;
1207  }
1208 
1209 
1210  // Bouml preserved body end 00075BF1
1211 }
1212 
1214  // Bouml preserved body begin 0005BD71
1215 
1216  bool unvalid = true;
1217  unsigned int retry = 0;
1218 
1220 
1222 
1223  do {
1225  /* LOG(trace) << "CommandNumber " << (int) message.stctInput.commandNumber
1226  << " moduleAddress " << (int) message.stctInput.moduleAddress
1227  << " replyAddress " << (int) message.stctInput.replyAddress
1228  << " status " << (int) message.stctInput.status
1229  << " value " << message.stctInput.value; */
1230 
1231  if (message.stctOutput.commandNumber == message.stctInput.commandNumber &&
1232  message.stctInput.status == MAILBOX_SUCCESS) {
1233  unvalid = false;
1234  } else {
1236  retry++;
1237  }
1238  } while (retry < mailboxMsgRetries && unvalid);
1239 
1240  if (unvalid) {
1241  this->parseMailboxStatusFlags(message);
1242  return false;
1243  } else {
1244  return true;
1245  }
1246 
1247  // Bouml preserved body end 0005BD71
1248 }
1249 
1251  // Bouml preserved body begin 00054AF1
1252 
1253  YouBotSlaveMailboxMsg mailboxMsgBuffer;
1254  mailboxMsgBuffer = mailboxMsg;
1255  bool unvalid = true;
1256  unsigned int retry = 0;
1257 
1258  ethercatMaster->setMailboxMsgBuffer(mailboxMsgBuffer, this->storage.jointNumber);
1259 
1261 
1262  do {
1263  ethercatMaster->getMailboxMsgBuffer(mailboxMsgBuffer, this->storage.jointNumber);
1264  /* LOG(trace) << "CommandNumber " << (int) mailboxMsgBuffer.stctInput.commandNumber
1265  << " moduleAddress " << (int) mailboxMsgBuffer.stctInput.moduleAddress
1266  << " replyAddress " << (int) mailboxMsgBuffer.stctInput.replyAddress
1267  << " status " << (int) mailboxMsgBuffer.stctInput.status
1268  << " value " << mailboxMsgBuffer.stctInput.value;
1269  */
1270  if (mailboxMsgBuffer.stctOutput.commandNumber == mailboxMsgBuffer.stctInput.commandNumber &&
1271  mailboxMsgBuffer.stctOutput.value == mailboxMsgBuffer.stctInput.value &&
1272  mailboxMsgBuffer.stctInput.status == MAILBOX_SUCCESS) {
1273  unvalid = false;
1274  } else {
1276  retry++;
1277  }
1278  } while (retry < mailboxMsgRetries && unvalid);
1279 
1280  if (unvalid) {
1281  this->parseMailboxStatusFlags(mailboxMsgBuffer);
1282  return false;
1283  } else {
1284  return true;
1285  }
1286 
1287  // Bouml preserved body end 00054AF1
1288 }
1289 
1290 
1291 } // namespace youbot
EthercatMasterInterface * ethercatMaster
Acceleration parameter for velocity control and position control.
void setParameter(const unsigned int parameter)
the firmware version of the joint
virtual void setMsgBuffer(const YouBotSlaveMsg &msgBuffer, const unsigned int jointNumber)=0
void parseMailboxStatusFlags(const YouBotSlaveMailboxMsg &mailboxMsg)
the resolution of the encoders, it is needed for the calculations of the youBot Driver ...
void setParameter(const quantity< plane_angle > &lowerLimit, const quantity< plane_angle > &upperLimit, const bool activateLimits)
virtual void getConfigurationParameter(JointParameter &parameter)
Definition: YouBotJoint.cpp:83
the resolution of the encoders, it is needed for the calculations of the youBot Driver ...
bool retrieveValueFromMotorContoller(YouBotSlaveMailboxMsg &message)
quantity< angular_velocity > angularVelocity
Definition: JointData.hpp:184
void setParameter(const double parameter)
YouBotSlaveMsg messageBuffer
virtual std::string getName() const =0
Sensed encoder ticks of the joint.
Definition: JointData.hpp:100
void setParameter(const bool parameter)
virtual void setMailboxMsgBuffer(const YouBotSlaveMailboxMsg &msgBuffer, const unsigned int jointNumber)=0
virtual ParameterType getType() const =0
unsigned int mailboxMsgRetries
Set-point velocity of the joint.
Definition: JointData.hpp:182
The torque set-point of the joint will be set by setting the computed current set-point.
Definition: JointData.hpp:218
Output part from the EtherCat message of the youBot EtherCat slaves.
virtual void setData(const JointDataSetpoint &data)
joint position limits in encoder ticks
void getUserVariable(const unsigned int index, int &data)
virtual void setConfigurationParameter(const JointParameter &parameter)
Definition: YouBotJoint.cpp:77
void parseYouBotErrorFlags(const YouBotSlaveMsg &messageBuffer)
This torque of the joint is computed from the actual current.
Definition: JointData.hpp:148
void storeConfigurationParameterPermanent(const YouBotJointParameter &parameter)
joint position limits in radian
Sensed velocity of the joint.
Definition: JointData.hpp:259
the name of the joint
quantity< plane_angle > angle
Definition: JointData.hpp:90
unsigned int getJointNumber()
quantity< si::torque > torque
Definition: JointData.hpp:150
virtual void setYouBotMailboxMsg(const YouBotSlaveMailboxMsg &message, const YouBotJointStorage &storage)=0
quantity< si::current > maxCurrent
#define LOG(level)
Definition: Logger.hpp:102
unsigned int timeTillNextMailboxUpdate
Sensed rounds per minute (rpm) of the joint.
Definition: JointData.hpp:124
virtual ParameterType getType() const =0
virtual void getYouBotMailboxMsg(YouBotSlaveMailboxMsg &message, TMCLCommandNumber msgType, const YouBotJointStorage &storage) const =0
Joint parameter exception.
Definition: Exceptions.hpp:104
It monitors the joint position and will decelerate and stop the joint if it is close the limits...
the gear ratio which is needed for the calculations in the youBot driver
Sensed electric current of the joint.
Definition: JointData.hpp:136
void getParameter(double &parameter) const
encoder ticks setpoint of the joint
Definition: JointData.hpp:242
YouBotJointStorage storage
int32_t int32
Definition: osal.h:32
EtherCAT mailbox message of the youBot slaves.
abstract youBot joint parameter
void setParameter(const double parameter)
SlaveMessageInput stctInput
abstract joint parameter
inverse the joint movement direction
quantity< si::angular_velocity > angularVelocity
Definition: JointData.hpp:114
#define SLEEP_MILLISEC(millisec)
Definition: Time.hpp:60
void setUserVariable(const unsigned int index, const int data)
void getYouBotMailboxMsg(YouBotSlaveMailboxMsg &message, TMCLCommandNumber msgType, const YouBotJointStorage &storage) const
virtual void getMsgBuffer(const unsigned int jointNumber, YouBotSlaveMsg &returnMsg)=0
void setParameter(const int lowerLimit, const int upperLimit, const bool activateLimits)
virtual std::string getName() const =0
bool setValueToMotorContoller(const YouBotSlaveMailboxMsg &mailboxMsg)
virtual void getYouBotMailboxMsg(YouBotSlaveMailboxMsg &message, TMCLCommandNumber msgType, const YouBotJointStorage &storage) const =0
virtual void setYouBotMailboxMsg(const YouBotSlaveMailboxMsg &message, const YouBotJointStorage &storage)=0
virtual void getStatus(std::vector< std::string > &statusMessages)
Returns the status messages for the motor controller.
quantity< si::torque > torque
Definition: JointData.hpp:220
abstract data class for commanded joint data
Definition: JointData.hpp:160
SlaveMessageOutput stctOutput
void restoreConfigurationParameter(YouBotJointParameter &parameter)
Restores the joint parameter from the EEPROM.
virtual void registerJointLimitMonitor(JointLimitMonitor *object, const unsigned int JointNumber)=0
Set-point current of the joint.
Definition: JointData.hpp:206
quantity< si::current > current
Definition: JointData.hpp:208
#define USER_VARIABLE_BANK
boost::scoped_ptr< JointLimitMonitor > limitMonitor
CalibrationDirection calibrationDirection
abstract youBot joint parameter which can be read only
virtual void getData(JointData &data)
static EthercatMasterInterface & getInstance(const std::string configFile="youbot-ethercat.cfg", const std::string configFilePath="../config/", const bool ethercatMasterWithThread=true)
void setEncoderToZero()
set the encoder values of the joint to zero. This postion will be the new reference.
virtual bool isEtherCATConnectionEstablished()=0
quantity< si::current > current
Definition: JointData.hpp:138
abstract data class for joints
Definition: JointData.hpp:61
EtherCat message of the youBot EtherCat slaves.
EtherCAT Connection Error.
Definition: Exceptions.hpp:142
Sensed position / angle of the joint.
Definition: JointData.hpp:88
quantity< plane_angle > angle
Definition: JointData.hpp:172
YouBotJoint(const unsigned int jointNo, const std::string &configFilePath="../config/")
Definition: YouBotJoint.cpp:55
Set-point angle / position of the joint.
Definition: JointData.hpp:170
quantity< si::angular_velocity > angularVelocity
Definition: JointData.hpp:261
virtual bool getMailboxMsgBuffer(YouBotSlaveMailboxMsg &mailboxMsg, const unsigned int jointNumber)=0
Rounds per minute set-point of the joint.
Definition: JointData.hpp:194
void getParameter(quantity< angular_acceleration > &parameter) const
void setParameter(const int controllerType, const std::string firmwareVersion)
Sensed velocity of the joint.
Definition: JointData.hpp:112


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