YiLun Cheng / MX28

20180723

Fork of MX28 by LDSC_Robotics_TAs

Mx28.cpp@4:e00500b45c78, 2017-02-14 (annotated)

Committer:
hoting
Date:
Tue Feb 14 07:01:19 2017 +0000
Revision:
4:e00500b45c78
Parent:
2:2e32ee9f0e51
Child:
5:ca1e55903d09
system identification for dynamixel

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
gert_lauritsen 0:45d73665e91f 360 unsigned int DynamixelClass::setStatusPaket(unsigned char ID,unsigned char Set){
gert_lauritsen 0:45d73665e91f 361
gert_lauritsen 0:45d73665e91f 362 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 363 Instruction_Packet_Array[1] = SET_RETURN_LEVEL_LENGTH;
gert_lauritsen 0:45d73665e91f 364 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 365 Instruction_Packet_Array[3] = EEPROM_RETURN_LEVEL;
gert_lauritsen 0:45d73665e91f 366 Instruction_Packet_Array[4] = Set;
gert_lauritsen 0:45d73665e91f 367 Instruction_Packet_Array[5] = ~(ID + SET_RETURN_LEVEL_LENGTH + COMMAND_WRITE_DATA + EEPROM_RETURN_LEVEL + Set);
gert_lauritsen 0:45d73665e91f 368
gert_lauritsen 0:45d73665e91f 369 Status_Return_Value = Set;
gert_lauritsen 0:45d73665e91f 370
gert_lauritsen 0:45d73665e91f 371 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 372 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 373
gert_lauritsen 0:45d73665e91f 374 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 375
gert_lauritsen 0:45d73665e91f 376 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 377 return (0x00);
gert_lauritsen 0:45d73665e91f 378 }else{
gert_lauritsen 0:45d73665e91f 379 readStatusPacket();
gert_lauritsen 0:45d73665e91f 380 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 381 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 382 }else{
gert_lauritsen 0:45d73665e91f 383 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 384 }
gert_lauritsen 0:45d73665e91f 385 }
gert_lauritsen 0:45d73665e91f 386
gert_lauritsen 0:45d73665e91f 387 }
gert_lauritsen 0:45d73665e91f 388
gert_lauritsen 0:45d73665e91f 389
gert_lauritsen 0:45d73665e91f 390 unsigned int DynamixelClass::setMode(unsigned char ID, bool Dynamixel_Mode, unsigned int Dynamixel_CW_Limit,unsigned int Dynamixel_CCW_Limit){
gert_lauritsen 0:45d73665e91f 391
gert_lauritsen 0:45d73665e91f 392 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 393 Instruction_Packet_Array[1] = SET_MODE_LENGTH;
gert_lauritsen 0:45d73665e91f 394 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 395 Instruction_Packet_Array[3] = EEPROM_CW_ANGLE_LIMIT_L;
gert_lauritsen 0:45d73665e91f 396 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 397 Instruction_Packet_Array[4] = 0x00;
gert_lauritsen 0:45d73665e91f 398 Instruction_Packet_Array[5] = 0x00;
gert_lauritsen 0:45d73665e91f 399 Instruction_Packet_Array[6] = 0x00;
gert_lauritsen 0:45d73665e91f 400 Instruction_Packet_Array[7] = 0x00;
gert_lauritsen 0:45d73665e91f 401 Instruction_Packet_Array[8] = ~(ID + SET_MODE_LENGTH + COMMAND_WRITE_DATA + EEPROM_CW_ANGLE_LIMIT_L);
gert_lauritsen 0:45d73665e91f 402 }else { // Else set SERVO mode
gert_lauritsen 0:45d73665e91f 403 Instruction_Packet_Array[4] = (char)(Dynamixel_CW_Limit);
gert_lauritsen 0:45d73665e91f 404 Instruction_Packet_Array[5] = (char)((Dynamixel_CW_Limit & 0x0F00) >> 8);
gert_lauritsen 0:45d73665e91f 405 Instruction_Packet_Array[6] = (char)(Dynamixel_CCW_Limit);
gert_lauritsen 0:45d73665e91f 406 Instruction_Packet_Array[7] = (char)((Dynamixel_CCW_Limit & 0x0F00) >> 8);
gert_lauritsen 0:45d73665e91f 407 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 408 }
gert_lauritsen 0:45d73665e91f 409
gert_lauritsen 0:45d73665e91f 410
gert_lauritsen 0:45d73665e91f 411 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 412 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 413
gert_lauritsen 0:45d73665e91f 414 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 415
gert_lauritsen 0:45d73665e91f 416 if (Status_Return_Value == ALL){
gert_lauritsen 0:45d73665e91f 417 readStatusPacket();
gert_lauritsen 0:45d73665e91f 418 if (Status_Packet_Array[2] != 0){
gert_lauritsen 0:45d73665e91f 419 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 420 }
gert_lauritsen 0:45d73665e91f 421
gert_lauritsen 0:45d73665e91f 422 }
gert_lauritsen 0:45d73665e91f 423 return 0x00; //if no errors
gert_lauritsen 0:45d73665e91f 424 }
gert_lauritsen 0:45d73665e91f 425
gert_lauritsen 0:45d73665e91f 426 unsigned int DynamixelClass::setPunch(unsigned char ID,unsigned int Punch){
gert_lauritsen 0:45d73665e91f 427
gert_lauritsen 0:45d73665e91f 428 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 429 Instruction_Packet_Array[1] = SET_PUNCH_LENGTH;
gert_lauritsen 0:45d73665e91f 430 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 431 Instruction_Packet_Array[3] = RAM_PUNCH_L;
gert_lauritsen 0:45d73665e91f 432 Instruction_Packet_Array[4] = (char)(Punch);
gert_lauritsen 0:45d73665e91f 433 Instruction_Packet_Array[5] = (char)(Punch >> 8);
gert_lauritsen 0:45d73665e91f 434 Instruction_Packet_Array[6] = ~(ID + SET_PUNCH_LENGTH + COMMAND_WRITE_DATA + RAM_PUNCH_L + (char)(Punch) + (char)(Punch >> 8) );
gert_lauritsen 0:45d73665e91f 435
gert_lauritsen 0:45d73665e91f 436 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 437 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 438
gert_lauritsen 0:45d73665e91f 439 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 440
gert_lauritsen 0:45d73665e91f 441 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 442 return (0x00);
gert_lauritsen 0:45d73665e91f 443 }else{
gert_lauritsen 0:45d73665e91f 444 readStatusPacket();
gert_lauritsen 0:45d73665e91f 445 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 446 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 447 }else{
gert_lauritsen 0:45d73665e91f 448 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 449 }
gert_lauritsen 0:45d73665e91f 450 }
gert_lauritsen 0:45d73665e91f 451
gert_lauritsen 0:45d73665e91f 452 }
gert_lauritsen 0:45d73665e91f 453
gert_lauritsen 0:45d73665e91f 454 unsigned int DynamixelClass::setPID(unsigned char ID ,unsigned char P,unsigned char I,unsigned char D){
gert_lauritsen 0:45d73665e91f 455
gert_lauritsen 0:45d73665e91f 456 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 457 Instruction_Packet_Array[1] = SET_PID_LENGTH;
gert_lauritsen 0:45d73665e91f 458 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 459 Instruction_Packet_Array[3] = RAM_PROPORTIONAL_GAIN;
gert_lauritsen 0:45d73665e91f 460 Instruction_Packet_Array[4] = P;
gert_lauritsen 0:45d73665e91f 461 Instruction_Packet_Array[5] = I;
gert_lauritsen 0:45d73665e91f 462 Instruction_Packet_Array[6] = D;
gert_lauritsen 0:45d73665e91f 463 Instruction_Packet_Array[7] = ~(ID + SET_PID_LENGTH + COMMAND_WRITE_DATA + RAM_PROPORTIONAL_GAIN + P + I + D );
gert_lauritsen 0:45d73665e91f 464
gert_lauritsen 0:45d73665e91f 465 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 466 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 467
gert_lauritsen 0:45d73665e91f 468 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 469
gert_lauritsen 0:45d73665e91f 470 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 471 return (0x00);
gert_lauritsen 0:45d73665e91f 472 }else{
gert_lauritsen 0:45d73665e91f 473 readStatusPacket();
gert_lauritsen 0:45d73665e91f 474 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 475 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 476 }else{
gert_lauritsen 0:45d73665e91f 477 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 478 }
gert_lauritsen 0:45d73665e91f 479 }
gert_lauritsen 0:45d73665e91f 480 }
gert_lauritsen 0:45d73665e91f 481
gert_lauritsen 0:45d73665e91f 482 unsigned int DynamixelClass::setTemp(unsigned char ID,unsigned char temp){
gert_lauritsen 0:45d73665e91f 483
gert_lauritsen 0:45d73665e91f 484 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 485 Instruction_Packet_Array[1] = SET_TEMP_LENGTH;
gert_lauritsen 0:45d73665e91f 486 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 487 Instruction_Packet_Array[3] = EEPROM_LIMIT_TEMPERATURE;
gert_lauritsen 0:45d73665e91f 488 Instruction_Packet_Array[4] = temp;
gert_lauritsen 0:45d73665e91f 489 Instruction_Packet_Array[5] = ~(ID + SET_TEMP_LENGTH + COMMAND_WRITE_DATA + EEPROM_LIMIT_TEMPERATURE + temp);
gert_lauritsen 0:45d73665e91f 490
gert_lauritsen 0:45d73665e91f 491 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 492 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 493
gert_lauritsen 0:45d73665e91f 494 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 495
gert_lauritsen 0:45d73665e91f 496 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 497 return (0x00);
gert_lauritsen 0:45d73665e91f 498 }else{
gert_lauritsen 0:45d73665e91f 499 readStatusPacket();
gert_lauritsen 0:45d73665e91f 500 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 501 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 502 }else{
gert_lauritsen 0:45d73665e91f 503 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 504 }
gert_lauritsen 0:45d73665e91f 505 }
gert_lauritsen 0:45d73665e91f 506 }
gert_lauritsen 0:45d73665e91f 507
gert_lauritsen 0:45d73665e91f 508 unsigned int DynamixelClass::setVoltage(unsigned char ID,unsigned char Volt_L, unsigned char Volt_H){
gert_lauritsen 0:45d73665e91f 509
gert_lauritsen 0:45d73665e91f 510 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 511 Instruction_Packet_Array[1] = SET_VOLT_LENGTH;
gert_lauritsen 0:45d73665e91f 512 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 513 Instruction_Packet_Array[3] = EEPROM_LOW_LIMIT_VOLTAGE;
gert_lauritsen 0:45d73665e91f 514 Instruction_Packet_Array[4] = Volt_L;
gert_lauritsen 0:45d73665e91f 515 Instruction_Packet_Array[5] = Volt_H;
gert_lauritsen 0:45d73665e91f 516 Instruction_Packet_Array[6] = ~(ID + SET_VOLT_LENGTH + COMMAND_WRITE_DATA + EEPROM_LOW_LIMIT_VOLTAGE + Volt_L + Volt_H);
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::servo(unsigned char ID,unsigned int Position,unsigned int Speed){
gert_lauritsen 0:45d73665e91f 536
gert_lauritsen 0:45d73665e91f 537 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 538 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
gert_lauritsen 0:45d73665e91f 539 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 540 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
gert_lauritsen 0:45d73665e91f 541 Instruction_Packet_Array[4] = (char)(Position);
gert_lauritsen 0:45d73665e91f 542 Instruction_Packet_Array[5] = (char)((Position & 0x0F00) >> 8);
gert_lauritsen 0:45d73665e91f 543 Instruction_Packet_Array[6] = (char)(Speed);
gert_lauritsen 0:45d73665e91f 544 Instruction_Packet_Array[7] = (char)((Speed & 0x0F00) >> 8);
gert_lauritsen 0:45d73665e91f 545 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 546
gert_lauritsen 0:45d73665e91f 547 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 548 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 549
gert_lauritsen 0:45d73665e91f 550 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 551
gert_lauritsen 0:45d73665e91f 552
gert_lauritsen 0:45d73665e91f 553 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 554 return (0x00);
gert_lauritsen 0:45d73665e91f 555 }else{
gert_lauritsen 0:45d73665e91f 556 readStatusPacket();
gert_lauritsen 0:45d73665e91f 557 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 558 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 559 }else{
gert_lauritsen 0:45d73665e91f 560 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 561 }
gert_lauritsen 0:45d73665e91f 562 }
gert_lauritsen 0:45d73665e91f 563 }
gert_lauritsen 0:45d73665e91f 564
gert_lauritsen 0:45d73665e91f 565 unsigned int DynamixelClass::servoPreload(unsigned char ID,unsigned int Position,unsigned int Speed){
gert_lauritsen 0:45d73665e91f 566
gert_lauritsen 0:45d73665e91f 567 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 568 Instruction_Packet_Array[1] = SERVO_GOAL_LENGTH;
gert_lauritsen 0:45d73665e91f 569 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 570 Instruction_Packet_Array[3] = RAM_GOAL_POSITION_L;
gert_lauritsen 0:45d73665e91f 571 Instruction_Packet_Array[4] = (char)(Position);
gert_lauritsen 0:45d73665e91f 572 Instruction_Packet_Array[5] = (char)(Position >> 8);
gert_lauritsen 0:45d73665e91f 573 Instruction_Packet_Array[6] = (char)(Speed);
gert_lauritsen 0:45d73665e91f 574 Instruction_Packet_Array[7] = (char)(Speed >> 8);
gert_lauritsen 0:45d73665e91f 575 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 576
gert_lauritsen 0:45d73665e91f 577 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 578 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 579
gert_lauritsen 0:45d73665e91f 580 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 581
gert_lauritsen 0:45d73665e91f 582 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 583 return (0x00);
gert_lauritsen 0:45d73665e91f 584 }else{
gert_lauritsen 0:45d73665e91f 585 readStatusPacket();
gert_lauritsen 0:45d73665e91f 586 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 587 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 588 }else{
gert_lauritsen 0:45d73665e91f 589 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 590 }
gert_lauritsen 0:45d73665e91f 591 }
gert_lauritsen 0:45d73665e91f 592 }
gert_lauritsen 0:45d73665e91f 593
gert_lauritsen 0:45d73665e91f 594 unsigned int DynamixelClass::wheel(unsigned char ID, bool Rotation,unsigned int Speed){
gert_lauritsen 0:45d73665e91f 595
gert_lauritsen 0:45d73665e91f 596 char Speed_H,Speed_L;
gert_lauritsen 0:45d73665e91f 597 Speed_L = Speed;
gert_lauritsen 0:45d73665e91f 598 if (Rotation == 0){ // Move Left
gert_lauritsen 0:45d73665e91f 599 Speed_H = Speed >> 8;
gert_lauritsen 0:45d73665e91f 600 }
gert_lauritsen 0:45d73665e91f 601 else if (Rotation == 1){ // Move Right
gert_lauritsen 0:45d73665e91f 602 Speed_H = (Speed >> 8)+4;
gert_lauritsen 0:45d73665e91f 603 }
gert_lauritsen 0:45d73665e91f 604
gert_lauritsen 0:45d73665e91f 605 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 606 Instruction_Packet_Array[1] = WHEEL_LENGTH;
gert_lauritsen 0:45d73665e91f 607 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 608 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
gert_lauritsen 0:45d73665e91f 609 Instruction_Packet_Array[4] = Speed_L;
gert_lauritsen 0:45d73665e91f 610 Instruction_Packet_Array[5] = Speed_H;
gert_lauritsen 0:45d73665e91f 611 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
gert_lauritsen 0:45d73665e91f 612
gert_lauritsen 0:45d73665e91f 613 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 614 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 615
gert_lauritsen 0:45d73665e91f 616 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 617
gert_lauritsen 0:45d73665e91f 618 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 619 return (0x00);
gert_lauritsen 0:45d73665e91f 620 }else{
gert_lauritsen 0:45d73665e91f 621 readStatusPacket();
gert_lauritsen 0:45d73665e91f 622 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 623 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 624 }else{
gert_lauritsen 0:45d73665e91f 625 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 626 }
gert_lauritsen 0:45d73665e91f 627 }
gert_lauritsen 0:45d73665e91f 628
gert_lauritsen 0:45d73665e91f 629 }
gert_lauritsen 0:45d73665e91f 630
gert_lauritsen 0:45d73665e91f 631 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 632
gert_lauritsen 0:45d73665e91f 633 char Speed1_H,Speed1_L;
gert_lauritsen 0:45d73665e91f 634 Speed1_L = Speed1;
gert_lauritsen 0:45d73665e91f 635 if (Dir1 == 0){ // Move Left
gert_lauritsen 0:45d73665e91f 636 Speed1_H = Speed1 >> 8;
gert_lauritsen 0:45d73665e91f 637 }
gert_lauritsen 0:45d73665e91f 638 else if (Dir1 == 1) // Move Right
gert_lauritsen 0:45d73665e91f 639 {
gert_lauritsen 0:45d73665e91f 640 Speed1_H = (Speed1 >> 8)+4;
gert_lauritsen 0:45d73665e91f 641 }
gert_lauritsen 0:45d73665e91f 642
gert_lauritsen 0:45d73665e91f 643 char Speed2_H,Speed2_L;
gert_lauritsen 0:45d73665e91f 644 Speed2_L = Speed2;
gert_lauritsen 0:45d73665e91f 645 if (Dir2 == 0){ // Move Left
gert_lauritsen 0:45d73665e91f 646 Speed2_H = Speed2 >> 8;
gert_lauritsen 0:45d73665e91f 647 }
gert_lauritsen 0:45d73665e91f 648 else if (Dir2 == 1) // Move Right
gert_lauritsen 0:45d73665e91f 649 {
gert_lauritsen 0:45d73665e91f 650 Speed2_H = (Speed2 >> 8)+4;
gert_lauritsen 0:45d73665e91f 651 }
gert_lauritsen 0:45d73665e91f 652
gert_lauritsen 0:45d73665e91f 653 char Speed3_H,Speed3_L;
gert_lauritsen 0:45d73665e91f 654 Speed3_L = Speed3;
gert_lauritsen 0:45d73665e91f 655 if (Dir3 == 0){ // Move Left
gert_lauritsen 0:45d73665e91f 656 Speed3_H = Speed3 >> 8;
gert_lauritsen 0:45d73665e91f 657 }
gert_lauritsen 0:45d73665e91f 658 else if (Dir3 == 1) // Move Right
gert_lauritsen 0:45d73665e91f 659 {
gert_lauritsen 0:45d73665e91f 660 Speed3_H = (Speed3 >> 8)+4;
gert_lauritsen 0:45d73665e91f 661 }
gert_lauritsen 0:45d73665e91f 662
gert_lauritsen 0:45d73665e91f 663 Instruction_Packet_Array[0] = 0xFE; // When Writing a Sync comman you must address all(0xFE) servos
gert_lauritsen 0:45d73665e91f 664 Instruction_Packet_Array[1] = SYNC_LOAD_LENGTH;
gert_lauritsen 0:45d73665e91f 665 Instruction_Packet_Array[2] = COMMAND_SYNC_WRITE;
gert_lauritsen 0:45d73665e91f 666 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
gert_lauritsen 0:45d73665e91f 667 Instruction_Packet_Array[4] = SYNC_DATA_LENGTH;
gert_lauritsen 0:45d73665e91f 668 Instruction_Packet_Array[5] = ID1;
gert_lauritsen 0:45d73665e91f 669 Instruction_Packet_Array[6] = Speed1_L;
gert_lauritsen 0:45d73665e91f 670 Instruction_Packet_Array[7] = Speed1_H;
gert_lauritsen 0:45d73665e91f 671 Instruction_Packet_Array[8] = ID2;
gert_lauritsen 0:45d73665e91f 672 Instruction_Packet_Array[9] = Speed2_L;
gert_lauritsen 0:45d73665e91f 673 Instruction_Packet_Array[10] = Speed2_H;
gert_lauritsen 0:45d73665e91f 674 Instruction_Packet_Array[11] = ID3;
gert_lauritsen 0:45d73665e91f 675 Instruction_Packet_Array[12] = Speed3_L;
gert_lauritsen 0:45d73665e91f 676 Instruction_Packet_Array[13] = Speed3_H;
gert_lauritsen 0:45d73665e91f 677 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 678
gert_lauritsen 0:45d73665e91f 679 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 680
gert_lauritsen 0:45d73665e91f 681 }
gert_lauritsen 0:45d73665e91f 682
gert_lauritsen 0:45d73665e91f 683 unsigned int DynamixelClass::wheelPreload(unsigned char ID, bool Dir,unsigned int Speed){
gert_lauritsen 0:45d73665e91f 684
gert_lauritsen 0:45d73665e91f 685 char Speed_H,Speed_L;
gert_lauritsen 0:45d73665e91f 686 Speed_L = Speed;
gert_lauritsen 0:45d73665e91f 687 if (Dir == 0){ // Move Left
gert_lauritsen 0:45d73665e91f 688 Speed_H = Speed >> 8;
gert_lauritsen 0:45d73665e91f 689 }
gert_lauritsen 0:45d73665e91f 690 else if (Dir == 1) // Move Right
gert_lauritsen 0:45d73665e91f 691 {
gert_lauritsen 0:45d73665e91f 692 Speed_H = (Speed >> 8)+4;
gert_lauritsen 0:45d73665e91f 693 }
gert_lauritsen 0:45d73665e91f 694
gert_lauritsen 0:45d73665e91f 695 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 696 Instruction_Packet_Array[1] = WHEEL_LENGTH;
gert_lauritsen 0:45d73665e91f 697 Instruction_Packet_Array[2] = COMMAND_REG_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 698 Instruction_Packet_Array[3] = RAM_GOAL_SPEED_L;
gert_lauritsen 0:45d73665e91f 699 Instruction_Packet_Array[4] = Speed_L;
gert_lauritsen 0:45d73665e91f 700 Instruction_Packet_Array[5] = Speed_H;
gert_lauritsen 0:45d73665e91f 701 Instruction_Packet_Array[6] = ~(ID + WHEEL_LENGTH + COMMAND_REG_WRITE_DATA + RAM_GOAL_SPEED_L + Speed_L + Speed_H);
gert_lauritsen 0:45d73665e91f 702
gert_lauritsen 0:45d73665e91f 703 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 704 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 705
gert_lauritsen 0:45d73665e91f 706 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 707
gert_lauritsen 0:45d73665e91f 708 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 709 return (0x00);
gert_lauritsen 0:45d73665e91f 710 }else{
gert_lauritsen 0:45d73665e91f 711 readStatusPacket();
gert_lauritsen 0:45d73665e91f 712 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 713 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 714 }else{
gert_lauritsen 0:45d73665e91f 715 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 716 }
gert_lauritsen 0:45d73665e91f 717 }
gert_lauritsen 0:45d73665e91f 718
gert_lauritsen 0:45d73665e91f 719 }
gert_lauritsen 0:45d73665e91f 720
gert_lauritsen 0:45d73665e91f 721 unsigned int DynamixelClass::action(unsigned char ID){
gert_lauritsen 0:45d73665e91f 722
gert_lauritsen 0:45d73665e91f 723 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 724 Instruction_Packet_Array[1] = RESET_LENGTH;
gert_lauritsen 0:45d73665e91f 725 Instruction_Packet_Array[2] = COMMAND_ACTION;
gert_lauritsen 0:45d73665e91f 726 Instruction_Packet_Array[3] = ~(ID + ACTION_LENGTH + COMMAND_ACTION);
gert_lauritsen 0:45d73665e91f 727
gert_lauritsen 0:45d73665e91f 728 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 729 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 730
gert_lauritsen 0:45d73665e91f 731 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 732
gert_lauritsen 0:45d73665e91f 733 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 734 return (0x00);
gert_lauritsen 0:45d73665e91f 735 }else{
gert_lauritsen 0:45d73665e91f 736 readStatusPacket();
gert_lauritsen 0:45d73665e91f 737 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 738 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 739 }else{
gert_lauritsen 0:45d73665e91f 740 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 741 }
gert_lauritsen 0:45d73665e91f 742 }
gert_lauritsen 0:45d73665e91f 743 }
gert_lauritsen 0:45d73665e91f 744
gert_lauritsen 0:45d73665e91f 745 unsigned int DynamixelClass::ledState(unsigned char ID, bool Status){
gert_lauritsen 0:45d73665e91f 746
gert_lauritsen 0:45d73665e91f 747 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 748 Instruction_Packet_Array[1] = LED_LENGTH;
gert_lauritsen 0:45d73665e91f 749 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
gert_lauritsen 0:45d73665e91f 750 Instruction_Packet_Array[3] = RAM_LED;
gert_lauritsen 0:45d73665e91f 751 Instruction_Packet_Array[4] = Status;
gert_lauritsen 0:45d73665e91f 752 Instruction_Packet_Array[5] = ~(ID + LED_LENGTH + COMMAND_WRITE_DATA + RAM_LED + Status);
gert_lauritsen 0:45d73665e91f 753
gert_lauritsen 0:45d73665e91f 754 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 755 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 756
gert_lauritsen 0:45d73665e91f 757 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 758
gert_lauritsen 0:45d73665e91f 759 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 760 return (0x00);
gert_lauritsen 0:45d73665e91f 761 }else{
gert_lauritsen 0:45d73665e91f 762 readStatusPacket();
gert_lauritsen 0:45d73665e91f 763 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 764 return (Status_Packet_Array[0]); // Return SERVO ID
gert_lauritsen 0:45d73665e91f 765 }else{
gert_lauritsen 0:45d73665e91f 766 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 767 }
gert_lauritsen 0:45d73665e91f 768 }
gert_lauritsen 0:45d73665e91f 769 }
gert_lauritsen 0:45d73665e91f 770
gert_lauritsen 0:45d73665e91f 771 unsigned int DynamixelClass::readTemperature(unsigned char ID){
gert_lauritsen 0:45d73665e91f 772
gert_lauritsen 0:45d73665e91f 773 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 774 Instruction_Packet_Array[1] = READ_TEMP_LENGTH;
gert_lauritsen 0:45d73665e91f 775 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 776 Instruction_Packet_Array[3] = RAM_PRESENT_TEMPERATURE;
gert_lauritsen 0:45d73665e91f 777 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 778 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_TEMPERATURE + READ_ONE_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 779
gert_lauritsen 0:45d73665e91f 780 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 781 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 782
gert_lauritsen 0:45d73665e91f 783 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 784 readStatusPacket();
gert_lauritsen 0:45d73665e91f 785
gert_lauritsen 0:45d73665e91f 786 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 787 return Status_Packet_Array[3];
gert_lauritsen 0:45d73665e91f 788 }else{
gert_lauritsen 0:45d73665e91f 789 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 790 }
gert_lauritsen 0:45d73665e91f 791 }
gert_lauritsen 0:45d73665e91f 792
gert_lauritsen 0:45d73665e91f 793 unsigned int DynamixelClass::readPosition(unsigned char ID){
gert_lauritsen 0:45d73665e91f 794
gert_lauritsen 0:45d73665e91f 795 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 796 Instruction_Packet_Array[1] = READ_POS_LENGTH;
gert_lauritsen 0:45d73665e91f 797 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 798 Instruction_Packet_Array[3] = RAM_PRESENT_POSITION_L;
gert_lauritsen 0:45d73665e91f 799 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 800 Instruction_Packet_Array[5] = ~(ID + READ_POS_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_POSITION_L + READ_TWO_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 801
gert_lauritsen 0:45d73665e91f 802 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 803 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 804
gert_lauritsen 0:45d73665e91f 805 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 806 readStatusPacket();
gert_lauritsen 0:45d73665e91f 807
gert_lauritsen 0:45d73665e91f 808 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 809 return Status_Packet_Array[4] << 8 | Status_Packet_Array[3]; // Return present position value
gert_lauritsen 0:45d73665e91f 810 }else{
gert_lauritsen 0:45d73665e91f 811 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 812 }
gert_lauritsen 0:45d73665e91f 813 }
gert_lauritsen 0:45d73665e91f 814
gert_lauritsen 0:45d73665e91f 815 unsigned int DynamixelClass::readLoad(unsigned char ID){
gert_lauritsen 0:45d73665e91f 816
gert_lauritsen 0:45d73665e91f 817 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 818 Instruction_Packet_Array[1] = READ_LOAD_LENGTH;
gert_lauritsen 0:45d73665e91f 819 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 820 Instruction_Packet_Array[3] = RAM_PRESENT_LOAD_L;
gert_lauritsen 0:45d73665e91f 821 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 822 Instruction_Packet_Array[5] = ~(ID + READ_LOAD_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_LOAD_L + READ_TWO_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 823
gert_lauritsen 0:45d73665e91f 824 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 825 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 826
gert_lauritsen 0:45d73665e91f 827 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 828 readStatusPacket();
gert_lauritsen 0:45d73665e91f 829
gert_lauritsen 0:45d73665e91f 830 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 831 return ((Status_Packet_Array[4] << 8) | Status_Packet_Array[3]); // Return present load value
gert_lauritsen 0:45d73665e91f 832 }else{
gert_lauritsen 0:45d73665e91f 833 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 834 }
gert_lauritsen 0:45d73665e91f 835 }
gert_lauritsen 0:45d73665e91f 836
gert_lauritsen 0:45d73665e91f 837 unsigned int DynamixelClass::readSpeed(unsigned char ID){
gert_lauritsen 0:45d73665e91f 838
gert_lauritsen 0:45d73665e91f 839 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 840 Instruction_Packet_Array[1] = READ_SPEED_LENGTH;
gert_lauritsen 0:45d73665e91f 841 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 842 Instruction_Packet_Array[3] = RAM_PRESENT_SPEED_L;
gert_lauritsen 0:45d73665e91f 843 Instruction_Packet_Array[4] = READ_TWO_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 844 Instruction_Packet_Array[5] = ~(ID + READ_SPEED_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_SPEED_L + READ_TWO_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 845
gert_lauritsen 0:45d73665e91f 846 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 847 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 848
gert_lauritsen 0:45d73665e91f 849 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 850 readStatusPacket();
gert_lauritsen 0:45d73665e91f 851
gert_lauritsen 0:45d73665e91f 852 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 853 return (Status_Packet_Array[4] << 8) | Status_Packet_Array[3]; // Return present position value
gert_lauritsen 0:45d73665e91f 854 }else{
gert_lauritsen 0:45d73665e91f 855 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 856 }
gert_lauritsen 0:45d73665e91f 857 }
gert_lauritsen 0:45d73665e91f 858
gert_lauritsen 0:45d73665e91f 859
gert_lauritsen 0:45d73665e91f 860 unsigned int DynamixelClass::readVoltage(unsigned char ID){
gert_lauritsen 0:45d73665e91f 861
gert_lauritsen 0:45d73665e91f 862 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 863 Instruction_Packet_Array[1] = READ_VOLT_LENGTH;
gert_lauritsen 0:45d73665e91f 864 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 865 Instruction_Packet_Array[3] = RAM_PRESENT_VOLTAGE;
gert_lauritsen 0:45d73665e91f 866 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 867 Instruction_Packet_Array[5] = ~(ID + READ_VOLT_LENGTH + COMMAND_READ_DATA + RAM_PRESENT_VOLTAGE + READ_ONE_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 868
gert_lauritsen 0:45d73665e91f 869 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 870 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 871
gert_lauritsen 0:45d73665e91f 872 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 873 readStatusPacket();
gert_lauritsen 0:45d73665e91f 874
gert_lauritsen 0:45d73665e91f 875 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 876 return Status_Packet_Array[3]; // Return voltage value (value retured by Dynamixel is 10 times actual voltage)
gert_lauritsen 0:45d73665e91f 877 }else{
gert_lauritsen 0:45d73665e91f 878 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 879 }
gert_lauritsen 0:45d73665e91f 880 }
gert_lauritsen 0:45d73665e91f 881
gert_lauritsen 0:45d73665e91f 882 unsigned int DynamixelClass::checkRegister(unsigned char ID){
gert_lauritsen 0:45d73665e91f 883
gert_lauritsen 0:45d73665e91f 884 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 885 Instruction_Packet_Array[1] = READ_REGISTER_LENGTH;
gert_lauritsen 0:45d73665e91f 886 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 887 Instruction_Packet_Array[3] = RAM_REGISTER;
gert_lauritsen 0:45d73665e91f 888 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 889 Instruction_Packet_Array[5] = ~(ID + READ_REGISTER_LENGTH + COMMAND_READ_DATA + RAM_REGISTER + READ_ONE_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 890
gert_lauritsen 0:45d73665e91f 891 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 892 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 893
gert_lauritsen 0:45d73665e91f 894 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 895 readStatusPacket();
gert_lauritsen 0:45d73665e91f 896
gert_lauritsen 0:45d73665e91f 897 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 898 return (Status_Packet_Array[3]); // Return register value
gert_lauritsen 0:45d73665e91f 899 }else{
gert_lauritsen 0:45d73665e91f 900 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 901 }
gert_lauritsen 0:45d73665e91f 902 }
gert_lauritsen 0:45d73665e91f 903
gert_lauritsen 0:45d73665e91f 904 unsigned int DynamixelClass::checkMovement(unsigned char ID){
gert_lauritsen 0:45d73665e91f 905
gert_lauritsen 0:45d73665e91f 906 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 907 Instruction_Packet_Array[1] = READ_MOVING_LENGTH;
gert_lauritsen 0:45d73665e91f 908 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 909 Instruction_Packet_Array[3] = RAM_MOVING;
gert_lauritsen 0:45d73665e91f 910 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 911 Instruction_Packet_Array[5] = ~(ID + READ_MOVING_LENGTH + COMMAND_READ_DATA + RAM_MOVING + READ_ONE_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 912
gert_lauritsen 0:45d73665e91f 913 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 914 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 915
gert_lauritsen 0:45d73665e91f 916 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 917 readStatusPacket();
gert_lauritsen 0:45d73665e91f 918
gert_lauritsen 0:45d73665e91f 919 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 920 return (Status_Packet_Array[3]); // Return movement value
gert_lauritsen 0:45d73665e91f 921 }else{
gert_lauritsen 0:45d73665e91f 922 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 923 }
gert_lauritsen 0:45d73665e91f 924 }
gert_lauritsen 0:45d73665e91f 925
gert_lauritsen 0:45d73665e91f 926 unsigned int DynamixelClass::checkLock(unsigned char ID){
gert_lauritsen 0:45d73665e91f 927
gert_lauritsen 0:45d73665e91f 928 Instruction_Packet_Array[0] = ID;
gert_lauritsen 0:45d73665e91f 929 Instruction_Packet_Array[1] = READ_LOCK_LENGTH;
gert_lauritsen 0:45d73665e91f 930 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
gert_lauritsen 0:45d73665e91f 931 Instruction_Packet_Array[3] = RAM_LOCK;
gert_lauritsen 0:45d73665e91f 932 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
gert_lauritsen 0:45d73665e91f 933 Instruction_Packet_Array[5] = ~(ID + READ_LOCK_LENGTH + COMMAND_READ_DATA + RAM_LOCK + READ_ONE_BYTE_LENGTH);
gert_lauritsen 0:45d73665e91f 934
gert_lauritsen 0:45d73665e91f 935 if (servoSerial->readable())
gert_lauritsen 0:45d73665e91f 936 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 937
gert_lauritsen 0:45d73665e91f 938 transmitInstructionPacket();
gert_lauritsen 0:45d73665e91f 939 readStatusPacket();
gert_lauritsen 0:45d73665e91f 940
gert_lauritsen 1:c866ce96ceb3 941 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
gert_lauritsen 0:45d73665e91f 942 return (Status_Packet_Array[3]); // Return Lock value
gert_lauritsen 0:45d73665e91f 943 }else{
gert_lauritsen 0:45d73665e91f 944 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
gert_lauritsen 0:45d73665e91f 945 }
gert_lauritsen 0:45d73665e91f 946 }
gert_lauritsen 0:45d73665e91f 947
weisnail 2:2e32ee9f0e51 948 unsigned int DynamixelClass::torqueMode(unsigned char ID, bool Status){
weisnail 2:2e32ee9f0e51 949
weisnail 2:2e32ee9f0e51 950 Instruction_Packet_Array[0] = ID;
weisnail 2:2e32ee9f0e51 951 Instruction_Packet_Array[1] = 0x04;
weisnail 2:2e32ee9f0e51 952 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
weisnail 2:2e32ee9f0e51 953 Instruction_Packet_Array[3] = RAM_TORQUE_CONTROL_MODE_ENABLE;
weisnail 2:2e32ee9f0e51 954 Instruction_Packet_Array[4] = Status;
weisnail 2:2e32ee9f0e51 955 Instruction_Packet_Array[5] = ~(ID + 0x04 + COMMAND_WRITE_DATA + RAM_TORQUE_CONTROL_MODE_ENABLE + Status);
weisnail 2:2e32ee9f0e51 956
weisnail 2:2e32ee9f0e51 957 if (servoSerial->readable())
weisnail 2:2e32ee9f0e51 958 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
gert_lauritsen 0:45d73665e91f 959
weisnail 2:2e32ee9f0e51 960 transmitInstructionPacket();
weisnail 2:2e32ee9f0e51 961
weisnail 2:2e32ee9f0e51 962 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 963 return (0x00);
weisnail 2:2e32ee9f0e51 964 }else{
weisnail 2:2e32ee9f0e51 965 readStatusPacket();
weisnail 2:2e32ee9f0e51 966 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
weisnail 2:2e32ee9f0e51 967 return (Status_Packet_Array[0]); // Return SERVO ID
weisnail 2:2e32ee9f0e51 968 }else{
weisnail 2:2e32ee9f0e51 969 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
weisnail 2:2e32ee9f0e51 970 }
weisnail 2:2e32ee9f0e51 971 }
weisnail 2:2e32ee9f0e51 972 }
weisnail 2:2e32ee9f0e51 973
weisnail 2:2e32ee9f0e51 974 unsigned int DynamixelClass::torque(unsigned char ID,unsigned int Torque){
weisnail 2:2e32ee9f0e51 975
weisnail 2:2e32ee9f0e51 976
weisnail 2:2e32ee9f0e51 977 Instruction_Packet_Array[0] = ID;
weisnail 2:2e32ee9f0e51 978 Instruction_Packet_Array[1] = 0x05;
weisnail 2:2e32ee9f0e51 979 Instruction_Packet_Array[2] = COMMAND_WRITE_DATA;
weisnail 2:2e32ee9f0e51 980 Instruction_Packet_Array[3] = RAM_GOAL_TORQUE_L;
weisnail 2:2e32ee9f0e51 981 Instruction_Packet_Array[4] = (char)(Torque);
weisnail 2:2e32ee9f0e51 982 Instruction_Packet_Array[5] = (char)((Torque & 0x0F00) >> 8);
weisnail 2:2e32ee9f0e51 983 Instruction_Packet_Array[6] = ~(ID + 0x05 + COMMAND_WRITE_DATA + RAM_GOAL_TORQUE_L + (char)(Torque) + (char)((Torque & 0x0F00) >> 8) );
weisnail 2:2e32ee9f0e51 984
weisnail 2:2e32ee9f0e51 985 if (servoSerial->readable())
weisnail 2:2e32ee9f0e51 986 while (servoSerial->readable()) servoSerial->getc(); //emthy buffer
weisnail 2:2e32ee9f0e51 987
weisnail 2:2e32ee9f0e51 988 transmitInstructionPacket();
weisnail 2:2e32ee9f0e51 989
weisnail 2:2e32ee9f0e51 990 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 991 return (0x00);
weisnail 2:2e32ee9f0e51 992 }else{
weisnail 2:2e32ee9f0e51 993 readStatusPacket();
weisnail 2:2e32ee9f0e51 994 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
weisnail 2:2e32ee9f0e51 995 return (Status_Packet_Array[0]); // Return SERVO ID
weisnail 2:2e32ee9f0e51 996 }else{
weisnail 2:2e32ee9f0e51 997 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
weisnail 2:2e32ee9f0e51 998 }
weisnail 2:2e32ee9f0e51 999 }
weisnail 2:2e32ee9f0e51 1000
weisnail 2:2e32ee9f0e51 1001 }
weisnail 2:2e32ee9f0e51 1002
weisnail 2:2e32ee9f0e51 1003 unsigned int DynamixelClass::readRegister(unsigned char ID,unsigned char Register){
weisnail 2:2e32ee9f0e51 1004
weisnail 2:2e32ee9f0e51 1005 Instruction_Packet_Array[0] = ID;
weisnail 2:2e32ee9f0e51 1006 Instruction_Packet_Array[1] = 0x04;
weisnail 2:2e32ee9f0e51 1007 Instruction_Packet_Array[2] = COMMAND_READ_DATA;
weisnail 2:2e32ee9f0e51 1008 Instruction_Packet_Array[3] = Register;
weisnail 2:2e32ee9f0e51 1009 Instruction_Packet_Array[4] = READ_ONE_BYTE_LENGTH;
weisnail 2:2e32ee9f0e51 1010 Instruction_Packet_Array[5] = ~(ID + READ_TEMP_LENGTH + COMMAND_READ_DATA + Register + READ_ONE_BYTE_LENGTH);
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 readStatusPacket();
weisnail 2:2e32ee9f0e51 1017
weisnail 2:2e32ee9f0e51 1018 if (Status_Packet_Array[2] == 0){ // If there is no status packet error return value
weisnail 2:2e32ee9f0e51 1019 return Status_Packet_Array[3];
weisnail 2:2e32ee9f0e51 1020 }else{
weisnail 2:2e32ee9f0e51 1021 return (Status_Packet_Array[2] | 0xF000); // If there is a error Returns error value
weisnail 2:2e32ee9f0e51 1022 }
weisnail 2:2e32ee9f0e51 1023 }
weisnail 2:2e32ee9f0e51 1024
weisnail 2:2e32ee9f0e51 1025