00001
00005
00006
00007 #include "dynamixelUse.h"
00008
00009
00011
00012
00013
00014 Dynamixel::Dynamixel()
00015 {
00016 TimeOut=10000000;
00017 InitDynamixel();
00018 }
00019
00020
00021 Dynamixel::~Dynamixel()
00022 {
00023 Serial3.end();
00024
00025 #ifdef __DEBUG__DYNAMIXEL_USE__
00026 Serial.println("Communication with servomotors close");
00027 #endif
00028 }
00029
00030
00031 bool Dynamixel::SendPing(unsigned char ID)
00032 {
00033 #ifdef __DEBUG__DYNAMIXEL_USE__
00034 Serial.print("Ping...");
00035 #endif
00036
00037 unsigned char Buffer[6];
00038 SendPacket(ID,PING,0,0);
00039 ReceivePacket(Buffer,6,TimeOut);
00040
00041 if(Errors(Buffer))
00042 return 0;
00043 else
00044 return 1;
00045 }
00046
00047
00048 bool Dynamixel::ManSend(unsigned char ID,unsigned char Instruction,unsigned char *Parameters,int NbParam,int LengthToRead)
00049 {
00050 #ifdef __DEBUG__DYNAMIXEL_USE__
00051 Serial.print("Sending packet manually...");
00052 #endif
00053
00054 unsigned char Buffer[LengthToRead+1];
00055 SendPacket(ID,Instruction,Parameters,NbParam);
00056 ReceivePacket(Buffer,LengthToRead,TimeOut);
00057 if(Errors(Buffer))
00058 return 0;
00059 else
00060 return 1;
00061 }
00062
00063
00064 int Dynamixel::CheckPacket(unsigned char *Packet)
00065 {
00066 return (Errors(Packet));
00067 }
00068
00069
00070 void Dynamixel::ClearReceptionBuffer()
00071 {
00072 EmptyReceptionBuffer();
00073 }
00074
00075
00076
00077 bool Dynamixel::LeaveAlarmShutdownMode(unsigned char ID)
00078 {
00079 #ifdef __DEBUG__DYNAMIXEL_USE__
00080 Serial.println("Leaving Alarm Shutdown mode...");
00081 #endif
00082 SetTorqueLimit(ID,0);
00083 delay(1000);
00084
00085 Serial.println("torque limit 0");
00086 SetTorqueLimit(ID,1023);
00087 return 1;
00088
00089 return 0;
00090 }
00091
00093
00094
00095
00096 bool Dynamixel::SetGoalPosition(unsigned char ID,unsigned short Angle)
00097 {
00098 #ifdef __DEBUG__DYNAMIXEL_USE__
00099 Serial.print("Setting goal position...");
00100 #endif
00101
00102 unsigned char Buffer[7];
00103 unsigned char PacketGoalPos[3]={0x1E,Angle,Angle>>8};
00104 SendPacket(ID,WRITE,PacketGoalPos,3);
00105 ReceivePacket(Buffer,6,TimeOut);
00106
00107 if(Errors(Buffer))
00108 return 0;
00109 else
00110 return 1;
00111 }
00112
00113
00114 bool Dynamixel::SetCWAngleLimit(unsigned char ID,unsigned short Angle)
00115 {
00116 #ifdef __DEBUG__DYNAMIXEL_USE__
00117 Serial.println("Setting CW angle limit...");
00118 #endif
00119
00120 unsigned char Buffer[7];
00121 unsigned char PacketAngleLimit[3] ={0x06,Angle,Angle>>8};
00122 SendPacket(ID,WRITE,PacketAngleLimit,3);
00123 ReceivePacket(Buffer,6,TimeOut);
00124
00125 if(Errors(Buffer))
00126 return 0;
00127 else
00128 return 1;
00129 }
00130
00131
00132 bool Dynamixel::SetCCWAngleLimit(unsigned char ID,unsigned short Angle)
00133 {
00134 #ifdef __DEBUG__DYNAMIXEL_USE__
00135 Serial.println("Setting CCW angle limit...");
00136 #endif
00137
00138 unsigned char Buffer[7];
00139 unsigned char PacketAngleLimit[3] ={0x08,Angle,Angle>>8};
00140 SendPacket(ID,WRITE,PacketAngleLimit,3);
00141 ReceivePacket(Buffer,6,TimeOut);
00142
00143 if(Errors(Buffer))
00144 return 0;
00145 else
00146 return 1;
00147 }
00148
00149
00150 bool Dynamixel::SetMovSpeed(unsigned char ID,unsigned short Speed)
00151 {
00152 #ifdef __DEBUG__DYNAMIXEL_USE__
00153 Serial.println("Setting moving speed...");
00154 #endif
00155
00156 unsigned char PacketMovSpeed[3]={0x20,Speed,Speed>>8};
00157 unsigned char Buffer[7];
00158 SendPacket(ID,WRITE,PacketMovSpeed,3);
00159 ReceivePacket(Buffer,6,TimeOut);
00160 if(!Errors(Buffer))return true;
00161 else return false;
00162 }
00163
00164
00165 bool Dynamixel::SetReturnDelayTime(unsigned char ID, unsigned char Delay)
00166 {
00167 #ifdef __DEBUG__DYNAMIXEL_USE__
00168 Serial.println("Setting return delay time...");
00169 #endif
00170
00171 unsigned char PacketReturnDelayTime[2]={0x05,Delay};
00172 unsigned char Buffer[7];
00173 SendPacket(ID,WRITE,PacketReturnDelayTime,2);
00174 ReceivePacket(Buffer,6,TimeOut);
00175 if(!Errors(Buffer))return true;
00176 else return false;
00177 }
00178
00179
00180 bool Dynamixel::SetMaxTorque(unsigned char ID, unsigned short Torque)
00181 {
00182 #ifdef __DEBUG__DYNAMIXEL_USE__
00183 Serial.println("Setting max torque...");
00184 #endif
00185
00186 unsigned char PacketMaxTorque[3]={0x0E,Torque,Torque>>8};
00187 unsigned char Buffer[7];
00188 SendPacket(ID,WRITE,PacketMaxTorque,3);
00189 ReceivePacket(Buffer,6,TimeOut);
00190 if(!Errors(Buffer))return true;
00191 else return false;
00192 }
00193
00194
00195 bool Dynamixel::SetTorqueEnable(unsigned char ID, bool Enable)
00196 {
00197 #ifdef __DEBUG__DYNAMIXEL_USE__
00198 Serial.println("Setting torque enable...");
00199 #endif
00200
00201 unsigned char PacketTorqueEnable[2]={0x18,Enable};
00202 unsigned char Buffer[7];
00203 SendPacket(ID,WRITE,PacketTorqueEnable,2);
00204 ReceivePacket(Buffer,6,TimeOut);
00205 if(!Errors(Buffer))return true;
00206 else return false;
00207 }
00208
00209
00210 bool Dynamixel::SetCWComplianceMargin(unsigned char ID, unsigned char Margin)
00211 {
00212 #ifdef __DEBUG__DYNAMIXEL_USE__
00213 Serial.println("Setting CW compliance margin...");
00214 #endif
00215
00216 unsigned char PacketComplianceMargin[2]={0x1A,Margin};
00217 unsigned char Buffer[7];
00218 SendPacket(ID,WRITE,PacketComplianceMargin,2);
00219 ReceivePacket(Buffer,6,TimeOut);
00220 if(!Errors(Buffer))return true;
00221 else return false;
00222 }
00223
00224
00225 bool Dynamixel::SetCCWComplianceMargin(unsigned char ID, unsigned char Margin)
00226 {
00227 #ifdef __DEBUG__DYNAMIXEL_USE__
00228 Serial.println("Setting CCW compliance margin...");
00229 #endif
00230
00231 unsigned char PacketComplianceMargin[2]={0x1B,Margin};
00232 unsigned char Buffer[7];
00233 SendPacket(ID,WRITE,PacketComplianceMargin,2);
00234 ReceivePacket(Buffer,6,TimeOut);
00235 if(!Errors(Buffer))return true;
00236 else return false;
00237 }
00238
00239
00240 bool Dynamixel::SetCWComplianceSlope(unsigned char ID, unsigned char Slope)
00241 {
00242 #ifdef __DEBUG__DYNAMIXEL_USE__
00243 Serial.println("Setting CW compliance slope...");
00244 #endif
00245
00246 unsigned char PacketComplianceSlope[2]={0x1C,Slope};
00247 unsigned char Buffer[7];
00248 SendPacket(ID,WRITE,PacketComplianceSlope,2);
00249 ReceivePacket(Buffer,6,TimeOut);
00250 if(!Errors(Buffer))return true;
00251 else return false;
00252 }
00253
00254
00255 bool Dynamixel::SetCCWComplianceSlope(unsigned char ID, unsigned char Slope)
00256 {
00257 #ifdef __DEBUG__DYNAMIXEL_USE__
00258 Serial.println("Setting CCW compliance slope...");
00259 #endif
00260
00261 unsigned char PacketComplianceSlope[2]={0x1D,Slope};
00262 unsigned char Buffer[7];
00263 SendPacket(ID,WRITE,PacketComplianceSlope,2);
00264 ReceivePacket(Buffer,6,TimeOut);
00265 if(!Errors(Buffer))return true;
00266 else return false;
00267 }
00268
00269
00270 bool Dynamixel::SetTorqueLimit(unsigned char ID, unsigned short Torque)
00271 {
00272 #ifdef __DEBUG__DYNAMIXEL_USE__
00273 Serial.println("Setting torque limit...");
00274 #endif
00275
00276 unsigned char PacketTorqueLimit[3]={0x22,Torque,Torque>>8};
00277 unsigned char Buffer[7];
00278 SendPacket(ID,WRITE,PacketTorqueLimit,3);
00279 ReceivePacket(Buffer,6,TimeOut);
00280 if(!Errors(Buffer))return true;
00281 else return false;
00282 }
00283
00284
00286
00287
00288
00289 short Dynamixel::GetCWAngleLimit(unsigned char ID)
00290 {
00291 #ifdef __DEBUG__DYNAMIXEL_USE__
00292 Serial.println("Getting CW angle limit...");
00293 #endif
00294
00295 unsigned char Buffer[9];
00296 unsigned char PacketAngleLimit[2]={0x06,2};
00297 SendPacket(ID,READ,PacketAngleLimit,2);
00298 ReceivePacket(Buffer,8,TimeOut);
00299 if(Errors(Buffer))
00300 return (Buffer[5]+(Buffer[6]<<8));
00301 return(-1);
00302
00303 }
00304
00305
00306 short Dynamixel::GetCCWAngleLimit(unsigned char ID)
00307 {
00308 #ifdef __DEBUG__DYNAMIXEL_USE__
00309 Serial.println("Getting CCW angle limit...");
00310 #endif
00311
00312 unsigned char Buffer[9];
00313 unsigned char PacketAngleLimit[2]={0x08,2};
00314 SendPacket(ID,READ,PacketAngleLimit,2);
00315 ReceivePacket(Buffer,8,TimeOut);
00316 if(Errors(Buffer))
00317 return (Buffer[5]+(Buffer[6]<<8));
00318 return -1;
00319 }
00320
00321
00322 short Dynamixel::GetMovSpeed(unsigned char ID)
00323 {
00324 #ifdef __DEBUG__DYNAMIXEL_USE__
00325 Serial.println("Getting moving speed...");
00326 #endif
00327
00328 unsigned char Buffer[9];
00329 unsigned char PacketMovSpeed[2]={0x20,2};
00330 SendPacket(ID,READ,PacketMovSpeed,2);
00331 ReceivePacket(Buffer,8,TimeOut);
00332 if(Errors(Buffer))
00333 return (Buffer[5]+(Buffer[6]<<8));
00334 return -1;
00335 }
00336
00337
00338 short Dynamixel::GetReturnDelayTime(unsigned char ID)
00339 {
00340 #ifdef __DEBUG__DYNAMIXEL_USE__
00341 Serial.println("Getting return delay time...");
00342 #endif
00343
00344 unsigned char Buffer[8];
00345 unsigned char PacketReturnDelayTime[2]={0x05,1};
00346 SendPacket(ID,READ,PacketReturnDelayTime,2);
00347 ReceivePacket(Buffer,7,TimeOut);
00348 if(Errors(Buffer))
00349 return (Buffer[5]);
00350 return -1;
00351
00352 }
00353
00354
00355 short Dynamixel::GetMaxTorque(unsigned char ID)
00356 {
00357 #ifdef __DEBUG__DYNAMIXEL_USE__
00358 Serial.println("Getting max torque...");
00359 #endif
00360
00361 unsigned char Buffer[9];
00362 unsigned char PacketMaxTorque[2]={0x0E,2};
00363 SendPacket(ID,READ,PacketMaxTorque,2);
00364 ReceivePacket(Buffer,8,TimeOut);
00365 if(Errors(Buffer))
00366 return (Buffer[5]+(Buffer[6]<<8));
00367 return -1;
00368
00369 }
00370
00371
00372 char Dynamixel::GetTorqueEnable(unsigned char ID)
00373 {
00374 #ifdef __DEBUG__DYNAMIXEL_USE__
00375 Serial.println("Getting torque enable...");
00376 #endif
00377
00378 unsigned char Buffer[8];
00379 unsigned char PacketTorqueEnable[2]={0x18,1};
00380 SendPacket(ID,READ,PacketTorqueEnable,2);
00381 ReceivePacket(Buffer,7,TimeOut);
00382 if(Errors(Buffer))
00383 return (Buffer[5]);
00384 return -1;
00385 }
00386
00387
00388 short Dynamixel::GetCWComplianceMargin(unsigned char ID)
00389 {
00390 #ifdef __DEBUG__DYNAMIXEL_USE__
00391 Serial.println("Getting CW compliance margin...");
00392 #endif
00393
00394 unsigned char Buffer[8];
00395 unsigned char PacketComplianceMargin[2]={0x1A,1};
00396 SendPacket(ID,READ,PacketComplianceMargin,2);
00397 ReceivePacket(Buffer,7,TimeOut);
00398 if(Errors(Buffer))
00399 return (Buffer[5]);
00400 return -1;
00401
00402 }
00403
00404
00405 short Dynamixel::GetCCWComplianceMargin(unsigned char ID)
00406 {
00407 #ifdef __DEBUG__DYNAMIXEL_USE__
00408 Serial.println("Getting CCW compliance margin...");
00409 #endif
00410
00411 unsigned char Buffer[8];
00412 unsigned char PacketComplianceMargin[2]={0x1B,1};
00413 SendPacket(ID,READ,PacketComplianceMargin,2);
00414 ReceivePacket(Buffer,7,TimeOut);
00415 if(Errors(Buffer))
00416 return (Buffer[5]);
00417 return -1;
00418 }
00419
00420
00421 short Dynamixel::GetCWComplianceSlope(unsigned char ID)
00422 {
00423 #ifdef __DEBUG__DYNAMIXEL_USE__
00424 Serial.println("Getting CW compliance slope...");
00425 #endif
00426
00427 unsigned char Buffer[8];
00428 unsigned char PacketComplianceSlope[2]={0x1C,1};
00429 SendPacket(ID,READ,PacketComplianceSlope,2);
00430 ReceivePacket(Buffer,7,TimeOut);
00431 if(Errors(Buffer))
00432 return (Buffer[5]);
00433 return -1;
00434 }
00435
00436
00437 short Dynamixel::GetCCWComplianceSlope(unsigned char ID)
00438 {
00439 #ifdef __DEBUG__DYNAMIXEL_USE__
00440 Serial.println("Getting CCW compliance slope...");
00441 #endif
00442
00443 unsigned char Buffer[8];
00444 unsigned char PacketComplianceSlope[2]={0x1D,1};
00445 SendPacket(ID,READ,PacketComplianceSlope,2);
00446 ReceivePacket(Buffer,7,TimeOut);
00447 if(Errors(Buffer))
00448 return (Buffer[5]);
00449 return -1;
00450 }
00451
00452
00453 short Dynamixel::GetTorqueLimit(unsigned char ID)
00454 {
00455 #ifdef __DEBUG__DYNAMIXEL_USE__
00456 Serial.println("Getting torque limit...");
00457 #endif
00458
00459 unsigned char Buffer[9];
00460 unsigned char PacketTorqueLimit[2]={0x22,2};
00461 SendPacket(ID,READ,PacketTorqueLimit,2);
00462 ReceivePacket(Buffer,8,TimeOut);
00463 if(Errors(Buffer))
00464 return (Buffer[5]+(Buffer[6]<<8));
00465 return -1;
00466 }
00467
00468
00469 short Dynamixel::GetPresentPosition(unsigned char ID)
00470 {
00471 #ifdef __DEBUG__DYNAMIXEL_USE__
00472 Serial.println("Getting present position...");
00473 #endif
00474
00475 unsigned char Buffer[9];
00476 unsigned char PacketPresentPosition[2]={0x24,2};
00477 SendPacket(ID,READ,PacketPresentPosition,2);
00478 ReceivePacket(Buffer,8,TimeOut);
00479 if(Errors(Buffer))
00480 return (Buffer[5]+(Buffer[6]<<8));
00481 return -1;
00482 }
00483
00484
00485 short Dynamixel::GetPresentSpeed(unsigned char ID)
00486 {
00487 #ifdef __DEBUG__DYNAMIXEL_USE__
00488 Serial.println("Getting present speed...");
00489 #endif
00490
00491 unsigned char Buffer[9];
00492 unsigned char PacketPresentSpeed[2]={0x26,2};
00493 SendPacket(ID,READ,PacketPresentSpeed,2);
00494 ReceivePacket(Buffer,8,TimeOut);
00495 if(Errors(Buffer))
00496 {
00497 short Speed=Buffer[5]+(Buffer[6]<<8);
00498 return (Speed&(0<<11));
00499 }
00500 return -1;
00501 }
00502
00503
00504
00505 short Dynamixel::GetPresentLoad(unsigned char ID)
00506 {
00507 #ifdef __DEBUG__DYNAMIXEL_USE__
00508 Serial.println("Getting present load...");
00509 #endif
00510
00511 unsigned char Buffer[9];
00512 unsigned char PacketPresentLoad[2]={0x28,2};
00513 SendPacket(ID,READ,PacketPresentLoad,2);
00514 ReceivePacket(Buffer,8,TimeOut);
00515 if(Errors(Buffer))
00516 return (Buffer[5]+(Buffer[6]<<8));
00517 return -1;
00518 }
00519
00520
00521
00522 char Dynamixel::GetMoving(unsigned char ID)
00523 {
00524 #ifdef __DEBUG__DYNAMIXEL_USE__
00525 Serial.println("Getting moving...");
00526 #endif
00527
00528 unsigned char Buffer[8];
00529 unsigned char PacketMoving[2]={0x2E,1};
00530 SendPacket(ID,READ,PacketMoving,2);
00531 ReceivePacket(Buffer,7,TimeOut);
00532 if(Errors(Buffer))
00533 return (Buffer[5]);
00534 return -1;
00535 }