Dan Gongora / Dynamixel

mbed to Dynamixel servo communication.

Dependents:   dynamixel_wheel_test

Dynamixel.cpp@3:61785d105315, 2015-08-19 (annotated)

Committer:
dmgongora
Date:
Wed Aug 19 08:59:16 2015 +0000
Revision:
3:61785d105315
Parent:
2:32377cbec534
Child:
4:92ff8b2f3b3f
Added method to get the return delay time

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 3:61785d105315 4 Serial pc(USBTX, USBRX);
dmgongora 3:61785d105315 5
dmgongora 0:fb8a2d249639 6 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 7 {
dmgongora 0:fb8a2d249639 8 m_link.baud(m_baudrate);
dmgongora 0:fb8a2d249639 9 }
dmgongora 0:fb8a2d249639 10
dmgongora 1:ce081666d225 11 uint8_t Dynamixel::ping()
dmgongora 0:fb8a2d249639 12 {
dmgongora 3:61785d105315 13 pc.printf("\n ---Ping--- \n");
dmgongora 0:fb8a2d249639 14 uint8_t elements = 6;
dmgongora 1:ce081666d225 15 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 16 uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
dmgongora 1:ce081666d225 17
dmgongora 0:fb8a2d249639 18 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 19 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 20 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 21 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 22 instructionBuffer[3] = 0x02; // Length
dmgongora 1:ce081666d225 23 instructionBuffer[4] = PING; // Instruction
dmgongora 1:ce081666d225 24 instructionBuffer[5] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 25
dmgongora 1:ce081666d225 26 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 27 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 28 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 29 }
dmgongora 0:fb8a2d249639 30 wait_ms(1); // fix this!!!
dmgongora 1:ce081666d225 31 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 32 } else {
dmgongora 2:32377cbec534 33 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 34 }
dmgongora 1:ce081666d225 35 wait_ms(10); // fix this!!!
dmgongora 3:61785d105315 36
dmgongora 3:61785d105315 37 if ( m_motorID != BROADCAST_ID ) {
dmgongora 3:61785d105315 38 int i = 0;
dmgongora 3:61785d105315 39 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 40 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 41 i++;
dmgongora 1:ce081666d225 42 }
dmgongora 0:fb8a2d249639 43 }
dmgongora 3:61785d105315 44 // Check status packet content
dmgongora 3:61785d105315 45 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 3:61785d105315 46 pc.printf("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 47 }
dmgongora 1:ce081666d225 48 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 49 }
dmgongora 0:fb8a2d249639 50
dmgongora 1:ce081666d225 51 uint8_t Dynamixel::toggleLED(uint8_t ledState)
dmgongora 0:fb8a2d249639 52 {
dmgongora 3:61785d105315 53 pc.printf("\n ---Toggle--- \n");
dmgongora 0:fb8a2d249639 54 uint8_t elements = 8;
dmgongora 1:ce081666d225 55 uint8_t instructionBuffer[elements];
dmgongora 1:ce081666d225 56 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 57
dmgongora 0:fb8a2d249639 58 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 59 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 60 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 61 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 62 instructionBuffer[3] = 0x04; // Length
dmgongora 1:ce081666d225 63 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 64 instructionBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 65 instructionBuffer[6] = ledState; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 66 instructionBuffer[7] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 67
dmgongora 0:fb8a2d249639 68 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 69 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 70 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 71 }
dmgongora 0:fb8a2d249639 72 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 73 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 74 } else {
dmgongora 0:fb8a2d249639 75 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 76 }
dmgongora 0:fb8a2d249639 77 wait_ms(10);
dmgongora 1:ce081666d225 78 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 79 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 80 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 81 }
dmgongora 0:fb8a2d249639 82 }
dmgongora 3:61785d105315 83
dmgongora 1:ce081666d225 84 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 85 }
dmgongora 0:fb8a2d249639 86
dmgongora 1:ce081666d225 87 uint8_t Dynamixel::move(uint16_t position)
dmgongora 0:fb8a2d249639 88 {
dmgongora 3:61785d105315 89 pc.printf("\n ---Move--- \n");
dmgongora 0:fb8a2d249639 90 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 91 uint8_t elements = 9;
dmgongora 1:ce081666d225 92 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 93 uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0};
dmgongora 1:ce081666d225 94
dmgongora 0:fb8a2d249639 95 uint16_t checkSum = 0;
dmgongora 0:fb8a2d249639 96 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 97 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 98 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 99 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 100 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 101 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 102 instructionBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 1:ce081666d225 103 instructionBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 104 instructionBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 3:61785d105315 105 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
dmgongora 3:61785d105315 106 instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 3:61785d105315 107
dmgongora 3:61785d105315 108 for (int i = 0; i < elements; i++) {
dmgongora 3:61785d105315 109 pc.printf("%x ", instructionBuffer[i]);
dmgongora 0:fb8a2d249639 110 }
dmgongora 0:fb8a2d249639 111 pc.printf("\n");
dmgongora 3:61785d105315 112
dmgongora 0:fb8a2d249639 113 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 114 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 115 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 116 }
dmgongora 0:fb8a2d249639 117 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 118 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 119 } else {
dmgongora 3:61785d105315 120 pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 121 }
dmgongora 3:61785d105315 122
dmgongora 0:fb8a2d249639 123 wait_ms(10);
dmgongora 3:61785d105315 124 if ( m_motorID != BROADCAST_ID ) {
dmgongora 3:61785d105315 125 pc.printf("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 126 int i = 0;
dmgongora 3:61785d105315 127 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 128 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 129 i++;
dmgongora 3:61785d105315 130 //wait_ms(100);
dmgongora 1:ce081666d225 131 }
dmgongora 3:61785d105315 132 pc.printf("Read %d bytes.\n", i);
dmgongora 0:fb8a2d249639 133 }
dmgongora 3:61785d105315 134 // Check status packet content
dmgongora 3:61785d105315 135 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 3:61785d105315 136 pc.printf("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 137 }
dmgongora 3:61785d105315 138
dmgongora 1:ce081666d225 139 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 140 }
dmgongora 0:fb8a2d249639 141
dmgongora 1:ce081666d225 142 uint8_t Dynamixel::setSpeed(uint16_t speed)
dmgongora 0:fb8a2d249639 143 {
dmgongora 3:61785d105315 144 pc.printf("\n --- Set speed--- \n");
dmgongora 0:fb8a2d249639 145 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 146 uint8_t elements = 9;
dmgongora 1:ce081666d225 147 uint8_t instructionBuffer[elements];
dmgongora 0:fb8a2d249639 148 uint16_t checkSum = 0;
dmgongora 1:ce081666d225 149 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 150
dmgongora 0:fb8a2d249639 151 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 152 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 153 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 154 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 155 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 156 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 157 instructionBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 158 instructionBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 159 instructionBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen
dmgongora 3:61785d105315 160 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]);
dmgongora 3:61785d105315 161 instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 162
dmgongora 0:fb8a2d249639 163 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 164 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 165 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 166 }
dmgongora 0:fb8a2d249639 167 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 168 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 169 } else {
dmgongora 0:fb8a2d249639 170 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 171 }
dmgongora 0:fb8a2d249639 172 wait_ms(10);
dmgongora 3:61785d105315 173 if ( m_motorID != BROADCAST_ID ) {
dmgongora 3:61785d105315 174 pc.printf("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 175 int i = 0;
dmgongora 3:61785d105315 176 while( m_link.readable() && i<STATUS_PACKET_LENGTH) {
dmgongora 1:ce081666d225 177 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 178 i++;
dmgongora 3:61785d105315 179 //wait_ms(100);
dmgongora 1:ce081666d225 180 }
dmgongora 3:61785d105315 181 pc.printf("Read %d bytes.\n", i);
dmgongora 3:61785d105315 182 }
dmgongora 3:61785d105315 183 // Check status packet content
dmgongora 3:61785d105315 184 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 3:61785d105315 185 pc.printf("%x ", statusBuffer[i]);
dmgongora 0:fb8a2d249639 186 }
dmgongora 1:ce081666d225 187 return statusBuffer[4]; // Return error
dmgongora 3:61785d105315 188 }
dmgongora 3:61785d105315 189
dmgongora 3:61785d105315 190 uint8_t Dynamixel::getReturnDelayTime()
dmgongora 3:61785d105315 191 {
dmgongora 3:61785d105315 192 // 0 to 254 (0xFE)
dmgongora 3:61785d105315 193 uint8_t elements = 8;
dmgongora 3:61785d105315 194 uint8_t instructionBuffer[elements];
dmgongora 3:61785d105315 195 uint8_t statusBuffer[STATUS_PACKET_LENGTH+1] = {0,0,0,0,0,0,0}; // usual length plus the value read
dmgongora 3:61785d105315 196
dmgongora 3:61785d105315 197 uint16_t checkSum = 0;
dmgongora 3:61785d105315 198 if (m_link.writeable() ) {
dmgongora 3:61785d105315 199 instructionBuffer[0] = 0xff;
dmgongora 3:61785d105315 200 instructionBuffer[1] = 0xff;
dmgongora 3:61785d105315 201 instructionBuffer[2] = m_motorID; // ID
dmgongora 3:61785d105315 202 instructionBuffer[3] = 0x04; // Length
dmgongora 3:61785d105315 203 instructionBuffer[4] = READ_DATA; // Instruction
dmgongora 3:61785d105315 204 instructionBuffer[5] = ADDRESS_RETURN_DELAY_TIME; // Parameter 1: Starting address
dmgongora 3:61785d105315 205 instructionBuffer[6] = 0x01; // Parameter 2: Number of bytes to read
dmgongora 3:61785d105315 206
dmgongora 3:61785d105315 207 checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]);
dmgongora 3:61785d105315 208 instructionBuffer[7] = (uint8_t) (checkSum & 0xff); // Check sum
dmgongora 3:61785d105315 209
dmgongora 3:61785d105315 210 for (int i = 0; i < elements; i++) {
dmgongora 3:61785d105315 211 pc.printf("%x ", instructionBuffer[i]);
dmgongora 3:61785d105315 212 }
dmgongora 3:61785d105315 213 pc.printf("\n");
dmgongora 3:61785d105315 214
dmgongora 3:61785d105315 215 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 3:61785d105315 216 for (int i = 0; i<= elements; i++) {
dmgongora 3:61785d105315 217 m_link.putc(instructionBuffer[i]);
dmgongora 3:61785d105315 218 }
dmgongora 3:61785d105315 219 wait_ms(2); // fix this!!!
dmgongora 3:61785d105315 220 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 3:61785d105315 221 } else {
dmgongora 3:61785d105315 222 pc.printf("Dynamixel not writeable\n");
dmgongora 3:61785d105315 223 }
dmgongora 3:61785d105315 224
dmgongora 3:61785d105315 225 wait_ms(10);
dmgongora 3:61785d105315 226 if ( m_motorID != BROADCAST_ID ) {
dmgongora 3:61785d105315 227 pc.printf("Trying to receiv status packet.\n");
dmgongora 3:61785d105315 228 int i = 0;
dmgongora 3:61785d105315 229 while( m_link.readable() && i<STATUS_PACKET_LENGTH + 1) {
dmgongora 3:61785d105315 230 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 3:61785d105315 231 i++;
dmgongora 3:61785d105315 232 //wait_ms(100);
dmgongora 3:61785d105315 233 }
dmgongora 3:61785d105315 234 pc.printf("Read %d bytes.\n", i);
dmgongora 3:61785d105315 235 }
dmgongora 3:61785d105315 236 // Check status packet content
dmgongora 3:61785d105315 237 for (int i = 0; i < STATUS_PACKET_LENGTH + 1; i++) {
dmgongora 3:61785d105315 238 pc.printf("%x ", statusBuffer[i]);
dmgongora 3:61785d105315 239 }
dmgongora 3:61785d105315 240 pc.printf("\n Delay time: %d [us]\n",statusBuffer[5]*2);
dmgongora 3:61785d105315 241
dmgongora 3:61785d105315 242 return statusBuffer[5]; // Return parameter
dmgongora 0:fb8a2d249639 243 }