YiLun Cheng / MX28

20180723

Fork of MX28 by LDSC_Robotics_TAs

Mx28.cpp@3:f4f5f485eed3, 2017-02-04 (annotated)

Committer:
alan82914
Date:
Sat Feb 04 08:56:46 2017 +0000
Revision:
3:f4f5f485eed3
Parent:
2:2e32ee9f0e51 
MX28

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