Zachary Lang / MX28

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of MX28 by Gert Lauritsen

Mx28.cpp@2:6801e89014fa, 2017-07-06 (annotated)

Committer:
LangZachary
Date:
Thu Jul 06 00:38:06 2017 +0000
Revision:
2:6801e89014fa
Parent:
1:c866ce96ceb3 
General code cleaning, IMU setup, initializing data command receiving from NUC

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
gert_lauritsen 0:45d73665e91f 46 wait_us((Counter + 4)*190);
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
gert_lauritsen 0:45d73665e91f 947