Chris LU / Mbed 2 deprecated Self_Riding_Bicycle

2018/06/08

Dependencies:   LSM9DS0 mbed

Mx28.cpp@0:bf9bf4b7625f, 2018-06-08 (annotated)

Committer:
cpul5338
Date:
Fri Jun 08 14:11:49 2018 +0000
Revision:
0:bf9bf4b7625f
2018/06/08

Who changed what in which revision?

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