AnimatLab  2
Test
RbFirmataDynamixelServo.cpp
1 // RbFirmataDynamixelServo.cpp: implementation of the RbFirmataDynamixelServo class.
2 //
4 
5 #include "StdAfx.h"
6 #include <stdarg.h>
7 #include "RbMovableItem.h"
8 #include "RbBody.h"
9 #include "RbJoint.h"
10 #include "RbMotorizedJoint.h"
11 #include "RbHingeLimit.h"
12 #include "RbHinge.h"
13 #include "RbRigidBody.h"
14 #include "RbStructure.h"
15 #include "RbFirmataPart.h"
16 #include "RbFirmataDynamixelServo.h"
17 #include "RbFirmataController.h"
18 
19 namespace RoboticsAnimatSim
20 {
21  namespace Robotics
22  {
23  namespace RobotIOControls
24  {
25  namespace Firmata
26  {
27 
29 // Construction/Destruction
31 
32 RbFirmataDynamixelServo::RbFirmataDynamixelServo()
33 {
35  m_iModelNum = -1;
36  m_iFirmwareVersion = -1;
37  m_iReturnDelayTime = -1;
38  m_iCCWAngleLimit = -1;
39  m_iCWAngleLimit = -1;
40  m_iTorqueLimit = -1;
41  m_iIsMoving = -1;
42  m_iLED = -1;
43  m_iAlarm = -1;
44  m_iLastGetRegisterID = 0;
45  m_iLastGetRegisterValue = 0;
46 }
47 
48 RbFirmataDynamixelServo::~RbFirmataDynamixelServo()
49 {
50 }
51 
52 float RbFirmataDynamixelServo::QuantizeServoPosition(float fltPos)
53 {
54  return RbDynamixelServo::QuantizeServoPosition(fltPos);
55 }
56 
57 float RbFirmataDynamixelServo::QuantizeServoVelocity(float fltVel)
58 {
59  return RbDynamixelServo::QuantizeServoVelocity(fltVel);
60 }
61 
62 void RbFirmataDynamixelServo::IOComponentID(int iID)
63 {
64  Std_IsAboveMin((int) 0, iID, true, "ServoID");
65  RbFirmataPart::IOComponentID(iID);
67 }
68 
69 int RbFirmataDynamixelServo::ReadIsMoving(int iServoID)
70 {
71  if(m_lpFirmata)
72  {
73  //If we have made the call then m_iIsMoving = -2, so do not make it again till we get a response.
74  m_lpFirmata->sendDynamixelGetRegister(m_iServoID, P_MOVING, 1);
75  bool bRet = m_lpFirmata->waitForSysExMessage(SYSEX_DYNAMIXEL_GET_REGISTER, 2);
76  }
77 
78  return m_iIsMoving;
79 }
80 
81 int RbFirmataDynamixelServo::ReadLED(int iServoID)
82 {
83  if(m_lpFirmata)
84  {
85  m_lpFirmata->sendDynamixelGetRegister(m_iServoID, P_LED, 1);
86  bool bRet = m_lpFirmata->waitForSysExMessage(SYSEX_DYNAMIXEL_GET_REGISTER, 2);
87  }
88 
89  return m_iLED;
90 }
91 
92 int RbFirmataDynamixelServo::ReadAlarmShutdown(int iServoID)
93 {
94  if(m_lpFirmata)
95  {
96  m_lpFirmata->sendDynamixelGetRegister(m_iServoID, P_ALARM_SHUTDOWN, 1);
97  bool bRet = m_lpFirmata->waitForSysExMessage(SYSEX_DYNAMIXEL_GET_REGISTER, 2);
98  }
99 
100  return m_iAlarm;
101 }
102 
103 void RbFirmataDynamixelServo::WriteReturnDelayTime(int iServoID, int iVal)
104 {
105  if(m_lpFirmata)
106  {
107  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_RETURN_DELAY_TIME, 1, iVal);
108  m_iReturnDelayTime = iVal;
109  }
110 }
111 
112 void RbFirmataDynamixelServo::WriteCCWAngleLimit(int iServoID, int iVal)
113 {
114  if(m_lpFirmata)
115  {
116  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CCW_ANGLE_LIMIT_L, 2, iVal);
117  m_iCCWAngleLimit = iVal;
118  }
119 }
120 
121 void RbFirmataDynamixelServo::WriteCWAngleLimit(int iServoID, int iVal)
122 {
123  if(m_lpFirmata)
124  {
125  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CW_ANGLE_LIMIT_L, 2, iVal);
126  m_iCWAngleLimit = iVal;
127  }
128 }
129 
130 void RbFirmataDynamixelServo::WriteCWComplianceMargin(int iServoID, int iVal)
131 {
132  if(m_lpFirmata)
133  {
134  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CW_COMPLIANCE_MARGIN, 1, iVal);
135  m_iCWComplianceMargin = iVal;
136  }
137 }
138 
139 void RbFirmataDynamixelServo::WriteCCWComplianceMargin(int iServoID, int iVal)
140 {
141  if(m_lpFirmata)
142  {
143  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CCW_COMPLIANCE_MARGIN, 1, iVal);
144  m_iCCWComplianceMargin = iVal;
145  }
146 }
147 
148 void RbFirmataDynamixelServo::WriteCWComplianceSlope(int iServoID, int iVal)
149 {
150  if(m_lpFirmata)
151  {
152  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CW_COMPLIANCE_SLOPE, 1, iVal);
153  m_iCWComplianceSlope = iVal;
154  }
155 }
156 
157 void RbFirmataDynamixelServo::WriteCCWComplianceSlope(int iServoID, int iVal)
158 {
159  if(m_lpFirmata)
160  {
161  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_CCW_COMPLIANCE_SLOPE, 1, iVal);
162  m_iCCWComplianceSlope = iVal;
163  }
164 }
165 
166 void RbFirmataDynamixelServo::WriteMaxTorque(int iServoID, int iVal)
167 {
168  if(m_lpFirmata)
169  {
170  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_MAX_TORQUE_L, 2, iVal);
171  m_iMaxTorque = iVal;
172  }
173 }
174 
175 void RbFirmataDynamixelServo::WriteTorqueLimit(int iServoID, int iVal)
176 {
177  if(m_lpFirmata)
178  {
179  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_MAX_TORQUE_L, 2, iVal);
180  m_iTorqueLimit = iVal;
181  }
182 }
183 
184 void RbFirmataDynamixelServo::WriteGoalPosition(int iServoID, int iPos)
185 {
186  if(m_lpFirmata)
187  {
188  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_GOAL_POSITION_L, 2, iPos);
189  m_iLastGoalPos = iPos;
190  }
191 }
192 
193 void RbFirmataDynamixelServo::WriteMovingSpeed(int iServoID, int iVelocity)
194 {
195  if(m_lpFirmata)
196  {
197  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, P_MOVING_SPEED_L, 2, iVelocity);
198  m_iLastGoalVelocity = iVelocity;
199  }
200 }
201 
202 void RbFirmataDynamixelServo::SetRegister(unsigned char reg, unsigned char length, unsigned int value)
203 {
204  m_lpFirmata->sendDynamixelSetRegister(m_iServoID, reg, length, value);
205 }
206 
207 int RbFirmataDynamixelServo::GetRegister(unsigned char reg, unsigned char length)
208 {
209  m_lpFirmata->sendDynamixelGetRegister(m_iServoID, reg, length);
210  bool bRet = m_lpFirmata->waitForSysExMessage(SYSEX_DYNAMIXEL_GET_REGISTER, 2);
211  if(bRet && m_iLastGetRegisterID == reg)
212  return m_iLastGetRegisterValue;
213  else
214  return -1;
215 }
216 
217 void RbFirmataDynamixelServo::Move(float fltPos, float fltVel)
218 {
219  if(m_lpFirmata)
220  {
221  int iPos = ConvertPosFloatToFP(fltPos);
222  int iVel = 0;
223 
224  if(fltVel >= 0)
225  iVel = ConvertFloatVelocity(fltVel);
226 
227  m_lpFirmata->sendDynamixelMove(m_iServoID, iPos, iVel);
228  }
229 }
230 
232 {
233  if(m_lpFirmata)
234  {
235  int iCWLimit = m_iMinSimPos;
236  int iCCWLimit = m_iMaxSimPos;
237  int iRetDelay = 1;
238 
239  m_lpFirmata->sendDynamixelConfigureServo(m_iServoID, iCWLimit, iCCWLimit, m_iMaxTorque, iRetDelay,
242  }
243 }
244 
246 {
248 
249  //Make sure we detach the servo from the arbotix.
250  if(m_lpFirmata && m_bQueryMotorData)
251  m_lpFirmata->sendDynamixelServoDetach(m_iServoID);
252 }
253 
255 {
256  if(m_lpFirmata)
257  {
258  //Hook up the dynamixel events to notify this servo object.
259  //m_EDynamixelReceived = m_lpFirmata->EDynamixelKeyReceived.connect(boost::bind(&RbFirmataDynamixelServo::DynamixelRecieved, this, _1));
260  m_EDynamixelTransmitError = m_lpFirmata->EDynamixelTransmitError.connect(boost::bind(&RbFirmataDynamixelServo::DynamixelTransmitError, this, _1, _2));
261  m_EDynamixelGetRegister = m_lpFirmata->EDynamixelGetRegister.connect(boost::bind(&RbFirmataDynamixelServo::DynamixelGetRegister, this, _1, _2, _3));
262 
263  //If we are querying data from this motor then inform firmata it
264  //needs to send back constant updates.
266  m_lpFirmata->sendDynamixelServoAttach(m_iServoID);
267 
269 
270  m_lStartServoUpdateTick = m_lpSim->GetTimerTick();
271  }
272 }
273 
274 void RbFirmataDynamixelServo::WaitForMoveToFinish()
275 {
276  //Reset the value for a new wait sequence
277  m_iIsMoving = -1;
278 
279  if(m_lpFirmata)
280  {
281  m_lpFirmata->sendDynamixelStopped(m_iServoID);
282  m_lpFirmata->waitForSysExMessage(SYSEX_DYNAMIXEL_STOPPED, 10);
283  }
284 };
285 
286 #pragma region DataAccesMethods
287 
288 float *RbFirmataDynamixelServo::GetDataPointer(const std::string &strDataType)
289 {
290  float *fltVal = RbDynamixelServo::GetDataPointer(strDataType);
291  if(fltVal)
292  return fltVal;
293  else
294  return RbFirmataPart::GetDataPointer(strDataType);
295 }
296 
297 bool RbFirmataDynamixelServo::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
298 {
299  std::string strType = Std_CheckString(strDataType);
300 
301  if(RbFirmataPart::SetData(strDataType, strValue, false))
302  return true;
303 
304  if(RbDynamixelServo::SetData(strDataType, strValue))
305  return true;
306 
307  //If it was not one of those above then we have a problem.
308  if(bThrowError)
309  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
310 
311  return false;
312 }
313 
314 void RbFirmataDynamixelServo::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
315 {
316  RbFirmataPart::QueryProperties(aryProperties);
317  RbDynamixelServo::QueryProperties(aryProperties);
318 }
319 
320 #pragma endregion
321 
322 void RbFirmataDynamixelServo::UpdateMotorData()
323 {
324  if(!m_lpSim->InSimulation() && m_bQueryMotorData && m_lpFirmata)
325  {
326  if(m_lpFirmata->_dynamixelServos[m_iServoID]._allChanged)
327  UpdateAllMotorData();
328  else if(m_lpFirmata->_dynamixelServos[m_iServoID]._keyChanged)
329  UpdateKeyMotorData();
330  }
331 }
332 
333 void RbFirmataDynamixelServo::UpdateKeyMotorData()
334 {
335  if(m_lpFirmata && m_lpSim)
336  {
337  m_iPresentPos = m_lpFirmata->_dynamixelServos[m_iServoID]._actualPosition;
338  m_iPresentVelocity = m_lpFirmata->_dynamixelServos[m_iServoID]._actualSpeed;
339 
340  m_fltPresentPos = ConvertPosFPToFloat(m_iPresentPos);
342 
343  m_lpFirmata->_dynamixelServos[m_iServoID]._keyChanged = false;
344 
345  //Calculate time between updates.
346  unsigned long long lEndTick = m_lpSim->GetTimerTick();
347  m_fltReadParamTime = m_lpSim->TimerDiff_m(m_lStartServoUpdateTick, lEndTick);
349  m_lStartServoUpdateTick = lEndTick;
350  }
351 }
352 
353 void RbFirmataDynamixelServo::UpdateAllMotorData()
354 {
355  UpdateKeyMotorData();
356 
357  if(m_lpFirmata)
358  {
359  m_iLoad = m_lpFirmata->_dynamixelServos[m_iServoID]._load;
360  m_iVoltage = m_lpFirmata->_dynamixelServos[m_iServoID]._voltage;
361  m_iTemperature = m_lpFirmata->_dynamixelServos[m_iServoID]._temperature;
362 
364  m_fltVoltage = m_iVoltage/100.0;
366 
367  m_lpFirmata->_dynamixelServos[m_iServoID]._allChanged = false;
368  }
369 }
370 
371 
372 void RbFirmataDynamixelServo::DynamixelRecieved(const int &iServoID)
373 {
374  if(iServoID == 1 && m_iServoID == 1)
375  {
376  //std::cout << "Servo: " << iServoID << ", Pos: " << m_iPresentPos << ", Vel: " << m_iPresentVelocity << "\r\n";
377  //std::cout << "Servo: " << iServoID << ", Pos: " << m_lpFirmata->_dynamixelServos[m_iServoID]._actualPosition << ", Vel: " << m_lpFirmata->_dynamixelServos[m_iServoID]._actualSpeed << "\r\n";
378  }
379 }
380 
381 void RbFirmataDynamixelServo::DynamixelTransmitError(const int &iCmd, const int &iServoID)
382 {
383  if(iServoID == m_iServoID)
384  {
385  switch(iCmd)
386  {
387  case SYSEX_DYNAMIXEL_STOP:
388  Stop();
389  break;
390  case SYSEX_DYNAMIXEL_CONFIGURE_SERVO:
391  ConfigureServo();
392  break;
393  }
394 
395  std::cout << "Transmit error Cmd: " << iCmd << ", servo: " << iServoID << "\r\n";
396  }
397 }
398 
399 void RbFirmataDynamixelServo::DynamixelGetRegister(const unsigned char &iServoID, const unsigned char &iReg, const unsigned int &iValue)
400 {
401  if(iServoID == m_iServoID)
402  {
403  m_iLastGetRegisterID = iReg;
404  m_iLastGetRegisterValue = iValue;
405 
406  //std::cout << "Get Register Servo: " << iServoID << ", Reg: " << iReg << ", Value: " << iValue << "\r\n";
407 
408  switch(iReg)
409  {
410  case P_MODEL_NUMBER_L:
411  m_iModelNum = iValue;
412  break;
413  case P_FIRMWARE_VERSION:
414  m_iFirmwareVersion = iValue;
415  break;
416  case P_RETURN_DELAY_TIME:
417  m_iReturnDelayTime = iValue;
418  break;
419  case P_CW_ANGLE_LIMIT_L:
420  m_iCWAngleLimit = iValue;
421  break;
422  case P_CCW_ANGLE_LIMIT_L:
423  m_iCCWAngleLimit = iValue;
424  break;
425  case P_MAX_TORQUE_L:
426  m_iTorqueLimit = iValue;
427  break;
428  case P_MOVING:
429  m_iIsMoving = iValue;
430  break;
431  case P_LED:
432  m_iLED = iValue;
433  break;
434  case P_ALARM_SHUTDOWN:
435  m_iAlarm = iValue;
436  break;
437  }
438 
439  }
440 }
441 
442 void RbFirmataDynamixelServo::AddMotorUpdate(int iPos, int iSpeed)
443 {
444  if(!m_lpSim->InSimulation() && m_lpFirmata)
445  m_lpFirmata->sendDynamixelSynchMoveAdd(m_iServoID, iPos, iSpeed);
446 
447  m_fltIOValue = iSpeed;
448 }
449 
453 {
454  if(!m_lpSim->InSimulation() && m_lpFirmata)
455  {
456  m_lpFirmata->sendDynamixelStop(m_iServoID);
457 
458  //Set the next and goal pos to the current pos so we do not
459  //attempt to resend a servo command.
462 
463  //Do a similar thing with the velocity values so it thinks it is at 0 velocity.
464  m_iNextGoalVelocity = -1;
466  }
467  else
469 }
470 
472 {
474 
475  m_lpMotorJoint = dynamic_cast<MotorizedJoint *>(m_lpPart);
476 
477  GetLimitValues();
478 
479  RecalculateParams();
480 }
481 
482 //We need to get some key data when we first do setup so that info is available
483 //if queried
485 {
486  if(!m_lpSim->InSimulation() && m_lpFirmata)
487  {
490  InitMotorData();
491 
492  //Set the next goal positions to the current ones.
494 
496  }
497 }
498 
500 {
501  unsigned long long lStepStartTick = m_lpSim->GetTimerTick();
502 
503  UpdateMotorData();
504 
505  SetMotorPosVel();
506 
507  unsigned long long lEndStartTick = m_lpSim->GetTimerTick();
508  m_fltStepIODuration = m_lpSim->TimerDiff_m(lStepStartTick, lEndStartTick);
509 }
510 
512 {
513  ShutdownMotor();
514 }
515 
516 void RbFirmataDynamixelServo::StepSimulation()
517 {
519  RbDynamixelServo::StepSimulation();
520 }
521 
522 void RbFirmataDynamixelServo::ResetSimulation()
523 {
525  RbDynamixelServo::ResetSimulation();
526 }
527 
528 void RbFirmataDynamixelServo::Load(StdUtils::CStdXml &oXml)
529 {
530  RbFirmataPart::Load(oXml);
531  RbDynamixelServo::Load(oXml);
532 }
533 
534 void RbFirmataDynamixelServo::MicroSleep(unsigned int iTime)
535 {
536  if(m_lpSim)
537  m_lpSim->MicroSleep(iTime);
538 }
539 
540 Simulator *RbFirmataDynamixelServo::GetSimulator()
541 {
542  return m_lpSim;
543 }
544 
545  } //Firmata
546  } //RobotIOControls
547  } // Robotics
548 } //RoboticsAnimatSim
549 
unsigned long long m_lStartServoUpdateTick
The tick when we start a servo update.
virtual float ConvertFPVelocity(int iVel)
Gets the floating point value of the servo actual velocity. The fixed point value is retrieved from t...
virtual void ShutdownMotor()
Shuts the motor down cleanly and ensures that it is not continuing to move after processing has stopp...
int m_iCWComplianceSlope
Used to set the CW compliance slope of the servo.
int m_iMinSimPos
This is the minimum angle in fixed point that can be used for this servo as specified in the simulati...
virtual bool InSimulation()
Used to determine if we are running in a simulation, or in a real control mode.
Definition: Simulator.cpp:1673
int m_iMaxTorque
Used to set the maximum torque of the servo.
virtual void QueryProperties(CStdPtrArray< TypeProperty > &aryProperties)
Queries this object for a list of properties that can be changed using SetData.
int m_iCCWComplianceMargin
Used to set the CCW compliance margin of the servo.
virtual float * GetDataPointer(const std::string &strDataType)
Returns a float pointer to a data item of interest in this object.
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
int m_iMaxSimPos
This is the maximum angle in fixed point that can be used for this servo as specified in the simulati...
int m_iVoltage
The current voltage that was last read in for this servo.
virtual void Initialize()
Initializes this object.
virtual float ConvertFPLoad(int iLoad)
Gets the floating point value of the servo load. The fixed point value is retrieved from the servo an...
int m_iLastGoalVelocity
Keeps track of the last servo goal velocity that we set.
virtual void Stop()
Attempts to stop the motor at its current position.
int m_iServoID
ID used for communications with this servo.
virtual void StepIO(int iPartIdx)
This method is used to send/recieve the actual IO. This will often be in a seperate thread than the S...
int m_iNextGoalVelocity
This is the goal velocity that you would like to use in the next time step.
int m_iLastGoalPos
Keeps track of the last servo goal position that we set.
bool Std_IsAboveMin(int iMinVal, int iVal, bool bThrowError, std::string strParamName, bool bInclusiveLimit)
Tests if a number is above a minimum value.
float m_fltPresentPos
The current position that was last read in for this servo.
int m_iModelNum
Keeps track of the servos model number. This is updated once at the start of the sim and then used th...
int m_iTemperature
The current temperature that was last read in for this servo.
virtual int ServoID()
Gets the servo ID being used for communications.
int m_iPresentPos
The current position that was last read in for this servo.
virtual void ShutdownMotor()
Shuts the motor down cleanly and ensures that it is not continuing to move after processing has stopp...
Declares the vortex structure class.
int m_iFirmwareVersion
Keeps track of the servos firmware version. This is updated once at the start of the sim and then use...
float m_fltReadParamTime
The time taken to read the params of this motor for the current step.
virtual int ConvertFloatVelocity(float fltVelocity)
Gets the fixed point velocity form the floating point.
int m_iNextGoalPos
This is the goal position that you would like to use in the next time step.
Declares the vortex hinge class.
A standard xml manipulation class.
Definition: StdXml.h:19
virtual void StepSimulation()
Step the simulation for this object.
Definition: AnimatBase.cpp:643
int m_iLoad
The current load that was last read in for this servo.
virtual void ConfigureServo()
Checks the current limits on the motor and sets them according to the simulation params.
virtual void ShutdownIO()
This method is called just before the IO thread is closed down. It gives the IO objects a chance to d...
float m_fltPresentVelocity
The current velocity that was last read in for this servo.
MotorizedJoint * m_lpMotorJoint
Pointer to an associated motorized joint.
float m_fltLoad
The current load that was last read in for this servo.
int m_iCWComplianceMargin
Used to set the CW compliance margin of the servo.
virtual void ResetSimulation()
Resets the simulation back to time 0.
virtual void SetMaxSimPos(float fltVal)
Sets the maximum position that the simulation will allow for this joint in radians.
virtual void SetMinSimPos(float fltVal)
Sets the minimum position that the simulation will allow for this joint in radians.
int m_iCCWComplianceSlope
Used to set the CCW compliance slope of the servo.
virtual void Move(float fltPos, float fltVel)
Moves the motor to the specified position at the given speed.
virtual void SetupIO()
This method is called after all connections to whatever control board have been made. It is meant to be used to setup connection information specific to this part. For example, We connect to a Firmata microcontroller like an Arduino, and then do a setup that could take some time. We should not attempt to setup any of the pins until after the board itself has been setup. After that we need to loop through and setup all the parts. That is what this method is for.
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.
float m_fltVoltage
The current voltage that was last read in for this servo.
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
virtual void InitMotorData()
Initializes the internal data on position and velocity from the actual motor.
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
float m_fltTemperature
The current temperature that was last read in for this servo.
virtual void InitMotorData()
Initializes the internal data on position and velocity from the actual motor.
int m_iPresentVelocity
The current velocity that was last read in for this servo.