Chris LU
/
Self_Riding_Bicycle
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
Mx28.cpp
00001 #include <stdlib.h> 00002 #include <stdio.h> 00003 #include "mbed.h" 00004 #include "Mx28.h" 00005 00006 00007 unsigned char Instruction_Packet_Array[15]; // Array to hold instruction packet data 00008 unsigned char Status_Packet_Array[8]; // Array to hold returned status packet data 00009 unsigned int Time_Counter; // Timer for time out watchers 00010 unsigned char Direction_Pin; // Pin to control TX/RX buffer chip 00011 unsigned char Status_Return_Value = READ; // Status packet return states ( NON , READ , ALL ) 00012 00013 //------------------------------------------------------------------------------------------------------------------------------- 00014 // Private Methods 00015 //------------------------------------------------------------------------------------------------------------------------------- 00016 void DynamixelClass::debugframe(void) { 00017 for (int i=0; i<10; i++) 00018 printf("%x,",Instruction_Packet_Array[i]); 00019 printf("\r\n"); 00020 } 00021 00022 void DynamixelClass::debugStatusframe(void) { 00023 for (int i=0; i<10; i++) 00024 printf("%x,",Status_Packet_Array[i]); 00025 printf("\r\n"); 00026 } 00027 00028 00029 void DynamixelClass::transmitInstructionPacket(void){ // Transmit instruction packet to Dynamixel 00030 00031 unsigned char Counter; 00032 Counter = 0; 00033 servoSerialDir->write(1); // Set TX Buffer pin to HIGH 00034 00035 servoSerial->putc(HEADER); // Write Header (0xFF) data 1 to serial 00036 servoSerial->putc(HEADER); // Write Header (0xFF) data 2 to serial 00037 servoSerial->putc(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial 00038 servoSerial->putc(Instruction_Packet_Array[1]); // Write packet length to serial 00039 00040 do{ 00041 servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write Instuction & Parameters (if there is any) to serial 00042 Counter++; 00043 }while((Instruction_Packet_Array[1] - 2) >= Counter); 00044 00045 servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial 00046 wait_us((Counter + 4)*11); 00047 servoSerialDir->write(0); // Set TX Buffer pin to LOW after data has been sent 00048 00049 } 00050 00051 00052 unsigned int DynamixelClass::readStatusPacket(void){ 00053 00054 unsigned char Counter = 0x00; 00055 00056 static unsigned char InBuff[20]; 00057 unsigned char i, j, RxState; 00058 00059 00060 Status_Packet_Array[0] = 0x00; 00061 Status_Packet_Array[1] = 0x00; 00062 Status_Packet_Array[2] = 0x00; 00063 Status_Packet_Array[3] = 0x00; 00064 i=0; RxState=0; j=0; InBuff[0]=0; 00065 Timer timer; 00066 timer.start(); 00067 00068 00069 while (RxState<3){ // Wait for " header + header + frame length + error " RX data 00070 if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){ 00071 return Status_Packet_Array[2] = 0x80; // Return with Error if Serial data not received with in time limit 00072 00073 } 00074 00075 if (servoSerial->readable()) { 00076 InBuff[i]=servoSerial->getc(); 00077 if (InBuff[0]==0xff) i++; // When we have the first header we starts to inc data to inbuffer 00078 00079 00080 if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header 00081 00082 switch (RxState) { 00083 case 1: {//Read header 00084 if ((InBuff[j] == 0xFF) && (InBuff[j+1]== 0xFF)){ // checkes that we have got the buffer 00085 j=2; 00086 Status_Packet_Array[0] = InBuff[j++]; // ID sent from Dynamixel 00087 Status_Packet_Array[1] = InBuff[j++]; // Frame Length of status packet 00088 Status_Packet_Array[2] = InBuff[j++]; // Error (char) 00089 RxState++; led2->write(0); 00090 } 00091 } break; 00092 case 2: {//Read data 00093 if (i>Status_Packet_Array[1]+3) { //We have recieved all data 00094 do{ 00095 Status_Packet_Array[3 + Counter] = InBuff[j++]; 00096 Counter++; 00097 }while(Status_Packet_Array[1] > Counter); // Read Parameter(s) into array 00098 00099 Status_Packet_Array[Counter + 4] = InBuff[j++]; // Read Check sum 00100 RxState++; 00101 } 00102 } break; 00103 } //switch 00104 } 00105 }//while.. 00106 timer.stop(); 00107 // debugStatusframe(); 00108 00109 return 0x00; 00110 } 00111 00112 00113 00114 //------------------------------------------------------------------------------------------------------------------------------- 00115 // Public Methods 00116 //------------------------------------------------------------------------------------------------------------------------------- 00117 00118 DynamixelClass::DynamixelClass(int baud, PinName D_Pin, PinName tx, PinName rx){ 00119 servoSerial=new Serial(tx, rx); 00120 servoSerial->baud(baud); 00121 servoSerialDir= new DigitalOut(D_Pin); 00122 servoSerialDir->write(0); 00123 led2=new DigitalOut(LED2); 00124 00125 } 00126 00127 DynamixelClass::~DynamixelClass(){ 00128 if(servoSerial != NULL) 00129 delete servoSerial; 00130 } 00131 00132 unsigned int DynamixelClass::reset(unsigned char ID){ 00133 00134 Instruction_Packet_Array[0] = ID; 00135 Instruction_Packet_Array[1] = RESET_LENGTH; 00136 Instruction_Packet_Array[2] = COMMAND_RESET; 00137 Instruction_Packet_Array[3] = ~(ID + RESET_LENGTH + COMMAND_RESET); //Checksum; 00138 00139 if (servoSerial->readable()) 00140 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00141 00142 transmitInstructionPacket(); 00143 00144 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00145 return (0x00); 00146 }else{ 00147 readStatusPacket(); 00148 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00149 return (Status_Packet_Array[0]); // Return SERVO ID 00150 }else{ 00151 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00152 } 00153 } 00154 } 00155 00156 void DynamixelClass::NewBaudRate(int baud) { 00157 //Change the baudrate on then MBED 00158 int Baudrate_BPS = 0; 00159 Baudrate_BPS =(int) 2000000 / (baud + 1); // Calculate Baudrate as ber "Robotis e-manual" 00160 servoSerial->baud(Baudrate_BPS); 00161 } 00162 00163 unsigned int DynamixelClass::ping(unsigned char ID){ 00164 00165 Instruction_Packet_Array[0] = ID; 00166 Instruction_Packet_Array[1] = PING_LENGTH; 00167 Instruction_Packet_Array[2] = COMMAND_PING; 00168 Instruction_Packet_Array[3] = ~(ID + PING_LENGTH + COMMAND_PING); 00169 00170 if (servoSerial->readable()) 00171 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00172 00173 transmitInstructionPacket(); 00174 readStatusPacket(); 00175 00176 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00177 return (Status_Packet_Array[0]); // Return SERVO ID 00178 }else{ 00179 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00180 } 00181 } 00182 00183 unsigned int DynamixelClass::setStatusPaketReturnDelay(unsigned char ID,unsigned char ReturnDelay){ 00184 00185 Instruction_Packet_Array[0] = ID; 00186 Instruction_Packet_Array[1] = SET_RETURN_LENGTH; 00187 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00188 Instruction_Packet_Array[3] = EEPROM_RETURN_DELAY_TIME; 00189 Instruction_Packet_Array[4] = (char) (ReturnDelay/2); 00190 Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_DELAY_TIME + (char)(ReturnDelay/2)); 00191 00192 if (servoSerial->readable()) 00193 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00194 00195 transmitInstructionPacket(); 00196 00197 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00198 return (0x00); 00199 }else{ 00200 readStatusPacket(); 00201 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00202 return (Status_Packet_Array[0]); // Return SERVO ID 00203 }else{ 00204 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00205 } 00206 } 00207 00208 } 00209 00210 unsigned int DynamixelClass::setID(unsigned char ID, unsigned char New_ID){ 00211 00212 Instruction_Packet_Array[0] = ID; 00213 Instruction_Packet_Array[1] = SET_ID_LENGTH; 00214 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00215 Instruction_Packet_Array[3] = EEPROM_ID; 00216 Instruction_Packet_Array[4] = New_ID; 00217 Instruction_Packet_Array[5] = ~(ID + SET_ID_LENGTH + COMMAND_WRITE_DATA+ EEPROM_ID + New_ID); 00218 00219 if (servoSerial->readable()) 00220 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00221 00222 transmitInstructionPacket(); 00223 00224 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00225 return (0x00); 00226 }else{ 00227 readStatusPacket(); 00228 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00229 return (Status_Packet_Array[0]); // Return SERVO ID 00230 }else{ 00231 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00232 } 00233 } 00234 } 00235 00236 unsigned int DynamixelClass::setBaudRate(unsigned char ID, long Baud){ 00237 00238 Instruction_Packet_Array[0] = ID; 00239 Instruction_Packet_Array[1] = SET_BD_LENGTH; 00240 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00241 Instruction_Packet_Array[3] = EEPROM_BAUD_RATE; 00242 // if (Baud > 2250000){ 00243 if (Baud >= 1000000){ 00244 switch (Baud){ 00245 case 2250000: 00246 Instruction_Packet_Array[4] = 0xFA; 00247 break; 00248 case 2500000: 00249 Instruction_Packet_Array[4] = 0xFB; 00250 break; 00251 case 3000000: 00252 Instruction_Packet_Array[4] = 0xFC; 00253 break; 00254 case 1000000: 00255 Instruction_Packet_Array[4] = 0x01; 00256 } 00257 }else{ 00258 Instruction_Packet_Array[4] = (char)((2000000/Baud) - 1); 00259 } 00260 Instruction_Packet_Array[5] = ~(ID + SET_BD_LENGTH + COMMAND_WRITE_DATA + EEPROM_BAUD_RATE + (char)((2000000/Baud) - 1) ); 00261 00262 if (servoSerial->readable()) 00263 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00264 00265 transmitInstructionPacket(); 00266 00267 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00268 return (0x00); 00269 }else{ 00270 readStatusPacket(); 00271 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00272 return (Status_Packet_Array[0]); // Return SERVO ID 00273 }else{ 00274 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00275 } 00276 } 00277 } 00278 00279 unsigned int DynamixelClass::setMaxTorque( unsigned char ID, int Torque){ 00280 00281 Instruction_Packet_Array[0] = ID; 00282 Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH; 00283 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00284 Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ; 00285 Instruction_Packet_Array[4] = (char)(Torque); 00286 Instruction_Packet_Array[5] = (char)(Torque >> 8); 00287 Instruction_Packet_Array[6] = ~(ID + SET_MAX_TORQUE_LENGTH + COMMAND_WRITE_DATA + EEPROM_MAX_TORQUE_L + (char)(Torque) + (char)(Torque >> 8)); 00288 00289 if (servoSerial->readable()) 00290 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00291 00292 transmitInstructionPacket(); 00293 00294 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00295 return (0x00); 00296 }else{ 00297 readStatusPacket(); 00298 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00299 return (Status_Packet_Array[0]); // Return SERVO ID 00300 }else{ 00301 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00302 } 00303 } 00304 } 00305 00306 unsigned int DynamixelClass::setHoldingTorque(unsigned char ID, bool Set){ 00307 00308 Instruction_Packet_Array[0] = ID; 00309 Instruction_Packet_Array[1] = SET_HOLDING_TORQUE_LENGTH; 00310 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00311 Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE; 00312 Instruction_Packet_Array[4] = Set; 00313 Instruction_Packet_Array[5] = ~(ID + SET_HOLDING_TORQUE_LENGTH + COMMAND_WRITE_DATA + RAM_TORQUE_ENABLE + Set); 00314 00315 if (servoSerial->readable()) 00316 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00317 00318 transmitInstructionPacket(); 00319 00320 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00321 return (0x00); 00322 }else{ 00323 readStatusPacket(); 00324 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00325 return (Status_Packet_Array[0]); // Return SERVO ID 00326 }else{ 00327 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00328 } 00329 } 00330 } 00331 00332 unsigned int DynamixelClass::setAlarmShutdown(unsigned char ID,unsigned char Set){ 00333 00334 Instruction_Packet_Array[0] = ID; 00335 Instruction_Packet_Array[1] = SET_ALARM_LENGTH; 00336 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00337 Instruction_Packet_Array[3] = EEPROM_ALARM_SHUTDOWN; 00338 Instruction_Packet_Array[4] = Set; 00339 Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_SHUTDOWN + Set); 00340 00341 if (servoSerial->readable()) 00342 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00343 00344 transmitInstructionPacket(); 00345 00346 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00347 return (0x00); 00348 }else{ 00349 readStatusPacket(); 00350 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00351 return (Status_Packet_Array[0]); // Return SERVO ID 00352 }else{ 00353 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00354 } 00355 } 00356 00357 } 00358 00359 unsigned int DynamixelClass::setStatusPaket(unsigned char ID,unsigned char Set){ 00360 00361 Instruction_Packet_Array[0] = ID; 00362 Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH; 00363 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00364 Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL; 00365 Instruction_Packet_Array[4] = Set; 00366 Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set); 00367 00368 Status_Return_Value = Set; 00369 00370 if (servoSerial->readable()) 00371 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00372 00373 transmitInstructionPacket(); 00374 00375 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00376 return (0x00); 00377 }else{ 00378 readStatusPacket(); 00379 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00380 return (Status_Packet_Array[0]); // Return SERVO ID 00381 }else{ 00382 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00383 } 00384 } 00385 00386 } 00387 00388 00389 unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){ 00390 00391 Instruction_Packet_Array[0] = ID; 00392 Instruction_Packet_Array[1] = SET_MODE_LENGTH; 00393 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00394 Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L; 00395 if ( Dynamixel_Mode == WHEEL) { // Set WHEEL mode, this is done by setting both the clockwise and anti-clockwise angle limits to ZERO 00396 Instruction_Packet_Array[4] = 0x00; 00397 Instruction_Packet_Array[5] = 0x00; 00398 Instruction_Packet_Array[6] = 0x00; 00399 Instruction_Packet_Array[7] = 0x00; 00400 Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L); 00401 }else { // Else set SERVO mode 00402 Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit); 00403 Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8); 00404 Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit); 00405 Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8); 00406 Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L + (char)(Dynamixel_CW_Limit) + (char)((Dynamixel_CW_Limit & 0x0F00) >> 8) + (char)(Dynamixel_CCW_Limit) + (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8)); 00407 } 00408 00409 00410 if (servoSerial->readable()) 00411 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00412 00413 transmitInstructionPacket(); 00414 00415 if (Status_Return_Value == ALL){ 00416 readStatusPacket(); 00417 if (Status_Packet_Array[2] != 0){ 00418 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00419 } 00420 00421 } 00422 return 0x00; //if no errors 00423 } 00424 00425 unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){ 00426 00427 Instruction_Packet_Array[0] = ID; 00428 Instruction_Packet_Array[1] = SET_PUNCH_LENGTH; 00429 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00430 Instruction_Packet_Array[3] = RAM_PUNCH_L; 00431 Instruction_Packet_Array[4] = (char)(Punch); 00432 Instruction_Packet_Array[5] = (char)(Punch >> 8); 00433 Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) ); 00434 00435 if (servoSerial->readable()) 00436 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00437 00438 transmitInstructionPacket(); 00439 00440 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00441 return (0x00); 00442 }else{ 00443 readStatusPacket(); 00444 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00445 return (Status_Packet_Array[0]); // Return SERVO ID 00446 }else{ 00447 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00448 } 00449 } 00450 00451 } 00452 00453 unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){ 00454 00455 Instruction_Packet_Array[0] = ID; 00456 Instruction_Packet_Array[1] = SET_PID_LENGTH; 00457 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00458 Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN; 00459 Instruction_Packet_Array[4] = P; 00460 Instruction_Packet_Array[5] = I; 00461 Instruction_Packet_Array[6] = D; 00462 Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D ); 00463 00464 if (servoSerial->readable()) 00465 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00466 00467 transmitInstructionPacket(); 00468 00469 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00470 return (0x00); 00471 }else{ 00472 readStatusPacket(); 00473 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00474 return (Status_Packet_Array[0]); // Return SERVO ID 00475 }else{ 00476 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00477 } 00478 } 00479 } 00480 00481 unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){ 00482 00483 Instruction_Packet_Array[0] = ID; 00484 Instruction_Packet_Array[1] = SET_TEMP_LENGTH; 00485 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00486 Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE; 00487 Instruction_Packet_Array[4] = temp; 00488 Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp); 00489 00490 if (servoSerial->readable()) 00491 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00492 00493 transmitInstructionPacket(); 00494 00495 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00496 return (0x00); 00497 }else{ 00498 readStatusPacket(); 00499 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00500 return (Status_Packet_Array[0]); // Return SERVO ID 00501 }else{ 00502 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00503 } 00504 } 00505 } 00506 00507 unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){ 00508 00509 Instruction_Packet_Array[0] = ID; 00510 Instruction_Packet_Array[1] = SET_VOLT_LENGTH; 00511 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00512 Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE; 00513 Instruction_Packet_Array[4] = Volt_L; 00514 Instruction_Packet_Array[5] = Volt_H; 00515 Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H); 00516 00517 if (servoSerial->readable()) 00518 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00519 00520 transmitInstructionPacket(); 00521 00522 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00523 return (0x00); 00524 }else{ 00525 readStatusPacket(); 00526 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00527 return (Status_Packet_Array[0]); // Return SERVO ID 00528 }else{ 00529 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00530 } 00531 } 00532 } 00533 00534 unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){ 00535 00536 Instruction_Packet_Array[0] = ID; 00537 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; 00538 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00539 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; 00540 Instruction_Packet_Array[4] = (char)(Position); 00541 Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8); 00542 Instruction_Packet_Array[6] = (char)(Speed); 00543 Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8); 00544 Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_POSITION_L + Position + (char)((Position & 0x0F00) >> 8) + Speed + (char)((Speed & 0x0F00) >> 8)); 00545 00546 if (servoSerial->readable()) 00547 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00548 00549 transmitInstructionPacket(); 00550 00551 00552 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00553 return (0x00); 00554 }else{ 00555 readStatusPacket(); 00556 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00557 return (Status_Packet_Array[0]); // Return SERVO ID 00558 }else{ 00559 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00560 } 00561 } 00562 } 00563 00564 unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){ 00565 00566 Instruction_Packet_Array[0] = ID; 00567 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; 00568 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; 00569 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; 00570 Instruction_Packet_Array[4] = (char)(Position); 00571 Instruction_Packet_Array[5] = (char)(Position >> 8); 00572 Instruction_Packet_Array[6] = (char)(Speed); 00573 Instruction_Packet_Array[7] = (char)(Speed >> 8); 00574 Instruction_Packet_Array[8] = ~(ID + SERVO_GOAL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_POSITION_L + (char)(Position) + (char)(Position >> 8) + (char)(Speed) + (char)(Speed >> 8)); 00575 00576 if (servoSerial->readable()) 00577 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00578 00579 transmitInstructionPacket(); 00580 00581 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00582 return (0x00); 00583 }else{ 00584 readStatusPacket(); 00585 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00586 return (Status_Packet_Array[0]); // Return SERVO ID 00587 }else{ 00588 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00589 } 00590 } 00591 } 00592 00593 unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){ 00594 00595 char Speed_H,Speed_L; 00596 Speed_L = Speed; 00597 if (Rotation == 0){ // Move Left 00598 Speed_H = Speed >> 8; 00599 } 00600 else if (Rotation == 1){ // Move Right 00601 Speed_H = (Speed >> 8)+4; 00602 } 00603 00604 Instruction_Packet_Array[0] = ID; 00605 Instruction_Packet_Array[1] = WHEEL_LENGTH; 00606 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00607 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00608 Instruction_Packet_Array[4] = Speed_L; 00609 Instruction_Packet_Array[5] = Speed_H; 00610 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); 00611 00612 if (servoSerial->readable()) 00613 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00614 00615 transmitInstructionPacket(); 00616 00617 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00618 return (0x00); 00619 }else{ 00620 readStatusPacket(); 00621 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00622 return (Status_Packet_Array[0]); // Return SERVO ID 00623 }else{ 00624 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00625 } 00626 } 00627 00628 } 00629 00630 void DynamixelClass::wheelSync(unsigned char ID1, bool Dir1, unsigned int Speed1, unsigned char ID2, bool Dir2, unsigned int Speed2, unsigned char ID3, bool Dir3, unsigned int Speed3){ 00631 00632 char Speed1_H,Speed1_L; 00633 Speed1_L = Speed1; 00634 if (Dir1 == 0){ // Move Left 00635 Speed1_H = Speed1 >> 8; 00636 } 00637 else if (Dir1 == 1) // Move Right 00638 { 00639 Speed1_H = (Speed1 >> 8)+4; 00640 } 00641 00642 char Speed2_H,Speed2_L; 00643 Speed2_L = Speed2; 00644 if (Dir2 == 0){ // Move Left 00645 Speed2_H = Speed2 >> 8; 00646 } 00647 else if (Dir2 == 1) // Move Right 00648 { 00649 Speed2_H = (Speed2 >> 8)+4; 00650 } 00651 00652 char Speed3_H,Speed3_L; 00653 Speed3_L = Speed3; 00654 if (Dir3 == 0){ // Move Left 00655 Speed3_H = Speed3 >> 8; 00656 } 00657 else if (Dir3 == 1) // Move Right 00658 { 00659 Speed3_H = (Speed3 >> 8)+4; 00660 } 00661 00662 Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos 00663 Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH; 00664 Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE; 00665 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00666 Instruction_Packet_Array[4] = SYNC_DATA_LENGTH; 00667 Instruction_Packet_Array[5] = ID1; 00668 Instruction_Packet_Array[6] = Speed1_L; 00669 Instruction_Packet_Array[7] = Speed1_H; 00670 Instruction_Packet_Array[8] = ID2; 00671 Instruction_Packet_Array[9] = Speed2_L; 00672 Instruction_Packet_Array[10] = Speed2_H; 00673 Instruction_Packet_Array[11] = ID3; 00674 Instruction_Packet_Array[12] = Speed3_L; 00675 Instruction_Packet_Array[13] = Speed3_H; 00676 Instruction_Packet_Array[14] = (char)(~(0xFE + SYNC_LOAD_LENGTH + COMMAND_SYNC_WRITE + RAM_GOAL_SPEED_L + SYNC_DATA_LENGTH + ID1 + Speed1_L + Speed1_H + ID2 + Speed2_L + Speed2_H + ID3 + Speed3_L + Speed3_H)); 00677 00678 transmitInstructionPacket(); 00679 00680 } 00681 00682 unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){ 00683 00684 char Speed_H,Speed_L; 00685 Speed_L = Speed; 00686 if (Dir == 0){ // Move Left 00687 Speed_H = Speed >> 8; 00688 } 00689 else if (Dir == 1) // Move Right 00690 { 00691 Speed_H = (Speed >> 8)+4; 00692 } 00693 00694 Instruction_Packet_Array[0] = ID; 00695 Instruction_Packet_Array[1] = WHEEL_LENGTH; 00696 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; 00697 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00698 Instruction_Packet_Array[4] = Speed_L; 00699 Instruction_Packet_Array[5] = Speed_H; 00700 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); 00701 00702 if (servoSerial->readable()) 00703 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00704 00705 transmitInstructionPacket(); 00706 00707 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00708 return (0x00); 00709 }else{ 00710 readStatusPacket(); 00711 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00712 return (Status_Packet_Array[0]); // Return SERVO ID 00713 }else{ 00714 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00715 } 00716 } 00717 00718 } 00719 00720 unsigned int DynamixelClass::action(unsigned char ID){ 00721 00722 Instruction_Packet_Array[0] = ID; 00723 Instruction_Packet_Array[1] = RESET_LENGTH; 00724 Instruction_Packet_Array[2] = COMMAND_ACTION; 00725 Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION); 00726 00727 if (servoSerial->readable()) 00728 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00729 00730 transmitInstructionPacket(); 00731 00732 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00733 return (0x00); 00734 }else{ 00735 readStatusPacket(); 00736 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00737 return (Status_Packet_Array[0]); // Return SERVO ID 00738 }else{ 00739 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00740 } 00741 } 00742 } 00743 00744 unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){ 00745 00746 Instruction_Packet_Array[0] = ID; 00747 Instruction_Packet_Array[1] = LED_LENGTH; 00748 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00749 Instruction_Packet_Array[3] = RAM_LED; 00750 Instruction_Packet_Array[4] = Status; 00751 Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status); 00752 00753 if (servoSerial->readable()) 00754 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00755 00756 transmitInstructionPacket(); 00757 00758 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00759 return (0x00); 00760 }else{ 00761 readStatusPacket(); 00762 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00763 return (Status_Packet_Array[0]); // Return SERVO ID 00764 }else{ 00765 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00766 } 00767 } 00768 } 00769 00770 unsigned int DynamixelClass::readTemperature(unsigned char ID){ 00771 00772 Instruction_Packet_Array[0] = ID; 00773 Instruction_Packet_Array[1] = READ_TEMP_LENGTH; 00774 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00775 Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE; 00776 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00777 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH); 00778 00779 if (servoSerial->readable()) 00780 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00781 00782 transmitInstructionPacket(); 00783 readStatusPacket(); 00784 00785 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00786 return Status_Packet_Array[3]; 00787 }else{ 00788 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00789 } 00790 } 00791 00792 unsigned int DynamixelClass::readPosition(unsigned char ID){ 00793 00794 Instruction_Packet_Array[0] = ID; 00795 Instruction_Packet_Array[1] = READ_POS_LENGTH; 00796 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00797 Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L; 00798 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00799 Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH); 00800 00801 if (servoSerial->readable()) 00802 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00803 00804 transmitInstructionPacket(); 00805 readStatusPacket(); 00806 00807 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value 00808 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value 00809 }else{ 00810 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00811 } 00812 } 00813 00814 unsigned int DynamixelClass::readLoad(unsigned char ID){ 00815 00816 Instruction_Packet_Array[0] = ID; 00817 Instruction_Packet_Array[1] = READ_LOAD_LENGTH; 00818 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00819 Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L; 00820 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00821 Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH); 00822 00823 if (servoSerial->readable()) 00824 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00825 00826 transmitInstructionPacket(); 00827 readStatusPacket(); 00828 00829 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00830 return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value 00831 }else{ 00832 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00833 } 00834 } 00835 00836 unsigned int DynamixelClass::readSpeed(unsigned char ID){ 00837 00838 Instruction_Packet_Array[0] = ID; 00839 Instruction_Packet_Array[1] = READ_SPEED_LENGTH; 00840 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00841 Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L; 00842 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00843 Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH); 00844 00845 if (servoSerial->readable()) 00846 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00847 00848 transmitInstructionPacket(); 00849 readStatusPacket(); 00850 00851 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00852 return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value 00853 }else{ 00854 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00855 } 00856 } 00857 00858 00859 unsigned int DynamixelClass::readVoltage(unsigned char ID){ 00860 00861 Instruction_Packet_Array[0] = ID; 00862 Instruction_Packet_Array[1] = READ_VOLT_LENGTH; 00863 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00864 Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE; 00865 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00866 Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH); 00867 00868 if (servoSerial->readable()) 00869 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00870 00871 transmitInstructionPacket(); 00872 readStatusPacket(); 00873 00874 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00875 return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage) 00876 }else{ 00877 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00878 } 00879 } 00880 00881 unsigned int DynamixelClass::checkRegister(unsigned char ID){ 00882 00883 Instruction_Packet_Array[0] = ID; 00884 Instruction_Packet_Array[1] = READ_REGISTER_LENGTH; 00885 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00886 Instruction_Packet_Array[3] = RAM_REGISTER; 00887 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00888 Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH); 00889 00890 if (servoSerial->readable()) 00891 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00892 00893 transmitInstructionPacket(); 00894 readStatusPacket(); 00895 00896 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00897 return (Status_Packet_Array[3]); // Return register value 00898 }else{ 00899 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00900 } 00901 } 00902 00903 unsigned int DynamixelClass::checkMovement(unsigned char ID){ 00904 00905 Instruction_Packet_Array[0] = ID; 00906 Instruction_Packet_Array[1] = READ_MOVING_LENGTH; 00907 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00908 Instruction_Packet_Array[3] = RAM_MOVING; 00909 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00910 Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH); 00911 00912 if (servoSerial->readable()) 00913 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00914 00915 transmitInstructionPacket(); 00916 readStatusPacket(); 00917 00918 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00919 return (Status_Packet_Array[3]); // Return movement value 00920 }else{ 00921 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00922 } 00923 } 00924 00925 unsigned int DynamixelClass::checkLock(unsigned char ID){ 00926 00927 Instruction_Packet_Array[0] = ID; 00928 Instruction_Packet_Array[1] = READ_LOCK_LENGTH; 00929 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00930 Instruction_Packet_Array[3] = RAM_LOCK; 00931 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00932 Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH); 00933 00934 if (servoSerial->readable()) 00935 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00936 00937 transmitInstructionPacket(); 00938 readStatusPacket(); 00939 00940 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00941 return (Status_Packet_Array[3]); // Return Lock value 00942 }else{ 00943 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00944 } 00945 } 00946 00947 unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){ 00948 00949 Instruction_Packet_Array[0] = ID; 00950 Instruction_Packet_Array[1] = 0x04; 00951 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00952 Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE; 00953 Instruction_Packet_Array[4] = Status; 00954 Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status); 00955 00956 if (servoSerial->readable()) 00957 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00958 00959 transmitInstructionPacket(); 00960 00961 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00962 return (0x00); 00963 }else{ 00964 readStatusPacket(); 00965 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00966 return (Status_Packet_Array[0]); // Return SERVO ID 00967 }else{ 00968 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00969 } 00970 } 00971 } 00972 00973 unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){ 00974 00975 00976 Instruction_Packet_Array[0] = ID; 00977 Instruction_Packet_Array[1] = 0x05; 00978 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00979 Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L; 00980 Instruction_Packet_Array[4] = (char)(Torque); 00981 Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8); 00982 Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) ); 00983 00984 if (servoSerial->readable()) 00985 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00986 00987 transmitInstructionPacket(); 00988 00989 if (ID == 0XFE || Status_Return_Value != ALL ){ // If ID of FE is used no status packets are returned so we do not need to check it 00990 return (0x00); 00991 }else{ 00992 readStatusPacket(); 00993 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00994 return (Status_Packet_Array[0]); // Return SERVO ID 00995 }else{ 00996 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00997 } 00998 } 00999 01000 } 01001 01002 unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){ 01003 01004 Instruction_Packet_Array[0] = ID; 01005 Instruction_Packet_Array[1] = 0x04; 01006 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 01007 Instruction_Packet_Array[3] = Register; 01008 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 01009 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH); 01010 01011 if (servoSerial->readable()) 01012 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 01013 01014 transmitInstructionPacket(); 01015 readStatusPacket(); 01016 01017 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 01018 return Status_Packet_Array[3]; 01019 }else{ 01020 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 01021 } 01022 } 01023 01024
Generated on Thu Jul 14 2022 02:47:41 by
1.7.2