Dan Gongora / Dynamixel

mbed to Dynamixel servo communication.

Dependents:   dynamixel_wheel_test

Dynamixel.cpp@2:32377cbec534, 2015-08-19 (annotated)

Committer:
dmgongora
Date:
Wed Aug 19 06:31:45 2015 +0000
Revision:
2:32377cbec534
Parent:
1:ce081666d225
Child:
3:61785d105315
Fixed minor logic errors and removed references to pc.serial from within the class.

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 0:fb8a2d249639 11 uint8_t elements = 6;
dmgongora 1:ce081666d225 12 uint8_t instructionBuffer[elements];
dmgongora 1:ce081666d225 13 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 14
dmgongora 0:fb8a2d249639 15 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 16 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 17 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 18 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 19 instructionBuffer[3] = 0x02; // Length
dmgongora 1:ce081666d225 20 instructionBuffer[4] = PING; // Instruction
dmgongora 1:ce081666d225 21 instructionBuffer[5] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 22
dmgongora 1:ce081666d225 23 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 24 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 25 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 26 }
dmgongora 0:fb8a2d249639 27 wait_ms(1); // fix this!!!
dmgongora 1:ce081666d225 28 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 29 } else {
dmgongora 2:32377cbec534 30 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 31 }
dmgongora 1:ce081666d225 32 wait_ms(10); // fix this!!!
dmgongora 1:ce081666d225 33 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 34 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 35 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 36 }
dmgongora 0:fb8a2d249639 37 }
dmgongora 1:ce081666d225 38 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 39 }
dmgongora 0:fb8a2d249639 40
dmgongora 1:ce081666d225 41 uint8_t Dynamixel::toggleLED(uint8_t ledState)
dmgongora 0:fb8a2d249639 42 {
dmgongora 0:fb8a2d249639 43 uint8_t elements = 8;
dmgongora 1:ce081666d225 44 uint8_t instructionBuffer[elements];
dmgongora 1:ce081666d225 45 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 46
dmgongora 0:fb8a2d249639 47 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 48 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 49 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 50 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 51 instructionBuffer[3] = 0x04; // Length
dmgongora 1:ce081666d225 52 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 53 instructionBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 54 instructionBuffer[6] = ledState; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 55 instructionBuffer[7] = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]) & 0xFF; // Check sum
dmgongora 0:fb8a2d249639 56
dmgongora 0:fb8a2d249639 57 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 58 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 59 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 60 }
dmgongora 0:fb8a2d249639 61 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 62 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 63 } else {
dmgongora 0:fb8a2d249639 64 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 65 }
dmgongora 0:fb8a2d249639 66 wait_ms(10);
dmgongora 1:ce081666d225 67 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 68 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 69 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 70 }
dmgongora 0:fb8a2d249639 71 }
dmgongora 1:ce081666d225 72 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 73 }
dmgongora 0:fb8a2d249639 74
dmgongora 1:ce081666d225 75 uint8_t Dynamixel::move(uint16_t position)
dmgongora 0:fb8a2d249639 76 {
dmgongora 0:fb8a2d249639 77 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 78 uint8_t elements = 9;
dmgongora 1:ce081666d225 79 uint8_t instructionBuffer[elements];
dmgongora 1:ce081666d225 80 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 81
dmgongora 0:fb8a2d249639 82 uint16_t checkSum = 0;
dmgongora 0:fb8a2d249639 83 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 84 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 85 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 86 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 87 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 88 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 89 instructionBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address
dmgongora 1:ce081666d225 90 instructionBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 91 instructionBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen
dmgongora 1:ce081666d225 92 checkSum = instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7];
dmgongora 1:ce081666d225 93 instructionBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 94
dmgongora 0:fb8a2d249639 95 /*
dmgongora 0:fb8a2d249639 96 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 97 pc.printf("%c ", instructionBuffer[i]);
dmgongora 0:fb8a2d249639 98 }
dmgongora 0:fb8a2d249639 99 pc.printf("\n");
dmgongora 0:fb8a2d249639 100 */
dmgongora 0:fb8a2d249639 101 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 102 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 103 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 104 }
dmgongora 0:fb8a2d249639 105 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 106 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 107 } else {
dmgongora 0:fb8a2d249639 108 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 109 }
dmgongora 0:fb8a2d249639 110 wait_ms(10);
dmgongora 1:ce081666d225 111 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 112 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 113 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 114 }
dmgongora 0:fb8a2d249639 115 }
dmgongora 1:ce081666d225 116 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 117 }
dmgongora 0:fb8a2d249639 118
dmgongora 1:ce081666d225 119 uint8_t Dynamixel::setSpeed(uint16_t speed)
dmgongora 0:fb8a2d249639 120 {
dmgongora 0:fb8a2d249639 121 // 0 to 1023 (0x3FF)
dmgongora 0:fb8a2d249639 122 uint8_t elements = 9;
dmgongora 1:ce081666d225 123 uint8_t instructionBuffer[elements];
dmgongora 0:fb8a2d249639 124 uint16_t checkSum = 0;
dmgongora 1:ce081666d225 125 uint8_t statusBuffer[STATUS_PACKET_LENGTH];
dmgongora 1:ce081666d225 126
dmgongora 0:fb8a2d249639 127 if (m_link.writeable() ) {
dmgongora 1:ce081666d225 128 instructionBuffer[0] = 0xff;
dmgongora 1:ce081666d225 129 instructionBuffer[1] = 0xff;
dmgongora 1:ce081666d225 130 instructionBuffer[2] = m_motorID; // ID
dmgongora 1:ce081666d225 131 instructionBuffer[3] = 0x05; // Length
dmgongora 1:ce081666d225 132 instructionBuffer[4] = WRITE_DATA; // Instruction
dmgongora 1:ce081666d225 133 instructionBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address
dmgongora 1:ce081666d225 134 instructionBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen
dmgongora 1:ce081666d225 135 instructionBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen
dmgongora 1:ce081666d225 136 checkSum = instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7];
dmgongora 1:ce081666d225 137 instructionBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum
dmgongora 0:fb8a2d249639 138
dmgongora 0:fb8a2d249639 139 /*
dmgongora 0:fb8a2d249639 140 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 141 pc.printf("%c ", instructionBuffer[i]);
dmgongora 0:fb8a2d249639 142 }
dmgongora 0:fb8a2d249639 143 pc.printf("\n");
dmgongora 0:fb8a2d249639 144 */
dmgongora 0:fb8a2d249639 145 m_txEnable = 1; // Enable Tx / Disable Rx
dmgongora 0:fb8a2d249639 146 for (int i = 0; i<= elements; i++) {
dmgongora 1:ce081666d225 147 m_link.putc(instructionBuffer[i]);
dmgongora 0:fb8a2d249639 148 }
dmgongora 0:fb8a2d249639 149 wait_ms(2); // fix this!!!
dmgongora 0:fb8a2d249639 150 m_txEnable = 0; // Disable Tx / Enable Rx
dmgongora 0:fb8a2d249639 151 } else {
dmgongora 0:fb8a2d249639 152 //pc.printf("Dynamixel not writeable\n");
dmgongora 0:fb8a2d249639 153 }
dmgongora 0:fb8a2d249639 154 wait_ms(10);
dmgongora 1:ce081666d225 155 if ( m_link.readable() && m_motorID != BROADCAST_ID ) {
dmgongora 2:32377cbec534 156 for (int i = 0; i < STATUS_PACKET_LENGTH; i++) {
dmgongora 1:ce081666d225 157 statusBuffer[i] = m_link.getc(); // Read status packet
dmgongora 1:ce081666d225 158 }
dmgongora 0:fb8a2d249639 159 }
dmgongora 1:ce081666d225 160 return statusBuffer[4]; // Return error
dmgongora 0:fb8a2d249639 161 }