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?
User | Revision | Line number | New 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 | } |