Gert Lauritsen / MX28

Object for MX28 with rs485

Mx28.cpp@0:45d73665e91f, 2014-09-10 (annotated)

Committer:
gert_lauritsen
Date:
Wed Sep 10 10:36:12 2014 +0000
Revision:
0:45d73665e91f
Child:
1:c866ce96ceb3
Function version of MX28 servo libery for the RS485 version;

Who changed what in which revision?

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