UVic Assistive Technology Lab
/
DSLR_Camera_Gimbal
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.
Dependencies: mbed ros_lib_kinetic
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)*190); 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] = RAM_GOAL_POSITION_L; 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 + RAM_GOAL_POSITION_L + 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::writeReg(unsigned char ID,unsigned char reg,unsigned int data){ 00535 00536 Instruction_Packet_Array[0] = ID; 00537 Instruction_Packet_Array[1] = 0x05; 00538 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00539 Instruction_Packet_Array[3] = reg; 00540 Instruction_Packet_Array[4] = (char)data; 00541 Instruction_Packet_Array[5] = (char)((data & 0xFF00) >> 8); 00542 Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + reg + (char)data + (char)((data & 0xFF00) >> 8)); 00543 00544 if (servoSerial->readable()) 00545 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00546 00547 transmitInstructionPacket(); 00548 00549 00550 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 00551 return (0x00); 00552 }else{ 00553 readStatusPacket(); 00554 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00555 return (Status_Packet_Array[0]); // Return SERVO ID 00556 }else{ 00557 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00558 } 00559 } 00560 } 00561 00562 unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){ 00563 00564 Instruction_Packet_Array[0] = ID; 00565 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; 00566 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00567 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; 00568 Instruction_Packet_Array[4] = (char)(Position); 00569 Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8); 00570 Instruction_Packet_Array[6] = (char)(Speed); 00571 Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8); 00572 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)); 00573 00574 if (servoSerial->readable()) 00575 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00576 00577 transmitInstructionPacket(); 00578 00579 00580 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 00581 return (0x00); 00582 }else{ 00583 readStatusPacket(); 00584 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00585 return (Status_Packet_Array[0]); // Return SERVO ID 00586 }else{ 00587 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00588 } 00589 } 00590 } 00591 00592 unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){ 00593 00594 Instruction_Packet_Array[0] = ID; 00595 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH; 00596 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; 00597 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; 00598 Instruction_Packet_Array[4] = (char)(Position); 00599 Instruction_Packet_Array[5] = (char)(Position >> 8); 00600 Instruction_Packet_Array[6] = (char)(Speed); 00601 Instruction_Packet_Array[7] = (char)(Speed >> 8); 00602 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)); 00603 00604 if (servoSerial->readable()) 00605 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00606 00607 transmitInstructionPacket(); 00608 00609 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 00610 return (0x00); 00611 }else{ 00612 readStatusPacket(); 00613 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00614 return (Status_Packet_Array[0]); // Return SERVO ID 00615 }else{ 00616 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00617 } 00618 } 00619 } 00620 00621 unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){ 00622 00623 char Speed_H,Speed_L; 00624 Speed_L = Speed; 00625 if (Rotation == 0){ // Move Left 00626 Speed_H = Speed >> 8; 00627 } 00628 else if (Rotation == 1){ // Move Right 00629 Speed_H = (Speed >> 8)+4; 00630 } 00631 00632 Instruction_Packet_Array[0] = ID; 00633 Instruction_Packet_Array[1] = WHEEL_LENGTH; 00634 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00635 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00636 Instruction_Packet_Array[4] = Speed_L; 00637 Instruction_Packet_Array[5] = Speed_H; 00638 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); 00639 00640 if (servoSerial->readable()) 00641 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00642 00643 transmitInstructionPacket(); 00644 00645 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 00646 return (0x00); 00647 }else{ 00648 readStatusPacket(); 00649 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00650 return (Status_Packet_Array[0]); // Return SERVO ID 00651 }else{ 00652 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00653 } 00654 } 00655 00656 } 00657 00658 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){ 00659 00660 char Speed1_H,Speed1_L; 00661 Speed1_L = Speed1; 00662 if (Dir1 == 0){ // Move Left 00663 Speed1_H = Speed1 >> 8; 00664 } 00665 else if (Dir1 == 1) // Move Right 00666 { 00667 Speed1_H = (Speed1 >> 8)+4; 00668 } 00669 00670 char Speed2_H,Speed2_L; 00671 Speed2_L = Speed2; 00672 if (Dir2 == 0){ // Move Left 00673 Speed2_H = Speed2 >> 8; 00674 } 00675 else if (Dir2 == 1) // Move Right 00676 { 00677 Speed2_H = (Speed2 >> 8)+4; 00678 } 00679 00680 char Speed3_H,Speed3_L; 00681 Speed3_L = Speed3; 00682 if (Dir3 == 0){ // Move Left 00683 Speed3_H = Speed3 >> 8; 00684 } 00685 else if (Dir3 == 1) // Move Right 00686 { 00687 Speed3_H = (Speed3 >> 8)+4; 00688 } 00689 00690 Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos 00691 Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH; 00692 Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE; 00693 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00694 Instruction_Packet_Array[4] = SYNC_DATA_LENGTH; 00695 Instruction_Packet_Array[5] = ID1; 00696 Instruction_Packet_Array[6] = Speed1_L; 00697 Instruction_Packet_Array[7] = Speed1_H; 00698 Instruction_Packet_Array[8] = ID2; 00699 Instruction_Packet_Array[9] = Speed2_L; 00700 Instruction_Packet_Array[10] = Speed2_H; 00701 Instruction_Packet_Array[11] = ID3; 00702 Instruction_Packet_Array[12] = Speed3_L; 00703 Instruction_Packet_Array[13] = Speed3_H; 00704 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)); 00705 00706 transmitInstructionPacket(); 00707 00708 } 00709 00710 unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){ 00711 00712 char Speed_H,Speed_L; 00713 Speed_L = Speed; 00714 if (Dir == 0){ // Move Left 00715 Speed_H = Speed >> 8; 00716 } 00717 else if (Dir == 1) // Move Right 00718 { 00719 Speed_H = (Speed >> 8)+4; 00720 } 00721 00722 Instruction_Packet_Array[0] = ID; 00723 Instruction_Packet_Array[1] = WHEEL_LENGTH; 00724 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA; 00725 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L; 00726 Instruction_Packet_Array[4] = Speed_L; 00727 Instruction_Packet_Array[5] = Speed_H; 00728 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H); 00729 00730 if (servoSerial->readable()) 00731 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00732 00733 transmitInstructionPacket(); 00734 00735 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 00736 return (0x00); 00737 }else{ 00738 readStatusPacket(); 00739 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00740 return (Status_Packet_Array[0]); // Return SERVO ID 00741 }else{ 00742 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00743 } 00744 } 00745 00746 } 00747 00748 unsigned int DynamixelClass::action(unsigned char ID){ 00749 00750 Instruction_Packet_Array[0] = ID; 00751 Instruction_Packet_Array[1] = RESET_LENGTH; 00752 Instruction_Packet_Array[2] = COMMAND_ACTION; 00753 Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION); 00754 00755 if (servoSerial->readable()) 00756 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00757 00758 transmitInstructionPacket(); 00759 00760 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 00761 return (0x00); 00762 }else{ 00763 readStatusPacket(); 00764 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00765 return (Status_Packet_Array[0]); // Return SERVO ID 00766 }else{ 00767 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00768 } 00769 } 00770 } 00771 00772 unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){ 00773 00774 Instruction_Packet_Array[0] = ID; 00775 Instruction_Packet_Array[1] = LED_LENGTH; 00776 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA; 00777 Instruction_Packet_Array[3] = RAM_LED; 00778 Instruction_Packet_Array[4] = Status; 00779 Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status); 00780 00781 if (servoSerial->readable()) 00782 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00783 00784 transmitInstructionPacket(); 00785 00786 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 00787 return (0x00); 00788 }else{ 00789 readStatusPacket(); 00790 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00791 return (Status_Packet_Array[0]); // Return SERVO ID 00792 }else{ 00793 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00794 } 00795 } 00796 } 00797 00798 unsigned int DynamixelClass::readTemperature(unsigned char ID){ 00799 00800 Instruction_Packet_Array[0] = ID; 00801 Instruction_Packet_Array[1] = READ_TEMP_LENGTH; 00802 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00803 Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE; 00804 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00805 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH); 00806 00807 if (servoSerial->readable()) 00808 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00809 00810 transmitInstructionPacket(); 00811 readStatusPacket(); 00812 00813 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00814 return Status_Packet_Array[3]; 00815 }else{ 00816 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00817 } 00818 } 00819 00820 unsigned int DynamixelClass::readGoalPosition(unsigned char ID){ 00821 00822 Instruction_Packet_Array[0] = ID; 00823 Instruction_Packet_Array[1] = READ_POS_LENGTH; 00824 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00825 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L; 00826 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00827 Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_GOAL_POSITION_L + READ_TWO_BYTE_LENGTH); 00828 00829 if (servoSerial->readable()) 00830 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00831 00832 transmitInstructionPacket(); 00833 readStatusPacket(); 00834 00835 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value 00836 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value 00837 }else{ 00838 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00839 } 00840 } 00841 00842 unsigned int DynamixelClass::readPosition(unsigned char ID){ 00843 00844 Instruction_Packet_Array[0] = ID; 00845 Instruction_Packet_Array[1] = READ_POS_LENGTH; 00846 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00847 Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L; 00848 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00849 Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH); 00850 00851 if (servoSerial->readable()) 00852 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00853 00854 transmitInstructionPacket(); 00855 readStatusPacket(); 00856 00857 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value 00858 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value 00859 }else{ 00860 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00861 } 00862 } 00863 00864 unsigned int DynamixelClass::readLoad(unsigned char ID){ 00865 00866 Instruction_Packet_Array[0] = ID; 00867 Instruction_Packet_Array[1] = READ_LOAD_LENGTH; 00868 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00869 Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L; 00870 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00871 Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH); 00872 00873 if (servoSerial->readable()) 00874 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00875 00876 transmitInstructionPacket(); 00877 readStatusPacket(); 00878 00879 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00880 return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value 00881 }else{ 00882 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00883 } 00884 } 00885 00886 unsigned int DynamixelClass::readSpeed(unsigned char ID){ 00887 00888 Instruction_Packet_Array[0] = ID; 00889 Instruction_Packet_Array[1] = READ_SPEED_LENGTH; 00890 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00891 Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L; 00892 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH; 00893 Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH); 00894 00895 if (servoSerial->readable()) 00896 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00897 00898 transmitInstructionPacket(); 00899 readStatusPacket(); 00900 00901 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00902 return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value 00903 }else{ 00904 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00905 } 00906 } 00907 00908 00909 unsigned int DynamixelClass::readVoltage(unsigned char ID){ 00910 00911 Instruction_Packet_Array[0] = ID; 00912 Instruction_Packet_Array[1] = READ_VOLT_LENGTH; 00913 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00914 Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE; 00915 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00916 Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH); 00917 00918 if (servoSerial->readable()) 00919 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00920 00921 transmitInstructionPacket(); 00922 readStatusPacket(); 00923 00924 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00925 return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage) 00926 }else{ 00927 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00928 } 00929 } 00930 00931 unsigned int DynamixelClass::checkRegister(unsigned char ID){ 00932 00933 Instruction_Packet_Array[0] = ID; 00934 Instruction_Packet_Array[1] = READ_REGISTER_LENGTH; 00935 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00936 Instruction_Packet_Array[3] = RAM_REGISTER; 00937 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00938 Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH); 00939 00940 if (servoSerial->readable()) 00941 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00942 00943 transmitInstructionPacket(); 00944 readStatusPacket(); 00945 00946 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00947 return (Status_Packet_Array[3]); // Return register value 00948 }else{ 00949 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00950 } 00951 } 00952 00953 unsigned int DynamixelClass::checkMovement(unsigned char ID){ 00954 00955 Instruction_Packet_Array[0] = ID; 00956 Instruction_Packet_Array[1] = READ_MOVING_LENGTH; 00957 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00958 Instruction_Packet_Array[3] = RAM_MOVING; 00959 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00960 Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH); 00961 00962 if (servoSerial->readable()) 00963 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00964 00965 transmitInstructionPacket(); 00966 readStatusPacket(); 00967 00968 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00969 return (Status_Packet_Array[3]); // Return movement value 00970 }else{ 00971 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00972 } 00973 } 00974 00975 unsigned int DynamixelClass::checkLock(unsigned char ID){ 00976 00977 Instruction_Packet_Array[0] = ID; 00978 Instruction_Packet_Array[1] = READ_LOCK_LENGTH; 00979 Instruction_Packet_Array[2] = COMMAND_READ_DATA; 00980 Instruction_Packet_Array[3] = RAM_LOCK; 00981 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH; 00982 Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH); 00983 00984 if (servoSerial->readable()) 00985 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer 00986 00987 transmitInstructionPacket(); 00988 readStatusPacket(); 00989 00990 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value 00991 return (Status_Packet_Array[3]); // Return Lock value 00992 }else{ 00993 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value 00994 } 00995 } 00996 00997
Generated on Tue Jul 12 2022 19:42:54 by
1.7.2