AnimatLab  2
Test
RbDynamixelServo.cpp
1 // RbDynamixelServo.cpp: implementation of the RbDynamixelServo 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 "RbDynamixelServo.h"
16 
17 #define DYN_ID (2)
18 #define DYN_LENGTH (3)
19 #define DYN_INSTRUCTION (4)
20 #define DYN_ERRBIT (4)
21 #define DYN_PARAMETER (5)
22 #define DYN_DEFAULT_BAUDNUMBER (1)
23 
24 namespace RoboticsAnimatSim
25 {
26  namespace Robotics
27  {
28  namespace RobotIOControls
29  {
30 
32 // Construction/Destruction
34 
35 RbDynamixelServo::RbDynamixelServo()
36 {
37  m_iServoID = 0;
38  m_iMinPosFP = 0;
39  m_iMaxPosFP = 1023;
40 
41  m_iLastGoalPos = -1;
42 
43  m_iMinVelocityFP = 1;
44  m_iMaxVelocityFP = 1023;
45  m_fltRPMPerFPUnit = 0.111; //max rotations (rad) per fixed point unit.
46  m_iLastGoalVelocity = -10000;
47 
48  m_fltMinAngle = -150;
49  m_fltMaxAngle = 150;
50 
51  m_iMinLoadFP = 0;
52  m_iMaxLoadFP = 1023;
53 
54  m_iNextGoalPos = 0;
56 
57  m_iPresentPos = 0;
59  m_iLoad = 0;
60  m_iVoltage = 0;
61  m_iTemperature = 0;
62 
63  m_fltPresentPos = 0;
65  m_fltLoad = 0;
66  m_fltVoltage = 0;
67  m_fltTemperature = 0;
68 
70 
71  m_bQueryMotorData = true;
72 
73  m_lpMotorJoint = NULL;
74 
75  m_bIsHinge = true;
79 
80  m_fltHiLimit = RB_PI/2;
81  m_fltLowLimit = -RB_PI/2;
82 
83  m_fltIOPos = 0;
84  m_fltIOVelocity = 0;
85 
86  m_bNeedSetVelStopPos = false;
87  m_bVelStopPosSet = false;
88  m_bResetToStartPos = false;
89 
92 
95 
96  m_iMaxTorque = 1023;
97 
98  RecalculateParams();
99 }
100 
101 RbDynamixelServo::~RbDynamixelServo()
102 {
103  //Do not delete because we do not own it.
104  m_lpMotorJoint = NULL;
105 }
106 
107 void RbDynamixelServo::QueryMotorData(bool bVal) {m_bQueryMotorData = bVal;}
108 
109 bool RbDynamixelServo::QueryMotorData() {return m_bQueryMotorData;}
110 
111 void RbDynamixelServo::RecalculateParams()
112 {
114  m_iCenterPosFP = round(m_iTotalAngle/2.0);
115 
116  m_fltTotalAngle = (float) (((m_fltMaxAngle-m_fltMinAngle)/180.0)*RB_PI);
117 
122 
123  if(m_iTotalAngle > 0)
125 
127 
128  if(m_fltTotalAngle > 0)
130 
132 
133  m_fltMaxPosSec = (m_fltRPMPerFPUnit*2*RB_PI)/60.0f;
134  m_fltConvertFPToPosS = m_fltMaxPosSec; //0.01162389281828223498231178051813f; //0.111 rot/min = 0.0116 rad/s
135 
136  if(m_fltConvertFPToPosS > 0)
138 
139  int iTotalLoad = (m_iMaxLoadFP - m_iMinLoadFP);
140  if(iTotalLoad > 0)
141  m_fltConvertFPToLoad = 100.0/iTotalLoad;
142 
143  if(!m_bIsHinge)
144  {
145  if(m_fltTotalAngle >= 0)
147 
150  }
151  else
152  {
155  }
156 
157 }
158 
159 void RbDynamixelServo::MinPosFP(int iVal)
160 {
161  Std_IsBelowMax((int) m_iMaxPosFP, iVal, true, "MinPosFP");
162  m_iMinPosFP = iVal;
163  RecalculateParams();
164 }
165 
166 int RbDynamixelServo::MinPosFP() {return m_iMinPosFP;}
167 
168 void RbDynamixelServo::MaxPosFP(int iVal)
169 {
170  Std_IsAboveMin((int) m_iMinPosFP, iVal, true, "MinPosFP");
171  m_iMaxPosFP = iVal;
172  RecalculateParams();
173 }
174 
175 int RbDynamixelServo::MaxPosFP() {return m_iMaxPosFP;}
176 
177 void RbDynamixelServo::MinAngle(float fltVal)
178 {
179  Std_IsBelowMax((float) m_fltMaxAngle, fltVal, true, "MinAngle");
180  m_fltMinAngle = fltVal;
181  RecalculateParams();
182 }
183 
184 float RbDynamixelServo::MinAngle() {return m_fltMinAngle;}
185 
186 void RbDynamixelServo::MaxAngle(float fltVal)
187 {
188  Std_IsAboveMin((float) m_fltMinAngle, fltVal, true, "MaxAngle");
189  m_fltMaxAngle = fltVal;
190  RecalculateParams();
191 }
192 
193 float RbDynamixelServo::MaxAngle() {return m_fltMaxAngle;}
194 
195 void RbDynamixelServo::MinVelocityFP(int iVal)
196 {
197  Std_IsBelowMax((int) m_iMaxVelocityFP, iVal, true, "MinVelocityFP");
198  m_iMinVelocityFP = iVal;
199  RecalculateParams();
200 }
201 
202 int RbDynamixelServo::MinVelocityFP() {return m_iMinVelocityFP;}
203 
204 void RbDynamixelServo::MaxVelocityFP(int iVal)
205 {
206  Std_IsAboveMin((int) m_iMinVelocityFP, iVal, true, "MaxVelocityFP");
207  m_iMaxVelocityFP = iVal;
208  RecalculateParams();
209 }
210 
211 int RbDynamixelServo::MaxVelocityFP() {return m_iMaxVelocityFP;}
212 
213 void RbDynamixelServo::RPMPerFPUnit(float fltVal)
214 {
215  Std_IsAboveMin((float) 0, fltVal, true, "RPMPerFPUnit");
216  m_fltRPMPerFPUnit = fltVal;
217  RecalculateParams();
218 }
219 
220 float RbDynamixelServo::RPMPerFPUnit() {return m_fltRPMPerFPUnit;}
221 
222 void RbDynamixelServo::MinLoadFP(int iVal)
223 {
224  Std_IsBelowMax((int) m_iMaxLoadFP, iVal, true, "MinLoadFP");
225  m_iMinLoadFP = iVal;
226  RecalculateParams();
227 }
228 
229 int RbDynamixelServo::MinLoadFP() {return m_iMinLoadFP;}
230 
231 void RbDynamixelServo::MaxLoadFP(int iVal)
232 {
233  Std_IsAboveMin((int) m_iMinLoadFP, iVal, true, "MaxLoadFP");
234  m_iMaxLoadFP = iVal;
235  RecalculateParams();
236 }
237 
238 int RbDynamixelServo::MaxLoadFP() {return m_iMaxLoadFP;}
239 
240 int RbDynamixelServo::CWComplianceMargin() {return m_iCWComplianceMargin;}
241 
242 void RbDynamixelServo::CWComplianceMargin(int iVal)
243 {
244  Std_InValidRange((int) 0, (int) 255, iVal, true, "CWComplianceMargin");
245  m_iCWComplianceMargin = iVal;
246 }
247 
248 int RbDynamixelServo::CCWComplianceMargin() {return m_iCCWComplianceMargin;}
249 
250 void RbDynamixelServo::CCWComplianceMargin(int iVal)
251 {
252  Std_InValidRange((int) 0, (int) 255, iVal, true, "CCWComplianceMargin");
253  m_iCCWComplianceMargin = iVal;
254 }
255 
256 int RbDynamixelServo::CWComplianceSlope() {return m_iCWComplianceSlope;}
257 
258 void RbDynamixelServo::CWComplianceSlope(int iVal)
259 {
260  Std_InValidRange((int) 0, (int) 255, iVal, true, "CWComplianceSlope");
261  m_iCWComplianceSlope = iVal;
262 }
263 
264 int RbDynamixelServo::CCWComplianceSlope() {return m_iCCWComplianceSlope;}
265 
266 void RbDynamixelServo::CCWComplianceSlope(int iVal)
267 {
268  Std_InValidRange((int) 0, (int) 255, iVal, true, "CCWComplianceSlope");
269  m_iCCWComplianceSlope = iVal;
270 }
271 
272 int RbDynamixelServo::MaxTorque() {return m_iMaxTorque;}
273 
274 void RbDynamixelServo::MaxTorque(int iVal)
275 {
276  Std_InValidRange((int) 0, (int) 1023, iVal, true, "MaxTorque");
277  m_iMaxTorque = iVal;
278 }
279 
280 float RbDynamixelServo::ConvertPosFPToFloat(int iPos)
281 {
282  float fltAngle = (m_fltPosFPToFloatSlope*iPos) + m_fltPosFPToFloatIntercept;
283  float fltPos = m_fltFPToFloatTranslation * fltAngle;
284  return fltPos;
285 }
286 
287 int RbDynamixelServo::ConvertPosFloatToFP(float fltPos)
288 {
289  float fltAngle = m_fltFloatToFPTranslation * fltPos;
290  int iPos = (m_fltPosFloatToFPSlope*fltAngle) + m_fltPosFloatToFPIntercept;
291  return iPos;
292 }
293 void RbDynamixelServo::ServoID(int iID)
303 {
304  if(iID >= 0)
305  m_iServoID = iID;
306 }
307 
317 
329 {
330  //Verify we are not putting invalid values in.
331  if(iVelocity < m_iMinVelocityFP) iVelocity = m_iMinVelocityFP;
332  if(iVelocity > m_iMaxVelocityFP) iVelocity = m_iMaxVelocityFP;
333 
334  //If the velocity we are setting is the same as we just set then no point sending it.
335  if(m_iLastGoalVelocity == iVelocity)
336  return;
337 
338  m_iLastGoalVelocity = iVelocity;
339 
340  WriteMovingSpeed(m_iServoID, iVelocity);
341 }
342 
352 {
353  //Verify we are not putting invalid values in.
354  if(iVelocity != -1 && iVelocity < m_iMinVelocityFP) iVelocity = m_iMinVelocityFP;
355  if(iVelocity != -1 && iVelocity > m_iMaxVelocityFP) iVelocity = m_iMaxVelocityFP;
356 
357  m_iNextGoalVelocity = iVelocity;
358 }
359 
360 
368 {
369  //If the velocity we are setting is the same as we just set then no point sending it.
370  if(m_iLastGoalVelocity == 0)
371  return;
372 
374 
375  WriteMovingSpeed(m_iServoID, 0);
376 }
377 
385 {
386  //If the velocity we are setting is the same as we just set then no point sending it.
387  if(m_iLastGoalVelocity == 0)
388  return;
389 
391 }
392 
402 {
403  int iGoalVel = ReadMovingSpeed(m_iServoID);
404 
405  return iGoalVel;
406 }
407 
408 
418 void RbDynamixelServo::SetGoalVelocity(float fltVelocity)
419 {
420  int iVel = ConvertFloatVelocity(fltVelocity);
421 
422  SetGoalVelocity_FP(iVel);
423 }
424 
435 {
436  int iVel = ConvertFloatVelocity(fltVelocity);
437 
438  //If the velocity is being set to 0 then code this special.
439  if(fltVelocity == 0)
440  iVel = -1;
441 
443 }
444 
454 {
455  int iGoal = GetGoalVelocity_FP();
456 
457  float fltPos = iGoal*m_fltConvertFPToPosS;
458 
459  return fltPos;
460 }
461 
471 
481 {
482  //Verify we are not putting invalid values in.
483  if(iPos < m_iMinSimPos) iPos = m_iMinSimPos;
484  if(iPos > m_iMaxSimPos) iPos = m_iMaxSimPos;
485 
486  //If the position we are setting is the same as we just set then no point sending it.
487  if(m_iLastGoalPos == iPos)
488  return;
489 
490  m_iLastGoalPos = iPos;
491 
492  WriteGoalPosition(m_iServoID, iPos);
493 }
494 
504 {
505  //Verify we are not putting invalid values in.
506  if(iPos < m_iMinSimPos) iPos = m_iMinSimPos;
507  if(iPos > m_iMaxSimPos) iPos = m_iMaxSimPos;
508 
509  m_iNextGoalPos = iPos;
510 }
511 
521 {
522  int iGoalPos = ReadGoalPosition(m_iServoID);
523  return iGoalPos;
524 }
525 
537 {
538  int iPos = ConvertPosFloatToFP(fltPos);
539 
540  SetGoalPosition_FP(iPos);
541 }
542 
555 {
556  int iPos = ConvertPosFloatToFP(fltPos);
557 
559 }
560 
570 {
571  int iGoal = GetGoalPosition_FP();
572 
573  float fltPos = ConvertPosFPToFloat(iGoal);
574 
575  return fltPos;
576 }
577 
587 
597 {
598  int iPos = ReadPresentPosition(m_iServoID);
599  return iPos;
600 }
601 
611 {
612  int iPos = GetActualPosition_FP();
613 
614  float fltPos = ConvertPosFPToFloat(iPos);
615 
616  return fltPos;
617 }
618 
628 {
629  int iVel = ReadPresentSpeed(m_iServoID);
630  return iVel;
631 }
632 
644 {
645  int iDir = 1;
646  if(iVel > m_iMaxVelocityFP)
647  {
648  iVel -= m_iMaxVelocityFP;
649  iDir = -1;
650  }
651 
652  float fltVel = iDir*iVel*(m_fltFPToFloatTranslation*m_fltConvertFPToPosS);
653  return fltVel;
654 }
655 
667 {
668  if(fltVelocity < -0.4)
669  fltVelocity = fltVelocity;
670 
671  int iVel = (int) (fabs(fltVelocity)*(m_fltFloatToFPTranslation*m_fltConvertPosSToFP));
672 
673  if(iVel < m_iMinVelocityFP)
674  iVel = m_iMinVelocityFP;
675  if(iVel > m_iMaxVelocityFP)
676  iVel = m_iMaxVelocityFP;
677 
678  return iVel;
679 }
680 
681 
691 {
692  int iVel = GetActualVelocity_FP();
693  return ConvertFPVelocity(iVel);
694 }
695 
705 {
706  int iLoad = ReadPresentLoad(m_iServoID);
707  return iLoad;
708 }
709 
721 {
722  int iDir = -1;
723  if(iLoad > m_iMaxLoadFP)
724  {
725  iLoad -= m_iMaxLoadFP;
726  iDir = 1;
727  }
728 
729  float fltLoad = iDir*iLoad*m_fltConvertFPToLoad;
730 
731  return fltLoad;
732 }
733 
743 {
744  int iLoad = GetActualLoad_FP();
745  return ConvertFPLoad(iLoad);
746 }
747 
757 {
758  int iVoltage = ReadPresentVoltage(m_iServoID);
759  return iVoltage;
760 }
761 
771 {
772  int iVoltage = GetActualVoltage_FP();
773  float fltVoltage = iVoltage/100.0;
774  return fltVoltage;
775 }
776 
786 {
787  float fltTemp = ((GetActualTemperatureCelcius()*(9.0/5.0)) + 32.0);
788  return fltTemp;
789 }
790 
800 {
801  float fltTemp = (float) ReadPresentTemperature(m_iServoID);
802  return fltTemp;
803 }
804 
814 {
815  int iMoving = ReadIsMoving(m_iServoID);
816 
817  if(iMoving == 0)
818  return false;
819  else
820  return true;
821 }
822 
833 void RbDynamixelServo::GetIsLEDOn(bool &bIsBlueOn, bool &bIsGreenOn, bool &bIsRedOn)
834 {
835  int iLED = ReadLED(m_iServoID);
836 
837  bIsBlueOn = iLED & 0x04;
838  bIsGreenOn = iLED & 0x02;
839  bIsRedOn = iLED & 0x01;
840 }
841 
851 {
852  int iAlarm = ReadAlarmShutdown(m_iServoID);
853 
854  if(iAlarm)
855  return true;
856  else
857  return false;
858 }
859 
869 {
870  int iModel = ReadModelNumber(m_iServoID);
871  return iModel;
872 }
873 
883 {
884  int iID = ReadID(m_iServoID);
885  return iID;
886 }
887 
897 {
898  int iID = ReadFirmwareVersion(m_iServoID);
899  return iID;
900 }
901 
902 void RbDynamixelServo::WaitForMoveToFinish()
903 {
904  do
905  {
906 #ifdef Win32
907  //Do not attempt to sleep in linux while in a spinlock. Windows is fine with it.
908  MicroSleep(5000);
909 #endif
910  } while(GetIsMoving());
911 }
912 
920 {
922 }
923 
930 void RbDynamixelServo::Move(float fltPos, float fltVel)
931 {
932  if(fltVel < 0)
934  else
935  SetGoalVelocity(fltVel);
936  boost::this_thread::sleep(boost::posix_time::microseconds(100));
937 
938  SetGoalPosition(fltPos);
939  boost::this_thread::sleep(boost::posix_time::microseconds(100));
940 }
941 
949 {
950  int iMinPos = GetCWAngleLimit_FP();
951  int iMaxPos = GetCCWAngleLimit_FP();
952  int iRetDelay = GetReturnDelayTime_FP();
953  int iRetTorqueLimit = GetTorqueLimit_FP();
954  int iRetCWComplMargin = GetCWComplianceMargin_FP();
955  int iRetCCWComplMargin = GetCCWComplianceMargin_FP();
956  int iRetCWComplSlope = GetCWComplianceSlope_FP();
957  int iRetCCWComplSlope = GetCCWComplianceSlope_FP();
958 
959  if(iMinPos != m_iMinSimPos)
961 
962  if(iMaxPos != m_iMaxSimPos)
964 
965  if(iRetDelay > 1)
967 
968  //if(iRetTorqueLimit != m_iMaxTorque)
969  // SetMaxTorque_FP(m_iMaxTorque);
970 
971  if(iRetCWComplMargin != m_iCWComplianceMargin)
972  SetCWComplianceMargin_FP(m_iCWComplianceMargin);
973 
974  if(iRetCCWComplMargin != m_iCCWComplianceMargin)
975  SetCCWComplianceMargin_FP(m_iCCWComplianceMargin);
976 
977  if(iRetCWComplSlope != m_iCWComplianceSlope)
978  SetCWComplianceSlope_FP(m_iCWComplianceSlope);
979 
980  if(iRetCCWComplSlope != m_iCCWComplianceSlope)
981  SetCCWComplianceSlope_FP(m_iCCWComplianceSlope);
982 }
983 
991 {
992  ConfigureServo();
993 
995  {
996  //Move to the reset pos at max speed.
997  Move(0, -1);
998  WaitForMoveToFinish();
999  }
1000 
1002 
1003  //Reset the goal velocity to the minimum value.
1004  SetGoalVelocity_FP(1);
1005 
1006  std::cout << "Reset Servo " << m_iServoID << " Position: " << m_iLastGoalPos << "\r\n";
1007 }
1008 
1016 {
1017  int iCurPos = GetActualPosition_FP();
1018 
1019  //Reset the goal velocity to the minimum value.
1020  SetGoalVelocity_FP(1);
1021  SetGoalPosition_FP(iCurPos);
1022 
1023  std::cout << "Shutting down servo: " << m_iServoID << ", Pos: " << iCurPos << "\r\n";
1024 }
1025 
1035 {
1036  if(iVal >= 0 && iVal < 256)
1037  WriteReturnDelayTime(m_iServoID, iVal);
1038 }
1039 
1049 {
1050  return ReadReturnDelayTime(m_iServoID);
1051 }
1052 
1053 
1063 {
1064  if(iVal >= m_iMinPosFP && iVal <= m_iMaxPosFP)
1065  WriteCCWAngleLimit(m_iServoID, iVal);
1066 }
1067 
1077 {
1078  int iPos = ConvertPosFloatToFP(fltVal);
1079  SetCCWAngleLimit_FP(iPos);
1080 }
1081 
1091 
1101 {
1102  if(!m_bIsHinge)
1103  {
1104  Std_IsAboveMin((float) 0, fltVal, true, "TranslationRange", true);
1105 
1106  m_fltTranslationRange = fltVal;
1107  RecalculateParams();
1108  }
1109 }
1110 
1120 {
1121  return ReadCCWAngleLimit(m_iServoID);
1122 }
1123 
1124 
1134 {
1135  int iPos = GetCCWAngleLimit_FP();
1136  return ConvertPosFPToFloat(iPos);
1137 }
1138 
1148 {
1149  if(iVal >= m_iMinPosFP && iVal <= m_iMaxPosFP)
1150  WriteCWAngleLimit(m_iServoID, iVal);
1151 }
1152 
1162 {
1163  int iPos = ConvertPosFloatToFP(fltVal);
1164  SetCWAngleLimit_FP(iPos);
1165 }
1166 
1176 {
1177  return ReadCWAngleLimit(m_iServoID);
1178 }
1179 
1189 {
1190  int iPos = GetCWAngleLimit_FP();
1191  return ConvertPosFPToFloat(iPos);
1192 }
1193 
1194 void RbDynamixelServo::SetCWComplianceMargin_FP(int iVal)
1195 {
1196  if(iVal >= 0 && iVal <= 255)
1197  WriteCWComplianceMargin(m_iServoID, iVal);
1198 }
1199 
1200 int RbDynamixelServo::GetCWComplianceMargin_FP()
1201 {
1202  return ReadCWComplianceMargin(m_iServoID);
1203 }
1204 
1205 void RbDynamixelServo::SetCCWComplianceMargin_FP(int iVal)
1206 {
1207  if(iVal >= 0 && iVal <= 255)
1208  WriteCCWComplianceMargin(m_iServoID, iVal);
1209 }
1210 
1211 int RbDynamixelServo::GetCCWComplianceMargin_FP()
1212 {
1213  return ReadCCWComplianceMargin(m_iServoID);
1214 }
1215 
1216 void RbDynamixelServo::SetCWComplianceSlope_FP(int iVal)
1217 {
1218  if(iVal >= 0 && iVal <= 255)
1219  WriteCWComplianceSlope(m_iServoID, iVal);
1220 }
1221 
1222 int RbDynamixelServo::GetCWComplianceSlope_FP()
1223 {
1224  return ReadCWComplianceSlope(m_iServoID);
1225 }
1226 
1227 void RbDynamixelServo::SetCCWComplianceSlope_FP(int iVal)
1228 {
1229  if(iVal >= 0 && iVal <= 255)
1230  WriteCCWComplianceSlope(m_iServoID, iVal);
1231 }
1232 
1233 int RbDynamixelServo::GetCCWComplianceSlope_FP()
1234 {
1235  return ReadCCWComplianceSlope(m_iServoID);
1236 }
1237 
1238 void RbDynamixelServo::SetMaxTorque_FP(int iVal)
1239 {
1240  if(iVal >= 0 && iVal <= 1023)
1241  WriteMaxTorque(m_iServoID, iVal);
1242 }
1243 
1244 int RbDynamixelServo::GetMaxTorque_FP()
1245 {
1246  return ReadMaxTorque(m_iServoID);
1247 }
1248 
1258 
1268 
1278 {
1279  m_iMinSimPos = ConvertPosFloatToFP(fltVal);
1280 
1283 
1284  m_fltMinSimPos = ConvertPosFPToFloat(m_iMinSimPos);
1285 }
1286 
1296 
1306 
1316 {
1317  m_iMaxSimPos = ConvertPosFloatToFP(fltVal);
1318 
1321 
1322  m_fltMaxSimPos = ConvertPosFPToFloat(m_iMaxSimPos);
1323 }
1324 
1334 {
1335  if(iVal >= m_iMinLoadFP && iVal <= m_iMaxLoadFP)
1336  WriteTorqueLimit(m_iServoID, iVal);
1337 }
1338 
1348 {
1349  return ReadTorqueLimit(m_iServoID);
1350 }
1351 
1352 bool RbDynamixelServo::ResetToStartPos() {return m_bResetToStartPos;}
1353 
1354 void RbDynamixelServo::ResetToStartPos(bool bVal) {m_bResetToStartPos = bVal;}
1355 
1356 void RbDynamixelServo::SetMotorPosVel()
1357 {
1358  //std::cout << m_lpSim->Time() << ", servo: " << m_iServoID << ", Pos: " << m_iNextGoalPos << ", LasPos: " << m_iLastGoalPos << ", Vel: " << m_iNextGoalVelocity << ", LastVel: " << m_iLastGoalVelocity << "\r\n";
1359 
1361  {
1362  m_bVelStopPosSet = true;
1363 
1364  //Only set the goal position to the present position when the velocity
1365  //is changing to zero. If you keep setting it then the part skates.
1366  Stop();
1367  }
1368 
1370  {
1371  //std::cout << GetSimulator()->Time() << ", servo: " << m_iServoID << ", Pos: " << m_iNextGoalPos << ", LasPos: " << m_iLastGoalPos << ", Vel: " << m_iNextGoalVelocity << ", LastVel: " << m_iLastGoalVelocity << "\r\n";
1372  //std::cout << "************" << GetSimulator()->Time() << ", servo: " << m_iServoID << ", Pos: " << m_iNextGoalPos << ", Vel: " << m_iNextGoalVelocity << "\r\n";
1373 
1374  //If the next goal velocity was set to -1 then we are trying to set velocity to 0. So lets set the goal position to its current
1375  //loctation and velocity to lowest value.
1376  if(m_iNextGoalVelocity == -1)
1377  {
1379  m_iNextGoalVelocity = 1;
1380  }
1381 
1382  //Add a new update data so we can send the move command out synchronously to all motors.
1383  AddMotorUpdate(m_iNextGoalPos, m_iNextGoalVelocity);
1384 
1387 
1390  }
1391 }
1392 
1393 float RbDynamixelServo::QuantizeServoPosition(float fltPos)
1394 {
1395  int iPos = ConvertPosFloatToFP(fltPos);
1396  return ConvertPosFPToFloat(iPos);
1397 }
1398 
1399 float RbDynamixelServo::QuantizeServoVelocity(float fltVel)
1400 {
1401  int iPos = (int) (fabs(fltVel)*m_fltConvertPosSToFP);
1402  return iPos*m_fltConvertFPToPosS;
1403 }
1404 
1405 float *RbDynamixelServo::GetDataPointer(const std::string &strDataType)
1406 {
1407  std::string strType = Std_CheckString(strDataType);
1408 
1409  if(strType == "READPARAMTIME")
1410  return &m_fltReadParamTime;
1411  else if(strType == "IOPOS")
1412  return &m_fltIOPos;
1413  else if(strType == "IOVELOCITY")
1414  return &m_fltIOVelocity;
1415 
1416  return NULL;
1417 }
1418 
1419 bool RbDynamixelServo::SetData(const std::string &strDataType, const std::string &strValue)
1420 {
1421  std::string strType = Std_CheckString(strDataType);
1422 
1423  if(strType == "SERVOID")
1424  {
1425  ServoID((int) atoi(strValue.c_str()));
1426  return true;
1427  }
1428 
1429  if(strType == "QUERYMOTORDATA")
1430  {
1431  QueryMotorData(Std_ToBool(strValue));
1432  return true;
1433  }
1434 
1435  if(strType == "MINPOSFP")
1436  {
1437  MinPosFP((int) atoi(strValue.c_str()));
1438  return true;
1439  }
1440 
1441  if(strType == "MAXPOSFP")
1442  {
1443  MaxPosFP((int) atoi(strValue.c_str()));
1444  return true;
1445  }
1446 
1447  if(strType == "MINANGLE")
1448  {
1449  MinAngle((float) atof(strValue.c_str()));
1450  return true;
1451  }
1452 
1453  if(strType == "MAXANGLE")
1454  {
1455  MaxAngle((float) atof(strValue.c_str()));
1456  return true;
1457  }
1458 
1459  if(strType == "MINVELOCITYFP")
1460  {
1461  MinVelocityFP((int) atoi(strValue.c_str()));
1462  return true;
1463  }
1464 
1465  if(strType == "MAXVELOCITYFP")
1466  {
1467  MaxVelocityFP((int) atoi(strValue.c_str()));
1468  return true;
1469  }
1470 
1471  if(strType == "CWCOMPLIANCEMARGIN")
1472  {
1473  CWComplianceMargin((int) atoi(strValue.c_str()));
1474  return true;
1475  }
1476 
1477  if(strType == "CCWCOMPLIANCEMARGIN")
1478  {
1479  CCWComplianceMargin((int) atoi(strValue.c_str()));
1480  return true;
1481  }
1482 
1483  if(strType == "CWCOMPLIANCESLOPE")
1484  {
1485  CWComplianceSlope((int) atoi(strValue.c_str()));
1486  return true;
1487  }
1488 
1489  if(strType == "CCWCOMPLIANCESLOPE")
1490  {
1491  CCWComplianceSlope((int) atoi(strValue.c_str()));
1492  return true;
1493  }
1494 
1495  if(strType == "MAXTORQUE")
1496  {
1497  MaxTorque((int) atoi(strValue.c_str()));
1498  return true;
1499  }
1500 
1501  if(strType == "RPMPERFPUNIT")
1502  {
1503  RPMPerFPUnit((float) atof(strValue.c_str()));
1504  return true;
1505  }
1506 
1507  if(strType == "MINLOADFP")
1508  {
1509  MinLoadFP((int) atoi(strValue.c_str()));
1510  return true;
1511  }
1512 
1513  if(strType == "MAXLOADFP")
1514  {
1515  MaxLoadFP((int) atoi(strValue.c_str()));
1516  return true;
1517  }
1518 
1519  if(strType == "TRANSLATIONRANGE")
1520  {
1521  TranslationRange(atof(strValue.c_str()));
1522  return true;
1523  }
1524 
1525  if(strType == "RESETTOSTARTPOS")
1526  {
1527  ResetToStartPos(Std_ToBool(strValue));
1528  return true;
1529  }
1530 
1531  return false;
1532 }
1533 
1534 void RbDynamixelServo::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
1535 {
1536  aryProperties.Add(new TypeProperty("ReadParamTime", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
1537  aryProperties.Add(new TypeProperty("IOPos", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
1538  aryProperties.Add(new TypeProperty("IOVelocity", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
1539 
1540  aryProperties.Add(new TypeProperty("ServoID", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1541  aryProperties.Add(new TypeProperty("QueryMotorData", AnimatPropertyType::Boolean, AnimatPropertyDirection::Set));
1542 
1543  aryProperties.Add(new TypeProperty("MinPosFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1544  aryProperties.Add(new TypeProperty("MaxPosFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1545  aryProperties.Add(new TypeProperty("MinAngle", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
1546  aryProperties.Add(new TypeProperty("MaxAngle", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
1547  aryProperties.Add(new TypeProperty("MinVelocityFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1548  aryProperties.Add(new TypeProperty("MaxVelocityFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1549  aryProperties.Add(new TypeProperty("RPMPerFPUnit", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
1550  aryProperties.Add(new TypeProperty("MinLoadFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1551  aryProperties.Add(new TypeProperty("MaxLoadFP", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1552  aryProperties.Add(new TypeProperty("TranslationRange", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
1553  aryProperties.Add(new TypeProperty("ResetToStartPos", AnimatPropertyType::Boolean, AnimatPropertyDirection::Set));
1554 
1555  aryProperties.Add(new TypeProperty("CWComplianceMargin", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1556  aryProperties.Add(new TypeProperty("CCWComplianceMargin", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1557  aryProperties.Add(new TypeProperty("CWComplianceSlope", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1558  aryProperties.Add(new TypeProperty("CCWComplianceSlope", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1559  aryProperties.Add(new TypeProperty("MaxTorque", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
1560 }
1561 
1562 void RbDynamixelServo::ResetSimulation()
1563 {
1564  m_fltReadParamTime = 0;
1565  m_fltIOPos = 0;
1566  m_fltIOVelocity = 0;
1567  m_bNeedSetVelStopPos = false;
1568  m_bVelStopPosSet = false;
1569 }
1570 
1571 void RbDynamixelServo::StepSimulation()
1572 {
1574  //int i=5;
1575  //if(m_iServoID == 3 && GetSimulator()->Time() > 1) // &&
1576  // i=6;
1577  //if(!m_bIsHinge) // && m_lpSim->Time() > 1
1578  // i=6;
1579 
1580  if(m_lpMotorJoint)
1581  {
1582  //Here we need to get the set velocity for this motor that is coming from the neural controller, and then make the real motor go that speed.
1583  //Here we are setting the values that will be used the next time the IO is processed for this servo.
1584  if(m_lpMotorJoint->MotorType() == eJointMotorType::PositionVelocityControl)
1585  {
1586  float fltSetPosition = m_lpMotorJoint->SetPosition();
1587  float fltSetVelocity = m_lpMotorJoint->SetVelocity();
1588  SetNextGoalPosition(fltSetPosition);
1589  SetNextGoalVelocity(fltSetVelocity);
1590  }
1591  else if(m_lpMotorJoint->MotorType() == eJointMotorType::VelocityControl)
1592  {
1593  float fltSetVelocity = m_lpMotorJoint->SetVelocity();
1594  float fltPrevSetVel = m_lpMotorJoint->PrevSetVelocity();
1595  SetNextGoalVelocity(fltSetVelocity);
1596 
1597 
1598  if(fabs(fltSetVelocity) < 1e-4)
1599  {
1601  {
1602  m_bNeedSetVelStopPos = true;
1603 
1604  //if(m_iServoID == 3)
1605  //std::cout << "Time: " << GetSimulator()->TimeSlice() << " Servo: " << m_iServoID << " Pos: " << m_iPresentPos << "\r\n";
1606  }
1607  }
1608  else if(fltSetVelocity > 0)
1609  {
1611  m_bNeedSetVelStopPos = false;
1612  m_bVelStopPosSet = false;
1613  }
1614  else
1615  {
1617  m_bNeedSetVelStopPos = false;
1618  m_bVelStopPosSet = false;
1619  }
1620  }
1621  else
1622  {
1623  float fltSetPosition = m_lpMotorJoint->SetPosition();
1624  SetNextGoalPosition(fltSetPosition);
1626  }
1627 
1628  //only do this part if we are not in a simulation
1629  if(!GetSimulator()->InSimulation())
1630  {
1631  //Retrieve the values that we got from the last time the IO for this servo was read in.
1632  m_lpMotorJoint->JointPosition(m_fltPresentPos);
1633  m_lpMotorJoint->JointVelocity(m_fltPresentVelocity);
1634  m_lpMotorJoint->Temperature(m_fltVoltage);
1635  m_lpMotorJoint->Voltage(m_fltTemperature);
1636  m_lpMotorJoint->MotorTorqueToAMagnitude(m_fltLoad);
1637  m_lpMotorJoint->MotorTorqueToBMagnitude(m_fltLoad);
1638  }
1639  else
1640  {
1641  //If we are in a simulation then write out what the IO values should be
1644  }
1645  }
1646 }
1647 
1648 void RbDynamixelServo::GetLimitValues()
1649 {
1650  Hinge *lpHinge = dynamic_cast<Hinge *>(m_lpMotorJoint);
1651  if(lpHinge)
1652  {
1653  m_fltLowLimit = lpHinge->LowerLimit()->LimitPos();
1654  m_fltHiLimit = lpHinge->UpperLimit()->LimitPos();
1655  }
1656  else
1657  {
1658  Prismatic *lpPrismatic = dynamic_cast<Prismatic *>(m_lpMotorJoint);
1659  if(lpPrismatic)
1660  {
1661  m_fltLowLimit = lpPrismatic->LowerLimit()->LimitPos();
1662  m_fltHiLimit = lpPrismatic->UpperLimit()->LimitPos();
1663  }
1664  }
1665 }
1666 
1667 void RbDynamixelServo::Load(StdUtils::CStdXml &oXml)
1668 {
1669  oXml.IntoElem();
1670 
1671  QueryMotorData(oXml.GetChildBool("QueryMotorData", m_bQueryMotorData));
1672  MinPosFP(oXml.GetChildInt("MinPosFP", m_iMinPosFP));
1673  MaxPosFP(oXml.GetChildInt("MaxPosFP", m_iMaxPosFP));
1674  MinAngle(oXml.GetChildFloat("MinAngle", m_fltMinAngle));
1675  MaxAngle(oXml.GetChildFloat("MaxAngle", m_fltMaxAngle));
1676  MinVelocityFP(oXml.GetChildInt("MinVelocityFP", m_iMinVelocityFP));
1677  MaxVelocityFP(oXml.GetChildInt("MaxVelocityFP", m_iMaxVelocityFP));
1678  RPMPerFPUnit(oXml.GetChildFloat("RPMPerFPUnit", m_fltRPMPerFPUnit));
1679  MinLoadFP(oXml.GetChildInt("MinLoadFP", m_iMinLoadFP));
1680  MaxLoadFP(oXml.GetChildInt("MaxLoadFP", m_iMaxLoadFP));
1681  m_bIsHinge = oXml.GetChildBool("IsHinge", m_bIsHinge);
1682  TranslationRange(oXml.GetChildFloat("TranslationRange", m_fltTranslationRange));
1683  ResetToStartPos(oXml.GetChildBool("ResetToStartPos", m_bResetToStartPos));
1684 
1685  CWComplianceMargin(oXml.GetChildInt("CWComplianceMargin", m_iCWComplianceMargin));
1686  CCWComplianceMargin(oXml.GetChildInt("CCWComplianceMargin", m_iCCWComplianceMargin));
1687  CWComplianceSlope(oXml.GetChildInt("CWComplianceSlope", m_iCWComplianceSlope));
1688  CCWComplianceSlope(oXml.GetChildInt("CCWComplianceSlope", m_iCCWComplianceSlope));
1689  MaxTorque(oXml.GetChildInt("MaxTorque", m_iMaxTorque));
1690 
1691  oXml.OutOfElem();
1692 }
1693 
1694 
1695  } //RobotIOControls
1696  } // Robotics
1697 } //RoboticsAnimatSim
1698 
float m_fltPosFloatToFPSlope
The conversion factor to convert radians to FP position.
virtual float ConvertFPVelocity(int iVel)
Gets the floating point value of the servo actual velocity. The fixed point value is retrieved from t...
virtual float GetActualTemperatureFahrenheit()
Gets the floating point value of the servo actual temperature. The fixed point value is retrieved fro...
virtual void SetCWAngleLimit(float fltLimit)
Sets the limit for the CW limit of the servo using radians.
int m_iTotalAngle
Maximum angle the servo can move in fixed point number. m_iMaxPosFP - m_iMinPosFP.
int m_iCWComplianceSlope
Used to set the CW compliance slope of the servo.
int m_iMaxVelocityFP
Maximum value that can be set for the velocity.
int m_iMinSimPos
This is the minimum angle in fixed point that can be used for this servo as specified in the simulati...
virtual void SetCWAngleLimit_FP(int iVal)
Sets the limit for the CW limit of the servo using fixed point value.
float m_fltConvertFPToPosS
The conversion factor to convert FP velocity value to rad/s.
virtual bool GetIsAlarmShutdown()
Returns whether the alarm shutdown state is active.
virtual float GetGoalPosition()
Gets the floating point value of the servo goal position. The fixed point value is retrieved from the...
virtual float GetActualLoad()
Gets the floating point value of the servo actual torque. The fixed point value is retrieved from the...
int m_iMaxTorque
Used to set the maximum torque of the servo.
virtual int GetModelNumber()
Returns whether the model number of the servo.
bool m_bVelStopPosSet
Keeps track of whether we have already set velocity to stop when doing velocity control.
virtual void SetGoalPosition_FP(int iPos)
Sets the fixed point value of the servo goal position. This is the exact value that will be sent to t...
virtual float GetActualTemperatureCelcius()
Gets the floating point value of the servo actual temperature. The fixed point value is retrieved fro...
int m_iCCWComplianceMargin
Used to set the CCW compliance margin of the servo.
int m_iMaxLoadFP
Maximum value that can be set for the load.
virtual float GetActualVelocity()
Gets the floating point value of the servo actual velocity. The fixed point value is retrieved from t...
virtual int GetCCWAngleLimit_FP()
Gets the limit for the CCW limit of the servo in fixed point value.
virtual void SetReturnDelayTime_FP(int iVal)
Sets the return delay time of the servo.
float m_fltMaxAngle
Maximum value that can be set for the position in floating point number.
virtual bool IntoElem()
Goes into the next element where the cursor is located.
Definition: StdXml.cpp:42
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 int GetActualLoad_FP()
Gets the fixed point value of the servo actual load. This is the exact value returned from the servo...
float m_fltConvertPosSToFP
The conversion factor to convert rad/s to FP velocity.
virtual int GetActualVelocity_FP()
Gets the fixed point value of the servo actual velocity. This is the exact value returned from the se...
virtual float ConvertFPLoad(int iLoad)
Gets the floating point value of the servo load. The fixed point value is retrieved from the servo an...
float m_fltMinAngle
Minimum value that can be set for the position in floating point number.
int m_iLastGoalVelocity
Keeps track of the last servo goal velocity that we set.
virtual int GetReturnDelayTime_FP()
Gets the return delay time of the servo.
virtual void Stop()
Attempts to stop the motor at its current position.
int m_iServoID
ID used for communications with this servo.
int m_iMinLoadFP
Minimum value that can be set for the load.
virtual float TranslationRange()
Gets the the total range of translation for a prismatic joint (meters).
virtual int GetMinSimPos_FP()
Gets the minimum position in fixed point that the simulation will allow for the joint.
bool Std_InValidRange(int iMinVal, int iMaxVal, int iVal, bool bThrowError, std::string strParamName)
Tests whether a number is within a valid range.
virtual float GetMaxSimPos()
Gets the maximum position in radians that the simulation will allow for the joint.
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.
float m_fltFloatToFPTranslation
Conversion factor for when we are doing prismatic joints. Converts rotations to linear movements...
virtual int GetChildInt(std::string strElementName)
Gets an integer value from the element with the specified name.
Definition: StdXml.cpp:456
int m_iMinPosFP
Minimum value that can be set for the position in fixed point number.
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.
virtual int LastGoalVelocity_FP()
Gets the last goal velocity that was sent to the servo. This is used so we can see the last value we ...
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.
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.
virtual int GetIDNumber()
Returns whether the servo ID number.
Declares the vortex hinge class.
virtual int GetGoalVelocity_FP()
Gets the fixed point value of the servo velocity. This is the exact value returned from the servo...
virtual float GetActualPosition()
Gets the floating point value of the servo actual position. The fixed point value is retrieved from t...
virtual void Move(float fltPos, float fltVel)
Moves the motor to the specified position at the given speed.
float m_fltPosFloatToFPIntercept
The conversion factor to convert radians to FP position.
virtual void SetGoalPosition(float fltPos)
Sets the floating point value of the servo goal position. This is the position in radians...
A standard xml manipulation class.
Definition: StdXml.h:19
virtual int GetCWAngleLimit_FP()
Gets the limit for the CW limit of the servo in fixed point value.
float m_fltMaxPosSec
Stores the maximum rad/sec for this motor.
int m_iMinVelocityFP
Minimum value that can be set for the velocity.
int m_iLoad
The current load that was last read in for this servo.
virtual float GetCWAngleLimit()
Gets the limit for the CW limit of the servo in radian values.
float m_fltConvertFPToLoad
Used to conver the load fixed point value back to a percentage of load.
float m_fltMaxSimPos
This is the maximum angle in radians that can be used for this servo as specified in the simulation...
virtual int GetActualVoltage_FP()
Gets the fixed point value of the servo actual voltage. This is the exact value returned from the ser...
virtual void SetNextMaximumVelocity()
Sets the servo goal speed to 0 for maximum velocity the next time the IO for this servo is processed...
virtual void SetCCWAngleLimit(float fltLimit)
Sets the limit for the CCW limit of the servo using radians.
float m_fltPresentVelocity
The current velocity that was last read in for this servo.
MotorizedJoint * m_lpMotorJoint
Pointer to an associated motorized joint.
virtual int GetMaxSimPos_FP()
Gets the maximum position in fixed point that the simulation will allow for the joint.
float m_fltFPToFloatTranslation
Conversion factor for when we are doing prismatic joints. Converts rotations to linear movements...
bool Std_ToBool(int iVal)
Converts a value toa bool.
bool Std_IsBelowMax(int iMaxVal, int iVal, bool bThrowError, std::string strParamName, bool bInclusiveLimit)
Tests if a number is below a maximum value.
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 bool GetChildBool(std::string strElementName)
Gets a bool value from the element with the specified name.
Definition: StdXml.cpp:699
virtual void GetIsLEDOn(bool &bIsBlueOn, bool &bIsGreenOn, bool &bIsRedOn)
Returns whether the servo blue led is currently on.
float m_fltTranslationRange
The total translation range over which a prismatic joint can move. This does not apply to hinge joint...
float m_fltRPMPerFPUnit
Stores the maximum rot/min for this motor. The max rotations (rad) per fixed point unit...
virtual void SetMaxSimPos(float fltVal)
Sets the maximum position that the simulation will allow for this joint in radians.
virtual int GetActualPosition_FP()
Gets the fixed point value of the servo actual position. This is the exact value returned from the se...
virtual void SetMinSimPos(float fltVal)
Sets the minimum position that the simulation will allow for this joint in radians.
virtual void SetTorqueLimit_FP(int iVal)
Sets the torque limit of the servo using fixed point value.
virtual float GetCCWAngleLimit()
Gets the limit for the CCW limit of the servo in radian values.
int m_iCCWComplianceSlope
Used to set the CCW compliance slope of the servo.
float m_fltTotalAngle
Maximum angle the servo can move in floating point number. m_iMaxPosFP - m_iMinPosFP.
bool m_bResetToStartPos
If true then we should reset to the start position at the beginning of the simulation.
virtual bool OutOfElem()
Goes out of the element where the cursor is located.
Definition: StdXml.cpp:56
virtual float GetGoalVelocity()
Gets the floating point value of the servo velocity. The fixed point value is retrieved from the serv...
virtual int GetGoalPosition_FP()
Gets the fixed point value of the servo goal position. This is the exact value returned from the serv...
float m_fltIOPos
Used to report back the IO position of the servo at each step.
int m_iMaxPosFP
Maximum value that can be set for the position in fixed point number.
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 SetGoalVelocity_FP(int iVelocity)
Sets the fixed point value of the servo velocity. This is the exact value that will be sent to the se...
virtual void SetCCWAngleLimit_FP(int iVal)
Sets the limit for the CCW limit of the servo using fixed point value.
virtual void SetMaximumVelocity()
Sets the servo goal speed to 0 for maximum velocity.
virtual void InitMotorData()
Initializes the internal data on position and velocity from the actual motor.
virtual void SetNextGoalVelocity_FP(int iVelocity)
Sets the fixed point value of the servo velocity for the next time the IO for this servo is processed...
virtual void SetGoalVelocity(float fltVelocity)
Sets the floating point value of the servo velocity. This is the velocity in rad/s. It will be converted to a fixed point value based on the motor configuration and then used to set the velocity.
virtual int GetFirmwareVersion()
Returns whether the servo firmware version number.
virtual void SetNextGoalVelocity(float fltVelocity)
Sets the floating point value of the servo velocity the next time the IO for this servo is processed...
float m_fltPosFPToFloatIntercept
The conversion factor to convert FP position to floating point position..
float m_fltMinSimPos
This is the minimum angle in radians that can be used for this servo as specified in the simulation...
virtual float GetActualVoltage()
Gets the floating point value of the servo actual voltage. The fixed point value is retrieved from th...
virtual bool GetIsMoving()
Returns whether the servo is currently moving or has already reached its goal state.
virtual float GetMinSimPos()
Gets the minimum position in radians that the simulation will allow for the joint.
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 SetNextGoalPosition(float fltPos)
Sets the floating point value of the servo goal position that will be used the next time the iO for t...
virtual int GetTorqueLimit_FP()
Gets the torque limit of the servo in fixed point value.
float m_fltIOVelocity
Used to report back the IO velocity of the servo at each step.
int m_iPresentVelocity
The current velocity that was last read in for this servo.
int m_iCenterPosFP
The center point value in fixed point numbers.
virtual int LastGoalPosition_FP()
Gets the last goal position that was sent to the servo. This is used so we can see the last value we ...
virtual void ConfigureServo()
Checks the current limits on the motor and sets them according to the simulation params.
float m_fltPosFPToFloatSlope
The conversion factor to convert FP position to floating point position.
virtual void SetNextGoalPosition_FP(int iPos)
Sets the fixed point value of the servo goal position that will be used the next time the IO for this...
virtual float GetChildFloat(std::string strElementName)
Gets a float value from the element with the specified name.
Definition: StdXml.cpp:617