D:/DRISSI/arduino-0022/arduino-0022/libraries/dynamixelUse/dynamixelUse.cpp
Go to the documentation of this file.
00001 
00005 /* Preparing packets and control servomotors */
00006 
00007 #include "dynamixelUse.h"
00008 
00009 
00011 
00012 
00013 // Configure dynamixel communication
00014 Dynamixel::Dynamixel()
00015 {
00016         TimeOut=10000000;
00017         InitDynamixel();
00018 }
00019 
00020 // End communication with servomotors
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 // Send a ping
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 // Send a packet manually
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 // Check if packet contains no error
00064 int Dynamixel::CheckPacket(unsigned char *Packet)
00065 {
00066         return (Errors(Packet));
00067 }
00068 
00069 // Empty reception buffer
00070 void Dynamixel::ClearReceptionBuffer()
00071 {
00072         EmptyReceptionBuffer();
00073 }
00074 
00075 
00076 // Reuse servomotors after error occurenced
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                 //bool * err=false;
00085                 Serial.println("torque limit 0");
00086                 SetTorqueLimit(ID,1023); //GetMaxTorque(ID,err)&& (!err)
00087                         return 1;
00088         //}     
00089         return 0;
00090 }
00091 
00093 
00094 
00095 // Set goal position (in degrees)
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};                           // Prepare the packet
00104         SendPacket(ID,WRITE,PacketGoalPos,3);                                                           // Send the frame
00105         ReceivePacket(Buffer,6,TimeOut);                                                                        // Receive the return packet
00106 
00107         if(Errors(Buffer))
00108                 return 0;
00109         else
00110                 return 1;
00111 }
00112 
00113 // Set the clockwise angle limit (in degrees)
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 // Set the counter clockwise angle limit (in degrees)
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 // Set the moving speed (0 means 0% and 0xFE means 100% of the maximum speed)
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 // Set return delay time (0 to 254 where 1 data means 2 µs)
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 // Set max torque (0 to 1023 where 1023 means 100% of maximum torque)
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 // Set if torque is enable (0 means no torque and 1 generates torque)
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 // Set the CW margin of the compliance (0 to 255, unit is the same as goal pos)
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 // Set the CCW margin of the compliance (0 to 255, unit is the same as goal pos)
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 // Set the CW slope of the compliance (0 to 255 znd 255 is when the most flexibility is obtained)
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 // Set the CCW slope of the compliance (0 to 255 and 255 is when the most flexibility is obtained)
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 // Set the maximum torque limit (0 to 1023 where 1023 means 100% of the maximum torque)
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 // Get current clockwise angle limit (in degrees)
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 // Get current counter clockwise angle limit (in degrees)
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 // Return the moving speed
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 // Return the return delay time
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 // Returns the default value of the maximum torque
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 // Return if torque is enable or disable
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 // Return the CW margin of the compliance
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 // Return the CCW margin of the compliance
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 // Return the CW slope of the compliance
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 // Return the CCW slope of the compliance
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 // Return the limit of the maximum torque
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 // Return the present position
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 // Return the present speed
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 // Return the present load
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 // Return if there is any movement
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 }