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