Dan Gongora / Dynamixel

mbed to Dynamixel servo communication.

Dependents:   dynamixel_wheel_test

Dynamixel.cpp@1:ce081666d225, 2015-08-19 (annotated)

Committer:
dmgongora
Date:
Wed Aug 19 06:10:52 2015 +0000
Revision:
1:ce081666d225
Parent:
0:fb8a2d249639
Child:
2:32377cbec534
Methods now return the error byte from the status packet.

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