YiLun Cheng / MX28

20180723

Fork of MX28 by LDSC_Robotics_TAs

Mx28.cpp@5:ca1e55903d09, 2018-07-23 (annotated)

Committer:
alan82914
Date:
Mon Jul 23 08:35:17 2018 +0000
Revision:
5:ca1e55903d09
Parent:
4:e00500b45c78 
20180723

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