Chris LU / Mbed 2 deprecated Self_Riding_Bicycle_LQRgain

Self_Riding_Bicycle_LQRgain

Dependencies:   LSM9DS0_self_riding_bike_LQR mbed

Mx28.cpp@0:c2e43d17c8e4, 2018-08-20 (annotated)

Committer:
cpul5338
Date:
Mon Aug 20 13:21:31 2018 +0000
Revision:
0:c2e43d17c8e4
self_riding_bike_LQR

Who changed what in which revision?

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