Dan Gongora / Dynamixel

mbed to Dynamixel servo communication.

Dependents:   dynamixel_wheel_test

Dynamixel.cpp@6:edba46e8f8b4, 2015-10-25 (annotated)

Committer:
dmgongora
Date:
Sun Oct 25 02:15:04 2015 +0000
Revision:
6:edba46e8f8b4
Parent:
4:92ff8b2f3b3f 
Added function to set the rotation mode: Wheel, Joint, Multi-turn

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dmgongora 0:fb8a2d249639 1 #include "Dynamixel.h"
dmgongora 0:fb8a2d249639 2 #include "mbed.h"
dmgongora 0:fb8a2d249639 3
dmgongora 0:fb8a2d249639 4 Dynamixel::Dynamixel(PinName tx, PinName rx, PinName txEnable, uint8_t motorID, int baudrate) : m_link(tx, rx), m_txEnable(txEnable), m_motorID(motorID), m_baudrate(baudrate)
dmgongora 0:fb8a2d249639 5 {
dmgongora 0:fb8a2d249639 6 m_link.baud(m_baudrate);
dmgongora 0:fb8a2d249639 7 }
dmgongora 0:fb8a2d249639 8
dmgongora 1:ce081666d225 9 uint8_t Dynamixel::ping()
dmgongora 0:fb8a2d249639 10 {
dmgongora 4:92ff8b2f3b3f 11 DEBUG("\n ---Ping--- \n");
dmgongora 0:fb8a2d249639 12 uint8_t elements = 6;
dmgongora 1:ce081666d225 13 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 14 uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
dmgongora 1:ce081666d225 15
dmgongora 0:fb8a2d249639 16 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 17 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 18 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 19 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 20 instructionBuffer[3] = 0x02; // Length
dmgongora 1:ce081666d225 21 instructionBuffer[4] = PING; // Instruction
dmgongora 1:ce081666d225 22 instructionBuffer[5] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 23
dmgongora 1:ce081666d225 24 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 25 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 26 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 27 }
dmgongora 0:fb8a2d249639 28 wait_ms(1); // fix this!!!
dmgongora 1:ce081666d225 29 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 30 } else {
dmgongora 4:92ff8b2f3b3f 31 // Dynamixel not writeable
dmgongora 0:fb8a2d249639 32 }
dmgongora 1:ce081666d225 33 wait_ms(10); // fix this!!!
dmgongora 3:61785d105315 34
dmgongora 3:61785d105315 35 if ( m_motorID != BROADCAST_ID ) {
dmgongora 3:61785d105315 36 int i = 0;
dmgongora 3:61785d105315 37 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 38 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 39 i++;
dmgongora 1:ce081666d225 40 }
dmgongora 0:fb8a2d249639 41 }
dmgongora 3:61785d105315 42 // Check status packet content
dmgongora 3:61785d105315 43 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 4:92ff8b2f3b3f 44 DEBUG("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 45 }
dmgongora 1:ce081666d225 46 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 47 }
dmgongora 0:fb8a2d249639 48
dmgongora 1:ce081666d225 49 uint8_t Dynamixel::toggleLED(uint8_t ledState)
dmgongora 0:fb8a2d249639 50 {
dmgongora 4:92ff8b2f3b3f 51 DEBUG("\n ---Toggle--- \n");
dmgongora 0:fb8a2d249639 52 uint8_t elements = 8;
dmgongora 1:ce081666d225 53 uint8_t instructionBuffer[elements];
dmgongora 1:ce081666d225 54 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 55
dmgongora 0:fb8a2d249639 56 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 57 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 58 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 59 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 60 instructionBuffer[3] = 0x04; // Length
dmgongora 1:ce081666d225 61 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 62 instructionBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 63 instructionBuffer[6] = ledState; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 64 instructionBuffer[7] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 65
dmgongora 0:fb8a2d249639 66 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 67 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 68 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 69 }
dmgongora 0:fb8a2d249639 70 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 71 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 72 } else {
dmgongora 4:92ff8b2f3b3f 73 //Dynamixel not writeable
dmgongora 0:fb8a2d249639 74 }
dmgongora 0:fb8a2d249639 75 wait_ms(10);
dmgongora 1:ce081666d225 76 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 77 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 78 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 79 }
dmgongora 0:fb8a2d249639 80 }
dmgongora 3:61785d105315 81
dmgongora 1:ce081666d225 82 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 83 }
dmgongora 0:fb8a2d249639 84
dmgongora 1:ce081666d225 85 uint8_t Dynamixel::move(uint16_t position)
dmgongora 0:fb8a2d249639 86 {
dmgongora 4:92ff8b2f3b3f 87 DEBUG("\n ---Move--- \n");
dmgongora 0:fb8a2d249639 88 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 89 uint8_t elements = 9;
dmgongora 1:ce081666d225 90 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 91 uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
dmgongora 1:ce081666d225 92
dmgongora 0:fb8a2d249639 93 uint16_t checkSum = 0;
dmgongora 0:fb8a2d249639 94 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 95 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 96 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 97 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 98 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 99 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 100 instructionBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 1:ce081666d225 101 instructionBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 102 instructionBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 3:61785d105315 103 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
dmgongora 3:61785d105315 104 instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 4:92ff8b2f3b3f 105 // Check instruction buffer
dmgongora 3:61785d105315 106 for (int i = 0; i < elements; i++) {
dmgongora 4:92ff8b2f3b3f 107 DEBUG("%x ", instructionBuffer[i]);
dmgongora 0:fb8a2d249639 108 }
dmgongora 4:92ff8b2f3b3f 109 DEBUG("\n");
dmgongora 3:61785d105315 110
dmgongora 0:fb8a2d249639 111 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 112 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 113 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 114 }
dmgongora 0:fb8a2d249639 115 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 116 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 117 } else {
dmgongora 4:92ff8b2f3b3f 118 //Dynamixel not writeable
dmgongora 0:fb8a2d249639 119 }
dmgongora 3:61785d105315 120
dmgongora 0:fb8a2d249639 121 wait_ms(10);
dmgongora 3:61785d105315 122 if ( m_motorID != BROADCAST_ID ) {
dmgongora 4:92ff8b2f3b3f 123 DEBUG("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 124 int i = 0;
dmgongora 3:61785d105315 125 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 126 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 127 i++;
dmgongora 3:61785d105315 128 //wait_ms(100);
dmgongora 1:ce081666d225 129 }
dmgongora 4:92ff8b2f3b3f 130 DEBUG("Read %d bytes.\n", i);
dmgongora 0:fb8a2d249639 131 }
dmgongora 3:61785d105315 132 // Check status packet content
dmgongora 3:61785d105315 133 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 4:92ff8b2f3b3f 134 DEBUG("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 135 }
dmgongora 3:61785d105315 136
dmgongora 1:ce081666d225 137 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 138 }
dmgongora 0:fb8a2d249639 139
dmgongora 1:ce081666d225 140 uint8_t Dynamixel::setSpeed(uint16_t speed)
dmgongora 0:fb8a2d249639 141 {
dmgongora 4:92ff8b2f3b3f 142 DEBUG("\n --- Set speed--- \n");
dmgongora 0:fb8a2d249639 143 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 144 uint8_t elements = 9;
dmgongora 1:ce081666d225 145 uint8_t instructionBuffer[elements];
dmgongora 0:fb8a2d249639 146 uint16_t checkSum = 0;
dmgongora 1:ce081666d225 147 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 148
dmgongora 0:fb8a2d249639 149 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 150 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 151 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 152 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 153 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 154 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 155 instructionBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 156 instructionBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 157 instructionBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen
dmgongora 3:61785d105315 158 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
dmgongora 3:61785d105315 159 instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 160
dmgongora 0:fb8a2d249639 161 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 162 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 163 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 164 }
dmgongora 0:fb8a2d249639 165 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 166 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 167 } else {
dmgongora 4:92ff8b2f3b3f 168 //Dynamixel not writeable
dmgongora 0:fb8a2d249639 169 }
dmgongora 0:fb8a2d249639 170 wait_ms(10);
dmgongora 3:61785d105315 171 if ( m_motorID != BROADCAST_ID ) {
dmgongora 4:92ff8b2f3b3f 172 DEBUG("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 173 int i = 0;
dmgongora 3:61785d105315 174 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 175 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 176 i++;
dmgongora 3:61785d105315 177 //wait_ms(100);
dmgongora 1:ce081666d225 178 }
dmgongora 4:92ff8b2f3b3f 179 DEBUG("Read %d bytes.\n", i);
dmgongora 3:61785d105315 180 }
dmgongora 3:61785d105315 181 // Check status packet content
dmgongora 3:61785d105315 182 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 4:92ff8b2f3b3f 183 DEBUG("%x ", statusBuffer[i]);
dmgongora 0:fb8a2d249639 184 }
dmgongora 1:ce081666d225 185 return statusBuffer[4]; // Return error
dmgongora 3:61785d105315 186 }
dmgongora 3:61785d105315 187
dmgongora 3:61785d105315 188 uint8_t Dynamixel::getReturnDelayTime()
dmgongora 3:61785d105315 189 {
dmgongora 3:61785d105315 190 // 0 to 254 (0xFE)
dmgongora 3:61785d105315 191 uint8_t elements = 8;
dmgongora 3:61785d105315 192 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 193 uint8_t statusBuffer[STATUS_PACKET_LENGTH+1] = {0,0,0,0,0,0,0}; // usual length plus the value read
dmgongora 3:61785d105315 194
dmgongora 3:61785d105315 195 uint16_t checkSum = 0;
dmgongora 3:61785d105315 196 if (m_link.writeable() ) {
dmgongora 3:61785d105315 197 instructionBuffer[0] = 0xff;
dmgongora 3:61785d105315 198 instructionBuffer[1] = 0xff;
dmgongora 3:61785d105315 199 instructionBuffer[2] = m_motorID; // ID
dmgongora 3:61785d105315 200 instructionBuffer[3] = 0x04; // Length
dmgongora 3:61785d105315 201 instructionBuffer[4] = READ_DATA; // Instruction
dmgongora 3:61785d105315 202 instructionBuffer[5] = ADDRESS_RETURN_DELAY_TIME; // Parameter 1: Starting address
dmgongora 3:61785d105315 203 instructionBuffer[6] = 0x01; // Parameter 2: Number of bytes to read
dmgongora 3:61785d105315 204
dmgongora 3:61785d105315 205 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]);
dmgongora 3:61785d105315 206 instructionBuffer[7] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 4:92ff8b2f3b3f 207 // Check instruction buffer
dmgongora 3:61785d105315 208 for (int i = 0; i < elements; i++) {
dmgongora 4:92ff8b2f3b3f 209 DEBUG("%x ", instructionBuffer[i]);
dmgongora 3:61785d105315 210 }
dmgongora 4:92ff8b2f3b3f 211 DEBUG("\n");
dmgongora 3:61785d105315 212
dmgongora 3:61785d105315 213 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 3:61785d105315 214 for (int i = 0; i<= elements; i++) {
dmgongora 3:61785d105315 215 m_link.putc(instructionBuffer[i]);
dmgongora 3:61785d105315 216 }
dmgongora 3:61785d105315 217 wait_ms(2); // fix this!!!
dmgongora 3:61785d105315 218 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 3:61785d105315 219 } else {
dmgongora 4:92ff8b2f3b3f 220 //Dynamixel not writeable
dmgongora 3:61785d105315 221 }
dmgongora 3:61785d105315 222
dmgongora 3:61785d105315 223 wait_ms(10);
dmgongora 3:61785d105315 224 if ( m_motorID != BROADCAST_ID ) {
dmgongora 4:92ff8b2f3b3f 225 DEBUG("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 226 int i = 0;
dmgongora 3:61785d105315 227 while( m_link.readable() && i<STATUS_PACKET_LENGTH + 1) {
dmgongora 3:61785d105315 228 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 229 i++;
dmgongora 3:61785d105315 230 //wait_ms(100);
dmgongora 3:61785d105315 231 }
dmgongora 4:92ff8b2f3b3f 232 DEBUG("Read %d bytes.\n", i);
dmgongora 3:61785d105315 233 }
dmgongora 3:61785d105315 234 // Check status packet content
dmgongora 3:61785d105315 235 for (int i = 0; i < STATUS_PACKET_LENGTH + 1; i++) {
dmgongora 4:92ff8b2f3b3f 236 DEBUG("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 237 }
dmgongora 4:92ff8b2f3b3f 238 DEBUG("\n Delay time: %d [us]\n",statusBuffer[5]*2);
dmgongora 3:61785d105315 239
dmgongora 3:61785d105315 240 return statusBuffer[5]; // Return parameter
dmgongora 6:edba46e8f8b4 241 }
dmgongora 6:edba46e8f8b4 242
dmgongora 6:edba46e8f8b4 243 uint8_t Dynamixel::setAngleLimit(uint16_t mode, uint8_t address)
dmgongora 6:edba46e8f8b4 244 {
dmgongora 6:edba46e8f8b4 245 DEBUG("\n ---setAngleLimit--- \n");
dmgongora 6:edba46e8f8b4 246 // Sets allowable position values (angles) for Goal position
dmgongora 6:edba46e8f8b4 247 /*
dmgongora 6:edba46e8f8b4 248 Valid address
dmgongora 6:edba46e8f8b4 249 -------------
dmgongora 6:edba46e8f8b4 250 Use the values defined in the header file:
dmgongora 6:edba46e8f8b4 251 - ADDRESS_CW_ANGLE_LIMIT
dmgongora 6:edba46e8f8b4 252 - ADDRESS_CCW_ANGLE_LIMIT
dmgongora 6:edba46e8f8b4 253 Mode values
dmgongora 6:edba46e8f8b4 254 -----------
dmgongora 6:edba46e8f8b4 255 - Wheel mode: set both CW and CCW registers to 0
dmgongora 6:edba46e8f8b4 256 - Joint mode: set both CW and CCW registers to a value different of 0
dmgongora 6:edba46e8f8b4 257 - Multi-turn mode: both CW and CCW registers are 4095 (0x0FFF)
dmgongora 6:edba46e8f8b4 258 */
dmgongora 6:edba46e8f8b4 259 uint8_t elements = 9;
dmgongora 6:edba46e8f8b4 260 uint8_t instructionBuffer[elements];
dmgongora 6:edba46e8f8b4 261 uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
dmgongora 6:edba46e8f8b4 262
dmgongora 6:edba46e8f8b4 263 uint16_t checkSum = 0;
dmgongora 6:edba46e8f8b4 264 if (m_link.writeable() ) {
dmgongora 6:edba46e8f8b4 265 instructionBuffer[0] = 0xff;
dmgongora 6:edba46e8f8b4 266 instructionBuffer[1] = 0xff;
dmgongora 6:edba46e8f8b4 267 instructionBuffer[2] = m_motorID; // ID
dmgongora 6:edba46e8f8b4 268 instructionBuffer[3] = 0x05; // Length
dmgongora 6:edba46e8f8b4 269 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 6:edba46e8f8b4 270 instructionBuffer[5] = address; // Parameter 1: Starting address (Low byte)
dmgongora 6:edba46e8f8b4 271 instructionBuffer[6] = (uint8_t) mode & 0xff; // Parameter 2: First value to be writen
dmgongora 6:edba46e8f8b4 272 instructionBuffer[7] = (uint8_t) (mode >> 8); // Parameter 3: Second value to be writen
dmgongora 6:edba46e8f8b4 273 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
dmgongora 6:edba46e8f8b4 274 instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 6:edba46e8f8b4 275 // Check instruction buffer
dmgongora 6:edba46e8f8b4 276 for (int i = 0; i < elements; i++) {
dmgongora 6:edba46e8f8b4 277 DEBUG("%x ", instructionBuffer[i]);
dmgongora 6:edba46e8f8b4 278 }
dmgongora 6:edba46e8f8b4 279 DEBUG("\n");
dmgongora 6:edba46e8f8b4 280
dmgongora 6:edba46e8f8b4 281 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 6:edba46e8f8b4 282 for (int i = 0; i<= elements; i++) {
dmgongora 6:edba46e8f8b4 283 m_link.putc(instructionBuffer[i]);
dmgongora 6:edba46e8f8b4 284 }
dmgongora 6:edba46e8f8b4 285 wait_ms(2); // fix this!!!
dmgongora 6:edba46e8f8b4 286 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 6:edba46e8f8b4 287 } else {
dmgongora 6:edba46e8f8b4 288 //Dynamixel not writeable
dmgongora 6:edba46e8f8b4 289 }
dmgongora 6:edba46e8f8b4 290
dmgongora 6:edba46e8f8b4 291 wait_ms(10);
dmgongora 6:edba46e8f8b4 292 if ( m_motorID != BROADCAST_ID ) {
dmgongora 6:edba46e8f8b4 293 DEBUG("Trying to receiv status packet.\n");
dmgongora 6:edba46e8f8b4 294 int i = 0;
dmgongora 6:edba46e8f8b4 295 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 6:edba46e8f8b4 296 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 6:edba46e8f8b4 297 i++;
dmgongora 6:edba46e8f8b4 298 //wait_ms(100);
dmgongora 6:edba46e8f8b4 299 }
dmgongora 6:edba46e8f8b4 300 DEBUG("Read %d bytes.\n", i);
dmgongora 6:edba46e8f8b4 301 }
dmgongora 6:edba46e8f8b4 302 // Check status packet content
dmgongora 6:edba46e8f8b4 303 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 6:edba46e8f8b4 304 DEBUG("%x ", statusBuffer[i]);
dmgongora 6:edba46e8f8b4 305 }
dmgongora 6:edba46e8f8b4 306
dmgongora 6:edba46e8f8b4 307 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 308 }