AnimatLab  2
Test
RbDynamixelUSB.cpp
1 // RbDynamixelUSB.cpp: implementation of the RbDynamixelUSB 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 "RbDynamixelUSB.h"
16 #include "RbDynamixelUSBServo.h"
17 
18 namespace RoboticsAnimatSim
19 {
20  namespace Robotics
21  {
22  namespace RobotIOControls
23  {
24  namespace DynamixelUSB
25  {
26 
28 // Construction/Destruction
30 
31 RbDynamixelUSB::RbDynamixelUSB()
32 {
33  m_iPortNumber = 3;
34  m_iBaudRate = 1; //Max
35 }
36 
37 RbDynamixelUSB::~RbDynamixelUSB()
38 {
39  try
40  {
41  }
42  catch(...)
43  {Std_TraceMsg(0, "Caught Error in desctructor of RbDynamixelUSB\r\n", "", -1, false, true);}
44 }
45 
46 void RbDynamixelUSB::PortNumber(int iPort)
47 {
48  Std_IsAboveMin((int) 0, iPort, true, "PortNumber", true);
49  m_iPortNumber = iPort;
50 }
51 
52 int RbDynamixelUSB::PortNumber() {return m_iPortNumber;}
53 
54 void RbDynamixelUSB::BaudRate(int iRate)
55 {
56  if( !((iRate == 1) || (iRate == 3) || (iRate == 4) || (iRate == 7) || (iRate == 9) ||
57  (iRate == 9) || (iRate == 16) || (iRate == 34) || (iRate == 103) || (iRate == 207)) )
58  THROW_PARAM_ERROR(Rb_Err_lInvalidBaudRate, Rb_Err_strInvalidBaudRate, "Baud rate", iRate);
59  m_iBaudRate = iRate;
60 }
61 
62 int RbDynamixelUSB::BaudRate() {return m_iBaudRate;}
63 
64 void RbDynamixelUSB::SetRegister(unsigned char iServo, unsigned char reg, unsigned char length, unsigned int value)
65 {
66  if(length == 2)
67  dxl_write_word(iServo, reg, value);
68  else
69  dxl_write_byte(iServo, reg, value);
70 }
71 
72 int RbDynamixelUSB::GetRegister(unsigned char iServo, unsigned char reg, unsigned char length)
73 {
74  if(length == 2)
75  return dxl_read_word(iServo, reg);
76  else
77  return dxl_read_byte(iServo, reg);
78 }
79 
80 #pragma region DataAccesMethods
81 
82 float *RbDynamixelUSB::GetDataPointer(const std::string &strDataType)
83 {
84  std::string strType = Std_CheckString(strDataType);
85 
86  return RobotIOControl::GetDataPointer(strDataType);
87 }
88 
89 bool RbDynamixelUSB::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
90 {
91  std::string strType = Std_CheckString(strDataType);
92 
93  if(RobotIOControl::SetData(strDataType, strValue, false))
94  return true;
95 
96  if(strType == "PORTNUMBER")
97  {
98  PortNumber((int) atoi(strValue.c_str()));
99  return true;
100  }
101  else if(strType == "BAUDRATE")
102  {
103  BaudRate((int) atoi(strValue.c_str()));
104  return true;
105  }
106 
107  //If it was not one of those above then we have a problem.
108  if(bThrowError)
109  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
110 
111  return false;
112 }
113 
114 void RbDynamixelUSB::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
115 {
116  RobotIOControl::QueryProperties(aryProperties);
117 
118  aryProperties.Add(new TypeProperty("ComPort", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
119  aryProperties.Add(new TypeProperty("BaudRate", AnimatPropertyType::Integer, AnimatPropertyDirection::Set));
120 }
121 
122 #pragma endregion
123 
124 bool RbDynamixelUSB::OpenIO()
125 {
126  if(!dxl_initialize(m_iPortNumber, m_iBaudRate))
127  THROW_PARAM_ERROR(Rb_Err_lFailedDynamixelConnection, Rb_Err_strFailedDynamixelConnection, "Port", m_iPortNumber);
128 
129  return true;
130 }
131 
132 void RbDynamixelUSB::CloseIO()
133 {
134  TRACE_DEBUG("CloseIO.");
135 
136  if(!m_lpSim->InSimulation())
137  dxl_terminate();
138 }
139 
140 void RbDynamixelUSB::ProcessIO()
141 {
142  try
143  {
144  m_bIOThreadProcessing = true;
145 
146  SetupIO();
147 
148  m_bSetupComplete = true;
149  m_WaitForIOSetupCond.notify_all();
150 
151  while(!m_bStopIO)
152  {
153  if(m_bPauseIO || m_lpSim->Paused())
154  {
155  m_bIOPaused = true;
156  boost::this_thread::sleep(boost::posix_time::microseconds(1000));
157  }
158  else
159  {
160  m_bIOPaused = false;
161 
162  m_aryMotorData.RemoveAll();
163  StepIO();
164  SendSynchronousMoveCommand();
165  }
166 
167 #ifndef Win32
168  //Not needed in windows, not sure in linux. Keep it in till verify.
169  m_lpSim->MicroSleep(15000);
170 #endif
171  }
172  }
173  catch(CStdErrorInfo oError)
174  {
175  m_bIOThreadProcessing = false;
176  }
177  catch(...)
178  {
179  m_bIOThreadProcessing = false;
180  }
181 
182  m_bIOThreadProcessing = false;
183 }
184 
185 bool RbDynamixelUSB::SendSynchronousMoveCommand()
186 {
187  int iServos = m_aryMotorData.GetSize();
188  int iDataPerServo = 4;
189 
190  if(iServos > 0)
191  {
192  dxl_set_txpacket_id(BROADCAST_ID);
193  dxl_set_txpacket_instruction(INST_SYNC_WRITE);
194  dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
195  dxl_set_txpacket_parameter(1, iDataPerServo);
196 
197  for(int iServo=0; iServo<iServos; iServo++ )
198  {
199  RbDynamixelMotorUpdateData *lpServo = m_aryMotorData[iServo];
200 
201  int iOffset = (2+(iDataPerServo+1)*iServo);
202  dxl_set_txpacket_parameter((iOffset+0), lpServo->m_iID);
203  dxl_set_txpacket_parameter((iOffset+1), dxl_get_lowbyte(lpServo->m_iGoalPos));
204  dxl_set_txpacket_parameter((iOffset+2), dxl_get_highbyte(lpServo->m_iGoalPos));
205  dxl_set_txpacket_parameter((iOffset+3), dxl_get_lowbyte(lpServo->m_iGoalVelocity));
206  dxl_set_txpacket_parameter((iOffset+4), dxl_get_highbyte(lpServo->m_iGoalVelocity));
207  }
208 
209  dxl_set_txpacket_length((iDataPerServo+1)*iServos+4);
210 
211  dxl_txrx_packet();
212  int CommStatus = dxl_get_result();
213  if( CommStatus == COMM_RXSUCCESS )
214  return true;
215  else
216  {
217  std::cout << GetCommStatus(CommStatus);
218  }
219  }
220 
221  return false;
222 }
223 
233 {
234  if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
235  return "Input voltage error!";
236 
237  if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
238  return "Angle limit error!\n";
239 
240  if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
241  return "Overheat error!\n";
242 
243  if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
244  return "Out of range error!\n";
245 
246  if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
247  return "Checksum error!\n";
248 
249  if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
250  return "Overload error!\n";
251 
252  if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
253  return "Instruction code error!\n";
254 
255  return "Unknown error";
256 }
257 
258 std::string RbDynamixelUSB::GetCommStatus(int CommStatus)
259 {
260  switch(CommStatus)
261  {
262  case COMM_TXFAIL:
263  return "COMM_TXFAIL: Failed transmit instruction packet!\r\n";
264  break;
265 
266  case COMM_TXERROR:
267  return "COMM_TXERROR: Incorrect instruction packet!\r\n";
268  break;
269 
270  case COMM_RXFAIL:
271  return "COMM_RXFAIL: Failed get status packet from device!\r\n";
272  break;
273 
274  case COMM_RXWAITING:
275  return "COMM_RXWAITING: Now recieving status packet!\r\n";
276  break;
277 
278  case COMM_RXTIMEOUT:
279  return "COMM_RXTIMEOUT: There is no status packet!\r\n";
280  break;
281 
282  case COMM_RXCORRUPT:
283  return "COMM_RXCORRUPT: Incorrect status packet!\r\n";
284  break;
285 
286  default:
287  return "This is unknown error code!\r\n";
288  break;
289  }
290 }
291 
293 {
294  RobotIOControl::Load(oXml);
295 
296  oXml.IntoElem();
297  PortNumber(oXml.GetChildInt("PortNumber", m_iPortNumber));
298  BaudRate(oXml.GetChildInt("BaudRate", m_iBaudRate));
299  oXml.OutOfElem();
300 }
301 
302 
303  } //DynamixelUSB
304  } //RobotIOControls
305  } // Robotics
306 } //RoboticsAnimatSim
307 
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 InSimulation()
Used to determine if we are running in a simulation, or in a real control mode.
Definition: Simulator.cpp:1673
virtual std::string GetErrorCode()
Checks the error code and returns an associated error message.
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
virtual bool Paused()
Gets whether the Simulation is paused.
Definition: Simulator.cpp:347
virtual bool IntoElem()
Goes into the next element where the cursor is located.
Definition: StdXml.cpp:42
boost::interprocess::interprocess_condition m_WaitForIOSetupCond
Condition used to determine when the IO is setup.
bool m_bStopIO
Flags the thread processing loop to exit.
virtual void Load(StdUtils::CStdXml &oXml)
Loads the item using an XML data packet.
virtual int GetChildInt(std::string strElementName)
Gets an integer value from the element with the specified name.
Definition: StdXml.cpp:456
virtual void StepIO()
This method is called from within the IO thread. It calls StepIO for each part.
bool Std_IsAboveMin(int iMinVal, int iVal, bool bThrowError, std::string strParamName, bool bInclusiveLimit)
Tests if a number is above a minimum value.
Declares the vortex structure class.
Declares the vortex hinge class.
A standard xml manipulation class.
Definition: StdXml.h:19
virtual float * GetDataPointer(const std::string &strDataType)
Returns a float pointer to a data item of interest in this object.
bool m_bIOPaused
Is set to true once the IO loop is paused.
void Std_TraceMsg(const int iLevel, std::string strMessage, std::string strSourceFile, int iSourceLine, bool bLogToFile, bool bPrintHeader)
Traces a message to the debugger window.
bool m_bPauseIO
Set to true to pause the IO processing. Set back to false to resume it.
virtual bool OutOfElem()
Goes out of the element where the cursor is located.
Definition: StdXml.cpp:56
virtual void SetupIO()
This method is called after all connections to whatever control board have been made. It calls each parts SetupIO method. 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.
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
bool m_bIOThreadProcessing
True while the io thread processing loop is going on.
bool m_bSetupComplete
Set to true once the IO is setup correctly.
Classes for implementing the cm-labs vortex physics engine for AnimatLab.