Chris LU / Mbed 2 deprecated MX28_Sensor_Correction

Sensor_Correction

Dependencies:   LSM9DS0 mbed

Fork of MX28_Sensor_Correction by LDSC_Robotics_TAs

Mx28.cpp@1:3643ef2599cd, 2017-02-13 (annotated)

Committer:
alan82914
Date:
Mon Feb 13 05:05:52 2017 +0000
Revision:
1:3643ef2599cd
123456

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alan82914 1:3643ef2599cd 1 #include <stdlib.h>
alan82914 1:3643ef2599cd 2 #include <stdio.h>
alan82914 1:3643ef2599cd 3 #include "mbed.h"
alan82914 1:3643ef2599cd 4 #include "Mx28.h"
alan82914 1:3643ef2599cd 5
alan82914 1:3643ef2599cd 6
alan82914 1:3643ef2599cd 7 unsigned char Instruction_Packet_Array[15]; // Array to hold instruction packet data
alan82914 1:3643ef2599cd 8 unsigned char Status_Packet_Array[8]; // Array to hold returned status packet data
alan82914 1:3643ef2599cd 9 unsigned int Time_Counter; // Timer for time out watchers
alan82914 1:3643ef2599cd 10 unsigned char Direction_Pin; // Pin to control TX/RX buffer chip
alan82914 1:3643ef2599cd 11 unsigned char Status_Return_Value = READ; // Status packet return states ( NON , READ , ALL )
alan82914 1:3643ef2599cd 12
alan82914 1:3643ef2599cd 13 //-------------------------------------------------------------------------------------------------------------------------------
alan82914 1:3643ef2599cd 14 // Private Methods
alan82914 1:3643ef2599cd 15 //-------------------------------------------------------------------------------------------------------------------------------
alan82914 1:3643ef2599cd 16 void DynamixelClass::debugframe(void) {
alan82914 1:3643ef2599cd 17 for (int i=0; i<10; i++)
alan82914 1:3643ef2599cd 18 printf("%x,",Instruction_Packet_Array[i]);
alan82914 1:3643ef2599cd 19 printf("\r\n");
alan82914 1:3643ef2599cd 20 }
alan82914 1:3643ef2599cd 21
alan82914 1:3643ef2599cd 22 void DynamixelClass::debugStatusframe(void) {
alan82914 1:3643ef2599cd 23 for (int i=0; i<10; i++)
alan82914 1:3643ef2599cd 24 printf("%x,",Status_Packet_Array[i]);
alan82914 1:3643ef2599cd 25 printf("\r\n");
alan82914 1:3643ef2599cd 26 }
alan82914 1:3643ef2599cd 27
alan82914 1:3643ef2599cd 28
alan82914 1:3643ef2599cd 29 void DynamixelClass::transmitInstructionPacket(void){ // Transmit instruction packet to Dynamixel
alan82914 1:3643ef2599cd 30
alan82914 1:3643ef2599cd 31 unsigned char Counter;
alan82914 1:3643ef2599cd 32 Counter = 0;
alan82914 1:3643ef2599cd 33 servoSerialDir->write(1); // Set TX Buffer pin to HIGH
alan82914 1:3643ef2599cd 34
alan82914 1:3643ef2599cd 35 servoSerial->putc(HEADER); // Write Header (0xFF) data 1 to serial
alan82914 1:3643ef2599cd 36 servoSerial->putc(HEADER); // Write Header (0xFF) data 2 to serial
alan82914 1:3643ef2599cd 37 servoSerial->putc(Instruction_Packet_Array[0]); // Write Dynamixal ID to serial
alan82914 1:3643ef2599cd 38 servoSerial->putc(Instruction_Packet_Array[1]); // Write packet length to serial
alan82914 1:3643ef2599cd 39
alan82914 1:3643ef2599cd 40 do{
alan82914 1:3643ef2599cd 41 servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write Instuction & Parameters (if there is any) to serial
alan82914 1:3643ef2599cd 42 Counter++;
alan82914 1:3643ef2599cd 43 }while((Instruction_Packet_Array[1] - 2) >= Counter);
alan82914 1:3643ef2599cd 44
alan82914 1:3643ef2599cd 45 servoSerial->putc(Instruction_Packet_Array[Counter + 2]); // Write check sum to serial
alan82914 1:3643ef2599cd 46 wait_us((Counter + 4)*11);
alan82914 1:3643ef2599cd 47 servoSerialDir->write(0); // Set TX Buffer pin to LOW after data has been sent
alan82914 1:3643ef2599cd 48
alan82914 1:3643ef2599cd 49 }
alan82914 1:3643ef2599cd 50
alan82914 1:3643ef2599cd 51
alan82914 1:3643ef2599cd 52 unsigned int DynamixelClass::readStatusPacket(void){
alan82914 1:3643ef2599cd 53
alan82914 1:3643ef2599cd 54 unsigned char Counter = 0x00;
alan82914 1:3643ef2599cd 55
alan82914 1:3643ef2599cd 56 static unsigned char InBuff[20];
alan82914 1:3643ef2599cd 57 unsigned char i, j, RxState;
alan82914 1:3643ef2599cd 58
alan82914 1:3643ef2599cd 59
alan82914 1:3643ef2599cd 60 Status_Packet_Array[0] = 0x00;
alan82914 1:3643ef2599cd 61 Status_Packet_Array[1] = 0x00;
alan82914 1:3643ef2599cd 62 Status_Packet_Array[2] = 0x00;
alan82914 1:3643ef2599cd 63 Status_Packet_Array[3] = 0x00;
alan82914 1:3643ef2599cd 64 i=0; RxState=0; j=0; InBuff[0]=0;
alan82914 1:3643ef2599cd 65 Timer timer;
alan82914 1:3643ef2599cd 66 timer.start();
alan82914 1:3643ef2599cd 67
alan82914 1:3643ef2599cd 68
alan82914 1:3643ef2599cd 69 while (RxState<3){ // Wait for " header + header + frame length + error " RX data
alan82914 1:3643ef2599cd 70 if (timer.read_ms() >= STATUS_PACKET_TIMEOUT){
alan82914 1:3643ef2599cd 71 return Status_Packet_Array[2] = 0x80; // Return with Error if Serial data not received with in time limit
alan82914 1:3643ef2599cd 72
alan82914 1:3643ef2599cd 73 }
alan82914 1:3643ef2599cd 74
alan82914 1:3643ef2599cd 75 if (servoSerial->readable()) {
alan82914 1:3643ef2599cd 76 InBuff[i]=servoSerial->getc();
alan82914 1:3643ef2599cd 77 if (InBuff[0]==0xff) i++; // When we have the first header we starts to inc data to inbuffer
alan82914 1:3643ef2599cd 78
alan82914 1:3643ef2599cd 79
alan82914 1:3643ef2599cd 80 if ((i>=(STATUS_FRAME_BUFFER-1)) &&(RxState==0)) RxState++; //read header
alan82914 1:3643ef2599cd 81
alan82914 1:3643ef2599cd 82 switch (RxState) {
alan82914 1:3643ef2599cd 83 case 1: {//Read header
alan82914 1:3643ef2599cd 84 if ((InBuff[j] == 0xFF) && (InBuff[j+1]== 0xFF)){ // checkes that we have got the buffer
alan82914 1:3643ef2599cd 85 j=2;
alan82914 1:3643ef2599cd 86 Status_Packet_Array[0] = InBuff[j++]; // ID sent from Dynamixel
alan82914 1:3643ef2599cd 87 Status_Packet_Array[1] = InBuff[j++]; // Frame Length of status packet
alan82914 1:3643ef2599cd 88 Status_Packet_Array[2] = InBuff[j++]; // Error (char)
alan82914 1:3643ef2599cd 89 RxState++; led2->write(0);
alan82914 1:3643ef2599cd 90 }
alan82914 1:3643ef2599cd 91 } break;
alan82914 1:3643ef2599cd 92 case 2: {//Read data
alan82914 1:3643ef2599cd 93 if (i>Status_Packet_Array[1]+3) { //We have recieved all data
alan82914 1:3643ef2599cd 94 do{
alan82914 1:3643ef2599cd 95 Status_Packet_Array[3 + Counter] = InBuff[j++];
alan82914 1:3643ef2599cd 96 Counter++;
alan82914 1:3643ef2599cd 97 }while(Status_Packet_Array[1] > Counter); // Read Parameter(s) into array
alan82914 1:3643ef2599cd 98
alan82914 1:3643ef2599cd 99 Status_Packet_Array[Counter + 4] = InBuff[j++]; // Read Check sum
alan82914 1:3643ef2599cd 100 RxState++;
alan82914 1:3643ef2599cd 101 }
alan82914 1:3643ef2599cd 102 } break;
alan82914 1:3643ef2599cd 103 } //switch
alan82914 1:3643ef2599cd 104 }
alan82914 1:3643ef2599cd 105 }//while..
alan82914 1:3643ef2599cd 106 timer.stop();
alan82914 1:3643ef2599cd 107 // debugStatusframe();
alan82914 1:3643ef2599cd 108
alan82914 1:3643ef2599cd 109 return 0x00;
alan82914 1:3643ef2599cd 110 }
alan82914 1:3643ef2599cd 111
alan82914 1:3643ef2599cd 112
alan82914 1:3643ef2599cd 113
alan82914 1:3643ef2599cd 114 //-------------------------------------------------------------------------------------------------------------------------------
alan82914 1:3643ef2599cd 115 // Public Methods
alan82914 1:3643ef2599cd 116 //-------------------------------------------------------------------------------------------------------------------------------
alan82914 1:3643ef2599cd 117
alan82914 1:3643ef2599cd 118 DynamixelClass::DynamixelClass(int baud, PinName D_Pin, PinName tx, PinName rx){
alan82914 1:3643ef2599cd 119 servoSerial=new Serial(tx, rx);
alan82914 1:3643ef2599cd 120 servoSerial->baud(baud);
alan82914 1:3643ef2599cd 121 servoSerialDir= new DigitalOut(D_Pin);
alan82914 1:3643ef2599cd 122 servoSerialDir->write(0);
alan82914 1:3643ef2599cd 123 led2=new DigitalOut(LED2);
alan82914 1:3643ef2599cd 124
alan82914 1:3643ef2599cd 125 }
alan82914 1:3643ef2599cd 126
alan82914 1:3643ef2599cd 127 DynamixelClass::~DynamixelClass(){
alan82914 1:3643ef2599cd 128 if(servoSerial != NULL)
alan82914 1:3643ef2599cd 129 delete servoSerial;
alan82914 1:3643ef2599cd 130 }
alan82914 1:3643ef2599cd 131
alan82914 1:3643ef2599cd 132 unsigned int DynamixelClass::reset(unsigned char ID){
alan82914 1:3643ef2599cd 133
alan82914 1:3643ef2599cd 134 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 135 Instruction_Packet_Array[1] = RESET_LENGTH;
alan82914 1:3643ef2599cd 136 Instruction_Packet_Array[2] = COMMAND_RESET;
alan82914 1:3643ef2599cd 137 Instruction_Packet_Array[3] = ~(ID + RESET_LENGTH + COMMAND_RESET); //Checksum;
alan82914 1:3643ef2599cd 138
alan82914 1:3643ef2599cd 139 if (servoSerial->readable())
alan82914 1:3643ef2599cd 140 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 141
alan82914 1:3643ef2599cd 142 transmitInstructionPacket();
alan82914 1:3643ef2599cd 143
alan82914 1:3643ef2599cd 144 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
alan82914 1:3643ef2599cd 145 return (0x00);
alan82914 1:3643ef2599cd 146 }else{
alan82914 1:3643ef2599cd 147 readStatusPacket();
alan82914 1:3643ef2599cd 148 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 149 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 150 }else{
alan82914 1:3643ef2599cd 151 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 152 }
alan82914 1:3643ef2599cd 153 }
alan82914 1:3643ef2599cd 154 }
alan82914 1:3643ef2599cd 155
alan82914 1:3643ef2599cd 156 void DynamixelClass::NewBaudRate(int baud) {
alan82914 1:3643ef2599cd 157 //Change the baudrate on then MBED
alan82914 1:3643ef2599cd 158 int Baudrate_BPS = 0;
alan82914 1:3643ef2599cd 159 Baudrate_BPS =(int) 2000000 / (baud + 1); // Calculate Baudrate as ber "Robotis e-manual"
alan82914 1:3643ef2599cd 160 servoSerial->baud(Baudrate_BPS);
alan82914 1:3643ef2599cd 161 }
alan82914 1:3643ef2599cd 162
alan82914 1:3643ef2599cd 163 unsigned int DynamixelClass::ping(unsigned char ID){
alan82914 1:3643ef2599cd 164
alan82914 1:3643ef2599cd 165 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 166 Instruction_Packet_Array[1] = PING_LENGTH;
alan82914 1:3643ef2599cd 167 Instruction_Packet_Array[2] = COMMAND_PING;
alan82914 1:3643ef2599cd 168 Instruction_Packet_Array[3] = ~(ID + PING_LENGTH + COMMAND_PING);
alan82914 1:3643ef2599cd 169
alan82914 1:3643ef2599cd 170 if (servoSerial->readable())
alan82914 1:3643ef2599cd 171 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 172
alan82914 1:3643ef2599cd 173 transmitInstructionPacket();
alan82914 1:3643ef2599cd 174 readStatusPacket();
alan82914 1:3643ef2599cd 175
alan82914 1:3643ef2599cd 176 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 177 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 178 }else{
alan82914 1:3643ef2599cd 179 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 180 }
alan82914 1:3643ef2599cd 181 }
alan82914 1:3643ef2599cd 182
alan82914 1:3643ef2599cd 183 unsigned int DynamixelClass::setStatusPaketReturnDelay(unsigned char ID,unsigned char ReturnDelay){
alan82914 1:3643ef2599cd 184
alan82914 1:3643ef2599cd 185 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 186 Instruction_Packet_Array[1] = SET_RETURN_LENGTH;
alan82914 1:3643ef2599cd 187 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 188 Instruction_Packet_Array[3] = EEPROM_RETURN_DELAY_TIME;
alan82914 1:3643ef2599cd 189 Instruction_Packet_Array[4] = (char) (ReturnDelay/2);
alan82914 1:3643ef2599cd 190 Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_DELAY_TIME + (char)(ReturnDelay/2));
alan82914 1:3643ef2599cd 191
alan82914 1:3643ef2599cd 192 if (servoSerial->readable())
alan82914 1:3643ef2599cd 193 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 194
alan82914 1:3643ef2599cd 195 transmitInstructionPacket();
alan82914 1:3643ef2599cd 196
alan82914 1:3643ef2599cd 197 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
alan82914 1:3643ef2599cd 198 return (0x00);
alan82914 1:3643ef2599cd 199 }else{
alan82914 1:3643ef2599cd 200 readStatusPacket();
alan82914 1:3643ef2599cd 201 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 202 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 203 }else{
alan82914 1:3643ef2599cd 204 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 205 }
alan82914 1:3643ef2599cd 206 }
alan82914 1:3643ef2599cd 207
alan82914 1:3643ef2599cd 208 }
alan82914 1:3643ef2599cd 209
alan82914 1:3643ef2599cd 210 unsigned int DynamixelClass::setID(unsigned char ID, unsigned char New_ID){
alan82914 1:3643ef2599cd 211
alan82914 1:3643ef2599cd 212 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 213 Instruction_Packet_Array[1] = SET_ID_LENGTH;
alan82914 1:3643ef2599cd 214 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 215 Instruction_Packet_Array[3] = EEPROM_ID;
alan82914 1:3643ef2599cd 216 Instruction_Packet_Array[4] = New_ID;
alan82914 1:3643ef2599cd 217 Instruction_Packet_Array[5] = ~(ID + SET_ID_LENGTH + COMMAND_WRITE_DATA+ EEPROM_ID + New_ID);
alan82914 1:3643ef2599cd 218
alan82914 1:3643ef2599cd 219 if (servoSerial->readable())
alan82914 1:3643ef2599cd 220 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 221
alan82914 1:3643ef2599cd 222 transmitInstructionPacket();
alan82914 1:3643ef2599cd 223
alan82914 1:3643ef2599cd 224 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
alan82914 1:3643ef2599cd 225 return (0x00);
alan82914 1:3643ef2599cd 226 }else{
alan82914 1:3643ef2599cd 227 readStatusPacket();
alan82914 1:3643ef2599cd 228 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 229 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 230 }else{
alan82914 1:3643ef2599cd 231 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 232 }
alan82914 1:3643ef2599cd 233 }
alan82914 1:3643ef2599cd 234 }
alan82914 1:3643ef2599cd 235
alan82914 1:3643ef2599cd 236 unsigned int DynamixelClass::setBaudRate(unsigned char ID, long Baud){
alan82914 1:3643ef2599cd 237
alan82914 1:3643ef2599cd 238 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 239 Instruction_Packet_Array[1] = SET_BD_LENGTH;
alan82914 1:3643ef2599cd 240 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 241 Instruction_Packet_Array[3] = EEPROM_BAUD_RATE;
alan82914 1:3643ef2599cd 242 // if (Baud > 2250000){
alan82914 1:3643ef2599cd 243 if (Baud >= 1000000){
alan82914 1:3643ef2599cd 244 switch (Baud){
alan82914 1:3643ef2599cd 245 case 2250000:
alan82914 1:3643ef2599cd 246 Instruction_Packet_Array[4] = 0xFA;
alan82914 1:3643ef2599cd 247 break;
alan82914 1:3643ef2599cd 248 case 2500000:
alan82914 1:3643ef2599cd 249 Instruction_Packet_Array[4] = 0xFB;
alan82914 1:3643ef2599cd 250 break;
alan82914 1:3643ef2599cd 251 case 3000000:
alan82914 1:3643ef2599cd 252 Instruction_Packet_Array[4] = 0xFC;
alan82914 1:3643ef2599cd 253 break;
alan82914 1:3643ef2599cd 254 case 1000000:
alan82914 1:3643ef2599cd 255 Instruction_Packet_Array[4] = 0x01;
alan82914 1:3643ef2599cd 256 }
alan82914 1:3643ef2599cd 257 }else{
alan82914 1:3643ef2599cd 258 Instruction_Packet_Array[4] = (char)((2000000/Baud) - 1);
alan82914 1:3643ef2599cd 259 }
alan82914 1:3643ef2599cd 260 Instruction_Packet_Array[5] = ~(ID + SET_BD_LENGTH + COMMAND_WRITE_DATA + EEPROM_BAUD_RATE + (char)((2000000/Baud) - 1) );
alan82914 1:3643ef2599cd 261
alan82914 1:3643ef2599cd 262 if (servoSerial->readable())
alan82914 1:3643ef2599cd 263 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 264
alan82914 1:3643ef2599cd 265 transmitInstructionPacket();
alan82914 1:3643ef2599cd 266
alan82914 1:3643ef2599cd 267 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
alan82914 1:3643ef2599cd 268 return (0x00);
alan82914 1:3643ef2599cd 269 }else{
alan82914 1:3643ef2599cd 270 readStatusPacket();
alan82914 1:3643ef2599cd 271 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 272 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 273 }else{
alan82914 1:3643ef2599cd 274 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 275 }
alan82914 1:3643ef2599cd 276 }
alan82914 1:3643ef2599cd 277 }
alan82914 1:3643ef2599cd 278
alan82914 1:3643ef2599cd 279 unsigned int DynamixelClass::setMaxTorque( unsigned char ID, int Torque){
alan82914 1:3643ef2599cd 280
alan82914 1:3643ef2599cd 281 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 282 Instruction_Packet_Array[1] = SET_MAX_TORQUE_LENGTH;
alan82914 1:3643ef2599cd 283 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 284 Instruction_Packet_Array[3] = EEPROM_MAX_TORQUE_L ;
alan82914 1:3643ef2599cd 285 Instruction_Packet_Array[4] = (char)(Torque);
alan82914 1:3643ef2599cd 286 Instruction_Packet_Array[5] = (char)(Torque >> 8);
alan82914 1:3643ef2599cd 287 Instruction_Packet_Array[6] = ~(ID + SET_MAX_TORQUE_LENGTH + COMMAND_WRITE_DATA + EEPROM_MAX_TORQUE_L + (char)(Torque) + (char)(Torque >> 8));
alan82914 1:3643ef2599cd 288
alan82914 1:3643ef2599cd 289 if (servoSerial->readable())
alan82914 1:3643ef2599cd 290 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 291
alan82914 1:3643ef2599cd 292 transmitInstructionPacket();
alan82914 1:3643ef2599cd 293
alan82914 1:3643ef2599cd 294 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
alan82914 1:3643ef2599cd 295 return (0x00);
alan82914 1:3643ef2599cd 296 }else{
alan82914 1:3643ef2599cd 297 readStatusPacket();
alan82914 1:3643ef2599cd 298 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 299 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 300 }else{
alan82914 1:3643ef2599cd 301 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 302 }
alan82914 1:3643ef2599cd 303 }
alan82914 1:3643ef2599cd 304 }
alan82914 1:3643ef2599cd 305
alan82914 1:3643ef2599cd 306 unsigned int DynamixelClass::setHoldingTorque(unsigned char ID, bool Set){
alan82914 1:3643ef2599cd 307
alan82914 1:3643ef2599cd 308 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 309 Instruction_Packet_Array[1] = SET_HOLDING_TORQUE_LENGTH;
alan82914 1:3643ef2599cd 310 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 311 Instruction_Packet_Array[3] = RAM_TORQUE_ENABLE;
alan82914 1:3643ef2599cd 312 Instruction_Packet_Array[4] = Set;
alan82914 1:3643ef2599cd 313 Instruction_Packet_Array[5] = ~(ID + SET_HOLDING_TORQUE_LENGTH + COMMAND_WRITE_DATA + RAM_TORQUE_ENABLE + Set);
alan82914 1:3643ef2599cd 314
alan82914 1:3643ef2599cd 315 if (servoSerial->readable())
alan82914 1:3643ef2599cd 316 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 317
alan82914 1:3643ef2599cd 318 transmitInstructionPacket();
alan82914 1:3643ef2599cd 319
alan82914 1:3643ef2599cd 320 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
alan82914 1:3643ef2599cd 321 return (0x00);
alan82914 1:3643ef2599cd 322 }else{
alan82914 1:3643ef2599cd 323 readStatusPacket();
alan82914 1:3643ef2599cd 324 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 325 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 326 }else{
alan82914 1:3643ef2599cd 327 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 328 }
alan82914 1:3643ef2599cd 329 }
alan82914 1:3643ef2599cd 330 }
alan82914 1:3643ef2599cd 331
alan82914 1:3643ef2599cd 332 unsigned int DynamixelClass::setAlarmShutdown(unsigned char ID,unsigned char Set){
alan82914 1:3643ef2599cd 333
alan82914 1:3643ef2599cd 334 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 335 Instruction_Packet_Array[1] = SET_ALARM_LENGTH;
alan82914 1:3643ef2599cd 336 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 337 Instruction_Packet_Array[3] = EEPROM_ALARM_SHUTDOWN;
alan82914 1:3643ef2599cd 338 Instruction_Packet_Array[4] = Set;
alan82914 1:3643ef2599cd 339 Instruction_Packet_Array[5] = ~(ID + SET_ALARM_LENGTH + COMMAND_WRITE_DATA + EEPROM_ALARM_SHUTDOWN + Set);
alan82914 1:3643ef2599cd 340
alan82914 1:3643ef2599cd 341 if (servoSerial->readable())
alan82914 1:3643ef2599cd 342 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 343
alan82914 1:3643ef2599cd 344 transmitInstructionPacket();
alan82914 1:3643ef2599cd 345
alan82914 1:3643ef2599cd 346 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
alan82914 1:3643ef2599cd 347 return (0x00);
alan82914 1:3643ef2599cd 348 }else{
alan82914 1:3643ef2599cd 349 readStatusPacket();
alan82914 1:3643ef2599cd 350 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 351 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 352 }else{
alan82914 1:3643ef2599cd 353 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 354 }
alan82914 1:3643ef2599cd 355 }
alan82914 1:3643ef2599cd 356
alan82914 1:3643ef2599cd 357 }
alan82914 1:3643ef2599cd 358
alan82914 1:3643ef2599cd 359 unsigned int DynamixelClass::setStatusPaket(unsigned char ID,unsigned char Set){
alan82914 1:3643ef2599cd 360
alan82914 1:3643ef2599cd 361 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 362 Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH;
alan82914 1:3643ef2599cd 363 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 364 Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL;
alan82914 1:3643ef2599cd 365 Instruction_Packet_Array[4] = Set;
alan82914 1:3643ef2599cd 366 Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set);
alan82914 1:3643ef2599cd 367
alan82914 1:3643ef2599cd 368 Status_Return_Value = Set;
alan82914 1:3643ef2599cd 369
alan82914 1:3643ef2599cd 370 if (servoSerial->readable())
alan82914 1:3643ef2599cd 371 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 372
alan82914 1:3643ef2599cd 373 transmitInstructionPacket();
alan82914 1:3643ef2599cd 374
alan82914 1:3643ef2599cd 375 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
alan82914 1:3643ef2599cd 376 return (0x00);
alan82914 1:3643ef2599cd 377 }else{
alan82914 1:3643ef2599cd 378 readStatusPacket();
alan82914 1:3643ef2599cd 379 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 380 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 381 }else{
alan82914 1:3643ef2599cd 382 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 383 }
alan82914 1:3643ef2599cd 384 }
alan82914 1:3643ef2599cd 385
alan82914 1:3643ef2599cd 386 }
alan82914 1:3643ef2599cd 387
alan82914 1:3643ef2599cd 388
alan82914 1:3643ef2599cd 389 unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){
alan82914 1:3643ef2599cd 390
alan82914 1:3643ef2599cd 391 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 392 Instruction_Packet_Array[1] = SET_MODE_LENGTH;
alan82914 1:3643ef2599cd 393 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 394 Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L;
alan82914 1:3643ef2599cd 395 if ( Dynamixel_Mode == WHEEL) { // Set WHEEL mode, this is done by setting both the clockwise and anti-clockwise angle limits to ZERO
alan82914 1:3643ef2599cd 396 Instruction_Packet_Array[4] = 0x00;
alan82914 1:3643ef2599cd 397 Instruction_Packet_Array[5] = 0x00;
alan82914 1:3643ef2599cd 398 Instruction_Packet_Array[6] = 0x00;
alan82914 1:3643ef2599cd 399 Instruction_Packet_Array[7] = 0x00;
alan82914 1:3643ef2599cd 400 Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L);
alan82914 1:3643ef2599cd 401 }else { // Else set SERVO mode
alan82914 1:3643ef2599cd 402 Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit);
alan82914 1:3643ef2599cd 403 Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8);
alan82914 1:3643ef2599cd 404 Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit);
alan82914 1:3643ef2599cd 405 Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8);
alan82914 1:3643ef2599cd 406 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));
alan82914 1:3643ef2599cd 407 }
alan82914 1:3643ef2599cd 408
alan82914 1:3643ef2599cd 409
alan82914 1:3643ef2599cd 410 if (servoSerial->readable())
alan82914 1:3643ef2599cd 411 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 412
alan82914 1:3643ef2599cd 413 transmitInstructionPacket();
alan82914 1:3643ef2599cd 414
alan82914 1:3643ef2599cd 415 if (Status_Return_Value == ALL){
alan82914 1:3643ef2599cd 416 readStatusPacket();
alan82914 1:3643ef2599cd 417 if (Status_Packet_Array[2] != 0){
alan82914 1:3643ef2599cd 418 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 419 }
alan82914 1:3643ef2599cd 420
alan82914 1:3643ef2599cd 421 }
alan82914 1:3643ef2599cd 422 return 0x00; //if no errors
alan82914 1:3643ef2599cd 423 }
alan82914 1:3643ef2599cd 424
alan82914 1:3643ef2599cd 425 unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){
alan82914 1:3643ef2599cd 426
alan82914 1:3643ef2599cd 427 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 428 Instruction_Packet_Array[1] = SET_PUNCH_LENGTH;
alan82914 1:3643ef2599cd 429 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 430 Instruction_Packet_Array[3] = RAM_PUNCH_L;
alan82914 1:3643ef2599cd 431 Instruction_Packet_Array[4] = (char)(Punch);
alan82914 1:3643ef2599cd 432 Instruction_Packet_Array[5] = (char)(Punch >> 8);
alan82914 1:3643ef2599cd 433 Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) );
alan82914 1:3643ef2599cd 434
alan82914 1:3643ef2599cd 435 if (servoSerial->readable())
alan82914 1:3643ef2599cd 436 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 437
alan82914 1:3643ef2599cd 438 transmitInstructionPacket();
alan82914 1:3643ef2599cd 439
alan82914 1:3643ef2599cd 440 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
alan82914 1:3643ef2599cd 441 return (0x00);
alan82914 1:3643ef2599cd 442 }else{
alan82914 1:3643ef2599cd 443 readStatusPacket();
alan82914 1:3643ef2599cd 444 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 445 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 446 }else{
alan82914 1:3643ef2599cd 447 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 448 }
alan82914 1:3643ef2599cd 449 }
alan82914 1:3643ef2599cd 450
alan82914 1:3643ef2599cd 451 }
alan82914 1:3643ef2599cd 452
alan82914 1:3643ef2599cd 453 unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){
alan82914 1:3643ef2599cd 454
alan82914 1:3643ef2599cd 455 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 456 Instruction_Packet_Array[1] = SET_PID_LENGTH;
alan82914 1:3643ef2599cd 457 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 458 Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN;
alan82914 1:3643ef2599cd 459 Instruction_Packet_Array[4] = P;
alan82914 1:3643ef2599cd 460 Instruction_Packet_Array[5] = I;
alan82914 1:3643ef2599cd 461 Instruction_Packet_Array[6] = D;
alan82914 1:3643ef2599cd 462 Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D );
alan82914 1:3643ef2599cd 463
alan82914 1:3643ef2599cd 464 if (servoSerial->readable())
alan82914 1:3643ef2599cd 465 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 466
alan82914 1:3643ef2599cd 467 transmitInstructionPacket();
alan82914 1:3643ef2599cd 468
alan82914 1:3643ef2599cd 469 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
alan82914 1:3643ef2599cd 470 return (0x00);
alan82914 1:3643ef2599cd 471 }else{
alan82914 1:3643ef2599cd 472 readStatusPacket();
alan82914 1:3643ef2599cd 473 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 474 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 475 }else{
alan82914 1:3643ef2599cd 476 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 477 }
alan82914 1:3643ef2599cd 478 }
alan82914 1:3643ef2599cd 479 }
alan82914 1:3643ef2599cd 480
alan82914 1:3643ef2599cd 481 unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){
alan82914 1:3643ef2599cd 482
alan82914 1:3643ef2599cd 483 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 484 Instruction_Packet_Array[1] = SET_TEMP_LENGTH;
alan82914 1:3643ef2599cd 485 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 486 Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE;
alan82914 1:3643ef2599cd 487 Instruction_Packet_Array[4] = temp;
alan82914 1:3643ef2599cd 488 Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp);
alan82914 1:3643ef2599cd 489
alan82914 1:3643ef2599cd 490 if (servoSerial->readable())
alan82914 1:3643ef2599cd 491 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 492
alan82914 1:3643ef2599cd 493 transmitInstructionPacket();
alan82914 1:3643ef2599cd 494
alan82914 1:3643ef2599cd 495 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
alan82914 1:3643ef2599cd 496 return (0x00);
alan82914 1:3643ef2599cd 497 }else{
alan82914 1:3643ef2599cd 498 readStatusPacket();
alan82914 1:3643ef2599cd 499 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 500 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 501 }else{
alan82914 1:3643ef2599cd 502 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 503 }
alan82914 1:3643ef2599cd 504 }
alan82914 1:3643ef2599cd 505 }
alan82914 1:3643ef2599cd 506
alan82914 1:3643ef2599cd 507 unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){
alan82914 1:3643ef2599cd 508
alan82914 1:3643ef2599cd 509 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 510 Instruction_Packet_Array[1] = SET_VOLT_LENGTH;
alan82914 1:3643ef2599cd 511 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 512 Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE;
alan82914 1:3643ef2599cd 513 Instruction_Packet_Array[4] = Volt_L;
alan82914 1:3643ef2599cd 514 Instruction_Packet_Array[5] = Volt_H;
alan82914 1:3643ef2599cd 515 Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H);
alan82914 1:3643ef2599cd 516
alan82914 1:3643ef2599cd 517 if (servoSerial->readable())
alan82914 1:3643ef2599cd 518 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 519
alan82914 1:3643ef2599cd 520 transmitInstructionPacket();
alan82914 1:3643ef2599cd 521
alan82914 1:3643ef2599cd 522 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
alan82914 1:3643ef2599cd 523 return (0x00);
alan82914 1:3643ef2599cd 524 }else{
alan82914 1:3643ef2599cd 525 readStatusPacket();
alan82914 1:3643ef2599cd 526 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 527 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 528 }else{
alan82914 1:3643ef2599cd 529 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 530 }
alan82914 1:3643ef2599cd 531 }
alan82914 1:3643ef2599cd 532 }
alan82914 1:3643ef2599cd 533
alan82914 1:3643ef2599cd 534 unsigned int DynamixelClass::servo(unsigned char ID,unsigned int Position,unsigned int Speed){
alan82914 1:3643ef2599cd 535
alan82914 1:3643ef2599cd 536 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 537 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
alan82914 1:3643ef2599cd 538 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 539 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
alan82914 1:3643ef2599cd 540 Instruction_Packet_Array[4] = (char)(Position);
alan82914 1:3643ef2599cd 541 Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8);
alan82914 1:3643ef2599cd 542 Instruction_Packet_Array[6] = (char)(Speed);
alan82914 1:3643ef2599cd 543 Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8);
alan82914 1:3643ef2599cd 544 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));
alan82914 1:3643ef2599cd 545
alan82914 1:3643ef2599cd 546 if (servoSerial->readable())
alan82914 1:3643ef2599cd 547 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 548
alan82914 1:3643ef2599cd 549 transmitInstructionPacket();
alan82914 1:3643ef2599cd 550
alan82914 1:3643ef2599cd 551
alan82914 1:3643ef2599cd 552 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
alan82914 1:3643ef2599cd 553 return (0x00);
alan82914 1:3643ef2599cd 554 }else{
alan82914 1:3643ef2599cd 555 readStatusPacket();
alan82914 1:3643ef2599cd 556 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 557 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 558 }else{
alan82914 1:3643ef2599cd 559 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 560 }
alan82914 1:3643ef2599cd 561 }
alan82914 1:3643ef2599cd 562 }
alan82914 1:3643ef2599cd 563
alan82914 1:3643ef2599cd 564 unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){
alan82914 1:3643ef2599cd 565
alan82914 1:3643ef2599cd 566 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 567 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
alan82914 1:3643ef2599cd 568 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
alan82914 1:3643ef2599cd 569 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
alan82914 1:3643ef2599cd 570 Instruction_Packet_Array[4] = (char)(Position);
alan82914 1:3643ef2599cd 571 Instruction_Packet_Array[5] = (char)(Position >> 8);
alan82914 1:3643ef2599cd 572 Instruction_Packet_Array[6] = (char)(Speed);
alan82914 1:3643ef2599cd 573 Instruction_Packet_Array[7] = (char)(Speed >> 8);
alan82914 1:3643ef2599cd 574 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));
alan82914 1:3643ef2599cd 575
alan82914 1:3643ef2599cd 576 if (servoSerial->readable())
alan82914 1:3643ef2599cd 577 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 578
alan82914 1:3643ef2599cd 579 transmitInstructionPacket();
alan82914 1:3643ef2599cd 580
alan82914 1:3643ef2599cd 581 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
alan82914 1:3643ef2599cd 582 return (0x00);
alan82914 1:3643ef2599cd 583 }else{
alan82914 1:3643ef2599cd 584 readStatusPacket();
alan82914 1:3643ef2599cd 585 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 586 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 587 }else{
alan82914 1:3643ef2599cd 588 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 589 }
alan82914 1:3643ef2599cd 590 }
alan82914 1:3643ef2599cd 591 }
alan82914 1:3643ef2599cd 592
alan82914 1:3643ef2599cd 593 unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){
alan82914 1:3643ef2599cd 594
alan82914 1:3643ef2599cd 595 char Speed_H,Speed_L;
alan82914 1:3643ef2599cd 596 Speed_L = Speed;
alan82914 1:3643ef2599cd 597 if (Rotation == 0){ // Move Left
alan82914 1:3643ef2599cd 598 Speed_H = Speed >> 8;
alan82914 1:3643ef2599cd 599 }
alan82914 1:3643ef2599cd 600 else if (Rotation == 1){ // Move Right
alan82914 1:3643ef2599cd 601 Speed_H = (Speed >> 8)+4;
alan82914 1:3643ef2599cd 602 }
alan82914 1:3643ef2599cd 603
alan82914 1:3643ef2599cd 604 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 605 Instruction_Packet_Array[1] = WHEEL_LENGTH;
alan82914 1:3643ef2599cd 606 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 607 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
alan82914 1:3643ef2599cd 608 Instruction_Packet_Array[4] = Speed_L;
alan82914 1:3643ef2599cd 609 Instruction_Packet_Array[5] = Speed_H;
alan82914 1:3643ef2599cd 610 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
alan82914 1:3643ef2599cd 611
alan82914 1:3643ef2599cd 612 if (servoSerial->readable())
alan82914 1:3643ef2599cd 613 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 614
alan82914 1:3643ef2599cd 615 transmitInstructionPacket();
alan82914 1:3643ef2599cd 616
alan82914 1:3643ef2599cd 617 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
alan82914 1:3643ef2599cd 618 return (0x00);
alan82914 1:3643ef2599cd 619 }else{
alan82914 1:3643ef2599cd 620 readStatusPacket();
alan82914 1:3643ef2599cd 621 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 622 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 623 }else{
alan82914 1:3643ef2599cd 624 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 625 }
alan82914 1:3643ef2599cd 626 }
alan82914 1:3643ef2599cd 627
alan82914 1:3643ef2599cd 628 }
alan82914 1:3643ef2599cd 629
alan82914 1:3643ef2599cd 630 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){
alan82914 1:3643ef2599cd 631
alan82914 1:3643ef2599cd 632 char Speed1_H,Speed1_L;
alan82914 1:3643ef2599cd 633 Speed1_L = Speed1;
alan82914 1:3643ef2599cd 634 if (Dir1 == 0){ // Move Left
alan82914 1:3643ef2599cd 635 Speed1_H = Speed1 >> 8;
alan82914 1:3643ef2599cd 636 }
alan82914 1:3643ef2599cd 637 else if (Dir1 == 1) // Move Right
alan82914 1:3643ef2599cd 638 {
alan82914 1:3643ef2599cd 639 Speed1_H = (Speed1 >> 8)+4;
alan82914 1:3643ef2599cd 640 }
alan82914 1:3643ef2599cd 641
alan82914 1:3643ef2599cd 642 char Speed2_H,Speed2_L;
alan82914 1:3643ef2599cd 643 Speed2_L = Speed2;
alan82914 1:3643ef2599cd 644 if (Dir2 == 0){ // Move Left
alan82914 1:3643ef2599cd 645 Speed2_H = Speed2 >> 8;
alan82914 1:3643ef2599cd 646 }
alan82914 1:3643ef2599cd 647 else if (Dir2 == 1) // Move Right
alan82914 1:3643ef2599cd 648 {
alan82914 1:3643ef2599cd 649 Speed2_H = (Speed2 >> 8)+4;
alan82914 1:3643ef2599cd 650 }
alan82914 1:3643ef2599cd 651
alan82914 1:3643ef2599cd 652 char Speed3_H,Speed3_L;
alan82914 1:3643ef2599cd 653 Speed3_L = Speed3;
alan82914 1:3643ef2599cd 654 if (Dir3 == 0){ // Move Left
alan82914 1:3643ef2599cd 655 Speed3_H = Speed3 >> 8;
alan82914 1:3643ef2599cd 656 }
alan82914 1:3643ef2599cd 657 else if (Dir3 == 1) // Move Right
alan82914 1:3643ef2599cd 658 {
alan82914 1:3643ef2599cd 659 Speed3_H = (Speed3 >> 8)+4;
alan82914 1:3643ef2599cd 660 }
alan82914 1:3643ef2599cd 661
alan82914 1:3643ef2599cd 662 Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos
alan82914 1:3643ef2599cd 663 Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH;
alan82914 1:3643ef2599cd 664 Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE;
alan82914 1:3643ef2599cd 665 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
alan82914 1:3643ef2599cd 666 Instruction_Packet_Array[4] = SYNC_DATA_LENGTH;
alan82914 1:3643ef2599cd 667 Instruction_Packet_Array[5] = ID1;
alan82914 1:3643ef2599cd 668 Instruction_Packet_Array[6] = Speed1_L;
alan82914 1:3643ef2599cd 669 Instruction_Packet_Array[7] = Speed1_H;
alan82914 1:3643ef2599cd 670 Instruction_Packet_Array[8] = ID2;
alan82914 1:3643ef2599cd 671 Instruction_Packet_Array[9] = Speed2_L;
alan82914 1:3643ef2599cd 672 Instruction_Packet_Array[10] = Speed2_H;
alan82914 1:3643ef2599cd 673 Instruction_Packet_Array[11] = ID3;
alan82914 1:3643ef2599cd 674 Instruction_Packet_Array[12] = Speed3_L;
alan82914 1:3643ef2599cd 675 Instruction_Packet_Array[13] = Speed3_H;
alan82914 1:3643ef2599cd 676 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));
alan82914 1:3643ef2599cd 677
alan82914 1:3643ef2599cd 678 transmitInstructionPacket();
alan82914 1:3643ef2599cd 679
alan82914 1:3643ef2599cd 680 }
alan82914 1:3643ef2599cd 681
alan82914 1:3643ef2599cd 682 unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){
alan82914 1:3643ef2599cd 683
alan82914 1:3643ef2599cd 684 char Speed_H,Speed_L;
alan82914 1:3643ef2599cd 685 Speed_L = Speed;
alan82914 1:3643ef2599cd 686 if (Dir == 0){ // Move Left
alan82914 1:3643ef2599cd 687 Speed_H = Speed >> 8;
alan82914 1:3643ef2599cd 688 }
alan82914 1:3643ef2599cd 689 else if (Dir == 1) // Move Right
alan82914 1:3643ef2599cd 690 {
alan82914 1:3643ef2599cd 691 Speed_H = (Speed >> 8)+4;
alan82914 1:3643ef2599cd 692 }
alan82914 1:3643ef2599cd 693
alan82914 1:3643ef2599cd 694 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 695 Instruction_Packet_Array[1] = WHEEL_LENGTH;
alan82914 1:3643ef2599cd 696 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
alan82914 1:3643ef2599cd 697 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
alan82914 1:3643ef2599cd 698 Instruction_Packet_Array[4] = Speed_L;
alan82914 1:3643ef2599cd 699 Instruction_Packet_Array[5] = Speed_H;
alan82914 1:3643ef2599cd 700 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
alan82914 1:3643ef2599cd 701
alan82914 1:3643ef2599cd 702 if (servoSerial->readable())
alan82914 1:3643ef2599cd 703 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 704
alan82914 1:3643ef2599cd 705 transmitInstructionPacket();
alan82914 1:3643ef2599cd 706
alan82914 1:3643ef2599cd 707 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
alan82914 1:3643ef2599cd 708 return (0x00);
alan82914 1:3643ef2599cd 709 }else{
alan82914 1:3643ef2599cd 710 readStatusPacket();
alan82914 1:3643ef2599cd 711 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 712 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 713 }else{
alan82914 1:3643ef2599cd 714 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 715 }
alan82914 1:3643ef2599cd 716 }
alan82914 1:3643ef2599cd 717
alan82914 1:3643ef2599cd 718 }
alan82914 1:3643ef2599cd 719
alan82914 1:3643ef2599cd 720 unsigned int DynamixelClass::action(unsigned char ID){
alan82914 1:3643ef2599cd 721
alan82914 1:3643ef2599cd 722 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 723 Instruction_Packet_Array[1] = RESET_LENGTH;
alan82914 1:3643ef2599cd 724 Instruction_Packet_Array[2] = COMMAND_ACTION;
alan82914 1:3643ef2599cd 725 Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION);
alan82914 1:3643ef2599cd 726
alan82914 1:3643ef2599cd 727 if (servoSerial->readable())
alan82914 1:3643ef2599cd 728 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 729
alan82914 1:3643ef2599cd 730 transmitInstructionPacket();
alan82914 1:3643ef2599cd 731
alan82914 1:3643ef2599cd 732 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
alan82914 1:3643ef2599cd 733 return (0x00);
alan82914 1:3643ef2599cd 734 }else{
alan82914 1:3643ef2599cd 735 readStatusPacket();
alan82914 1:3643ef2599cd 736 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 737 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 738 }else{
alan82914 1:3643ef2599cd 739 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 740 }
alan82914 1:3643ef2599cd 741 }
alan82914 1:3643ef2599cd 742 }
alan82914 1:3643ef2599cd 743
alan82914 1:3643ef2599cd 744 unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){
alan82914 1:3643ef2599cd 745
alan82914 1:3643ef2599cd 746 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 747 Instruction_Packet_Array[1] = LED_LENGTH;
alan82914 1:3643ef2599cd 748 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 749 Instruction_Packet_Array[3] = RAM_LED;
alan82914 1:3643ef2599cd 750 Instruction_Packet_Array[4] = Status;
alan82914 1:3643ef2599cd 751 Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status);
alan82914 1:3643ef2599cd 752
alan82914 1:3643ef2599cd 753 if (servoSerial->readable())
alan82914 1:3643ef2599cd 754 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 755
alan82914 1:3643ef2599cd 756 transmitInstructionPacket();
alan82914 1:3643ef2599cd 757
alan82914 1:3643ef2599cd 758 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
alan82914 1:3643ef2599cd 759 return (0x00);
alan82914 1:3643ef2599cd 760 }else{
alan82914 1:3643ef2599cd 761 readStatusPacket();
alan82914 1:3643ef2599cd 762 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 763 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 764 }else{
alan82914 1:3643ef2599cd 765 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 766 }
alan82914 1:3643ef2599cd 767 }
alan82914 1:3643ef2599cd 768 }
alan82914 1:3643ef2599cd 769
alan82914 1:3643ef2599cd 770 unsigned int DynamixelClass::readTemperature(unsigned char ID){
alan82914 1:3643ef2599cd 771
alan82914 1:3643ef2599cd 772 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 773 Instruction_Packet_Array[1] = READ_TEMP_LENGTH;
alan82914 1:3643ef2599cd 774 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 775 Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE;
alan82914 1:3643ef2599cd 776 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 777 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 778
alan82914 1:3643ef2599cd 779 if (servoSerial->readable())
alan82914 1:3643ef2599cd 780 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 781
alan82914 1:3643ef2599cd 782 transmitInstructionPacket();
alan82914 1:3643ef2599cd 783 readStatusPacket();
alan82914 1:3643ef2599cd 784
alan82914 1:3643ef2599cd 785 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 786 return Status_Packet_Array[3];
alan82914 1:3643ef2599cd 787 }else{
alan82914 1:3643ef2599cd 788 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 789 }
alan82914 1:3643ef2599cd 790 }
alan82914 1:3643ef2599cd 791
alan82914 1:3643ef2599cd 792 unsigned int DynamixelClass::readPosition(unsigned char ID){
alan82914 1:3643ef2599cd 793
alan82914 1:3643ef2599cd 794 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 795 Instruction_Packet_Array[1] = READ_POS_LENGTH;
alan82914 1:3643ef2599cd 796 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 797 Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
alan82914 1:3643ef2599cd 798 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
alan82914 1:3643ef2599cd 799 Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH);
alan82914 1:3643ef2599cd 800
alan82914 1:3643ef2599cd 801 if (servoSerial->readable())
alan82914 1:3643ef2599cd 802 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 803
alan82914 1:3643ef2599cd 804 transmitInstructionPacket();
alan82914 1:3643ef2599cd 805 readStatusPacket();
alan82914 1:3643ef2599cd 806
alan82914 1:3643ef2599cd 807 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value // If there is no status packet error return value
alan82914 1:3643ef2599cd 808 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value
alan82914 1:3643ef2599cd 809 }else{
alan82914 1:3643ef2599cd 810 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 811 }
alan82914 1:3643ef2599cd 812 }
alan82914 1:3643ef2599cd 813
alan82914 1:3643ef2599cd 814 unsigned int DynamixelClass::readLoad(unsigned char ID){
alan82914 1:3643ef2599cd 815
alan82914 1:3643ef2599cd 816 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 817 Instruction_Packet_Array[1] = READ_LOAD_LENGTH;
alan82914 1:3643ef2599cd 818 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 819 Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L;
alan82914 1:3643ef2599cd 820 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
alan82914 1:3643ef2599cd 821 Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH);
alan82914 1:3643ef2599cd 822
alan82914 1:3643ef2599cd 823 if (servoSerial->readable())
alan82914 1:3643ef2599cd 824 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 825
alan82914 1:3643ef2599cd 826 transmitInstructionPacket();
alan82914 1:3643ef2599cd 827 readStatusPacket();
alan82914 1:3643ef2599cd 828
alan82914 1:3643ef2599cd 829 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 830 return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value
alan82914 1:3643ef2599cd 831 }else{
alan82914 1:3643ef2599cd 832 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 833 }
alan82914 1:3643ef2599cd 834 }
alan82914 1:3643ef2599cd 835
alan82914 1:3643ef2599cd 836 unsigned int DynamixelClass::readSpeed(unsigned char ID){
alan82914 1:3643ef2599cd 837
alan82914 1:3643ef2599cd 838 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 839 Instruction_Packet_Array[1] = READ_SPEED_LENGTH;
alan82914 1:3643ef2599cd 840 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 841 Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L;
alan82914 1:3643ef2599cd 842 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
alan82914 1:3643ef2599cd 843 Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH);
alan82914 1:3643ef2599cd 844
alan82914 1:3643ef2599cd 845 if (servoSerial->readable())
alan82914 1:3643ef2599cd 846 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 847
alan82914 1:3643ef2599cd 848 transmitInstructionPacket();
alan82914 1:3643ef2599cd 849 readStatusPacket();
alan82914 1:3643ef2599cd 850
alan82914 1:3643ef2599cd 851 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 852 return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value
alan82914 1:3643ef2599cd 853 }else{
alan82914 1:3643ef2599cd 854 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 855 }
alan82914 1:3643ef2599cd 856 }
alan82914 1:3643ef2599cd 857
alan82914 1:3643ef2599cd 858
alan82914 1:3643ef2599cd 859 unsigned int DynamixelClass::readVoltage(unsigned char ID){
alan82914 1:3643ef2599cd 860
alan82914 1:3643ef2599cd 861 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 862 Instruction_Packet_Array[1] = READ_VOLT_LENGTH;
alan82914 1:3643ef2599cd 863 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 864 Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE;
alan82914 1:3643ef2599cd 865 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 866 Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 867
alan82914 1:3643ef2599cd 868 if (servoSerial->readable())
alan82914 1:3643ef2599cd 869 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 870
alan82914 1:3643ef2599cd 871 transmitInstructionPacket();
alan82914 1:3643ef2599cd 872 readStatusPacket();
alan82914 1:3643ef2599cd 873
alan82914 1:3643ef2599cd 874 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 875 return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage)
alan82914 1:3643ef2599cd 876 }else{
alan82914 1:3643ef2599cd 877 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 878 }
alan82914 1:3643ef2599cd 879 }
alan82914 1:3643ef2599cd 880
alan82914 1:3643ef2599cd 881 unsigned int DynamixelClass::checkRegister(unsigned char ID){
alan82914 1:3643ef2599cd 882
alan82914 1:3643ef2599cd 883 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 884 Instruction_Packet_Array[1] = READ_REGISTER_LENGTH;
alan82914 1:3643ef2599cd 885 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 886 Instruction_Packet_Array[3] = RAM_REGISTER;
alan82914 1:3643ef2599cd 887 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 888 Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 889
alan82914 1:3643ef2599cd 890 if (servoSerial->readable())
alan82914 1:3643ef2599cd 891 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 892
alan82914 1:3643ef2599cd 893 transmitInstructionPacket();
alan82914 1:3643ef2599cd 894 readStatusPacket();
alan82914 1:3643ef2599cd 895
alan82914 1:3643ef2599cd 896 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 897 return (Status_Packet_Array[3]); // Return register value
alan82914 1:3643ef2599cd 898 }else{
alan82914 1:3643ef2599cd 899 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 900 }
alan82914 1:3643ef2599cd 901 }
alan82914 1:3643ef2599cd 902
alan82914 1:3643ef2599cd 903 unsigned int DynamixelClass::checkMovement(unsigned char ID){
alan82914 1:3643ef2599cd 904
alan82914 1:3643ef2599cd 905 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 906 Instruction_Packet_Array[1] = READ_MOVING_LENGTH;
alan82914 1:3643ef2599cd 907 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 908 Instruction_Packet_Array[3] = RAM_MOVING;
alan82914 1:3643ef2599cd 909 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 910 Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 911
alan82914 1:3643ef2599cd 912 if (servoSerial->readable())
alan82914 1:3643ef2599cd 913 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 914
alan82914 1:3643ef2599cd 915 transmitInstructionPacket();
alan82914 1:3643ef2599cd 916 readStatusPacket();
alan82914 1:3643ef2599cd 917
alan82914 1:3643ef2599cd 918 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 919 return (Status_Packet_Array[3]); // Return movement value
alan82914 1:3643ef2599cd 920 }else{
alan82914 1:3643ef2599cd 921 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 922 }
alan82914 1:3643ef2599cd 923 }
alan82914 1:3643ef2599cd 924
alan82914 1:3643ef2599cd 925 unsigned int DynamixelClass::checkLock(unsigned char ID){
alan82914 1:3643ef2599cd 926
alan82914 1:3643ef2599cd 927 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 928 Instruction_Packet_Array[1] = READ_LOCK_LENGTH;
alan82914 1:3643ef2599cd 929 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 930 Instruction_Packet_Array[3] = RAM_LOCK;
alan82914 1:3643ef2599cd 931 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 932 Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 933
alan82914 1:3643ef2599cd 934 if (servoSerial->readable())
alan82914 1:3643ef2599cd 935 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 936
alan82914 1:3643ef2599cd 937 transmitInstructionPacket();
alan82914 1:3643ef2599cd 938 readStatusPacket();
alan82914 1:3643ef2599cd 939
alan82914 1:3643ef2599cd 940 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 941 return (Status_Packet_Array[3]); // Return Lock value
alan82914 1:3643ef2599cd 942 }else{
alan82914 1:3643ef2599cd 943 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 944 }
alan82914 1:3643ef2599cd 945 }
alan82914 1:3643ef2599cd 946
alan82914 1:3643ef2599cd 947 unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){
alan82914 1:3643ef2599cd 948
alan82914 1:3643ef2599cd 949 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 950 Instruction_Packet_Array[1] = 0x04;
alan82914 1:3643ef2599cd 951 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 952 Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE;
alan82914 1:3643ef2599cd 953 Instruction_Packet_Array[4] = Status;
alan82914 1:3643ef2599cd 954 Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status);
alan82914 1:3643ef2599cd 955
alan82914 1:3643ef2599cd 956 if (servoSerial->readable())
alan82914 1:3643ef2599cd 957 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 958
alan82914 1:3643ef2599cd 959 transmitInstructionPacket();
alan82914 1:3643ef2599cd 960
alan82914 1:3643ef2599cd 961 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
alan82914 1:3643ef2599cd 962 return (0x00);
alan82914 1:3643ef2599cd 963 }else{
alan82914 1:3643ef2599cd 964 readStatusPacket();
alan82914 1:3643ef2599cd 965 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 966 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 967 }else{
alan82914 1:3643ef2599cd 968 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 969 }
alan82914 1:3643ef2599cd 970 }
alan82914 1:3643ef2599cd 971 }
alan82914 1:3643ef2599cd 972
alan82914 1:3643ef2599cd 973 unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){
alan82914 1:3643ef2599cd 974
alan82914 1:3643ef2599cd 975
alan82914 1:3643ef2599cd 976 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 977 Instruction_Packet_Array[1] = 0x05;
alan82914 1:3643ef2599cd 978 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
alan82914 1:3643ef2599cd 979 Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L;
alan82914 1:3643ef2599cd 980 Instruction_Packet_Array[4] = (char)(Torque);
alan82914 1:3643ef2599cd 981 Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8);
alan82914 1:3643ef2599cd 982 Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) );
alan82914 1:3643ef2599cd 983
alan82914 1:3643ef2599cd 984 if (servoSerial->readable())
alan82914 1:3643ef2599cd 985 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 986
alan82914 1:3643ef2599cd 987 transmitInstructionPacket();
alan82914 1:3643ef2599cd 988
alan82914 1:3643ef2599cd 989 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
alan82914 1:3643ef2599cd 990 return (0x00);
alan82914 1:3643ef2599cd 991 }else{
alan82914 1:3643ef2599cd 992 readStatusPacket();
alan82914 1:3643ef2599cd 993 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 994 return (Status_Packet_Array[0]); // Return SERVO ID
alan82914 1:3643ef2599cd 995 }else{
alan82914 1:3643ef2599cd 996 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 997 }
alan82914 1:3643ef2599cd 998 }
alan82914 1:3643ef2599cd 999
alan82914 1:3643ef2599cd 1000 }
alan82914 1:3643ef2599cd 1001
alan82914 1:3643ef2599cd 1002 unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){
alan82914 1:3643ef2599cd 1003
alan82914 1:3643ef2599cd 1004 Instruction_Packet_Array[0] = ID;
alan82914 1:3643ef2599cd 1005 Instruction_Packet_Array[1] = 0x04;
alan82914 1:3643ef2599cd 1006 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
alan82914 1:3643ef2599cd 1007 Instruction_Packet_Array[3] = Register;
alan82914 1:3643ef2599cd 1008 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
alan82914 1:3643ef2599cd 1009 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH);
alan82914 1:3643ef2599cd 1010
alan82914 1:3643ef2599cd 1011 if (servoSerial->readable())
alan82914 1:3643ef2599cd 1012 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
alan82914 1:3643ef2599cd 1013
alan82914 1:3643ef2599cd 1014 transmitInstructionPacket();
alan82914 1:3643ef2599cd 1015 readStatusPacket();
alan82914 1:3643ef2599cd 1016
alan82914 1:3643ef2599cd 1017 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
alan82914 1:3643ef2599cd 1018 return Status_Packet_Array[3];
alan82914 1:3643ef2599cd 1019 }else{
alan82914 1:3643ef2599cd 1020 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
alan82914 1:3643ef2599cd 1021 }
alan82914 1:3643ef2599cd 1022 }
alan82914 1:3643ef2599cd 1023
alan82914 1:3643ef2599cd 1024