mbed to Dynamixel servo communication.
Dependents: dynamixel_wheel_test
Diff: Dynamixel.cpp
- Revision:
- 0:fb8a2d249639
- Child:
- 1:ce081666d225
diff -r 000000000000 -r fb8a2d249639 Dynamixel.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Dynamixel.cpp Wed Aug 19 05:39:57 2015 +0000 @@ -0,0 +1,154 @@ +#include "Dynamixel.h" +#include "mbed.h" + +Serial pc(USBTX, USBRX); + +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) +{ + m_link.baud(m_baudrate); +} + +void Dynamixel::ping() +{ + uint8_t elements = 6; + uint8_t packetBuffer[elements]; + if (m_link.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = m_motorID; // ID + packetBuffer[3] = 0x02; // Length + packetBuffer[4] = PING; // Instruction + packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF; // Check sum + + pc.printf("\n Instruction packet:\n"); + for (int i = 0; i<= elements; i++) { + pc.printf("0x%x\t", packetBuffer[i]); + } + + m_txEnable = 1; // Enable Tx / Disable Rx + for (int i = 0; i<= elements; i++) { + m_link.putc(packetBuffer[i]); + } + wait_ms(1); // fix this!!! + m_txEnable = 0; // Disable Tx / Enable Rx + } else { + pc.printf("Dynamixel not writeable\n"); + } + //pc.baud(9600); + wait_ms(10); + pc.printf("\n Status packet:\n"); + while ( m_link.readable() ) { + pc.printf("0x%x\t", m_link.getc()); // Read status packet if available + } + pc.printf("\n PING: Done.\n"); +} + +void Dynamixel::toggleLED(uint8_t ledState) +{ + uint8_t elements = 8; + uint8_t packetBuffer[elements]; + if (m_link.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = m_motorID; // ID + packetBuffer[3] = 0x04; // Length + packetBuffer[4] = WRITE_DATA; // Instruction + packetBuffer[5] = ADDRESS_LED; // Parameter 1: Starting address + packetBuffer[6] = ledState; // Parameter 2: First value to be writen + packetBuffer[7] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6]) & 0xFF; // Check sum + + m_txEnable = 1; // Enable Tx / Disable Rx + for (int i = 0; i<= elements; i++) { + m_link.putc(packetBuffer[i]); + } + wait_ms(2); // fix this!!! + m_txEnable = 0; // Disable Tx / Enable Rx + } else { + //pc.printf("Dynamixel not writeable\n"); + } + wait_ms(10); + pc.printf("\n Status packet:\n"); + while ( m_link.readable() ) { + pc.printf("0x%x\t", m_link.getc()); // Read status packet if available + } +} + +void Dynamixel::move(uint16_t position) +{ + // 0 to 1023 (0x3FF) + uint8_t elements = 9; + uint8_t packetBuffer[elements]; + uint16_t checkSum = 0; + if (m_link.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = m_motorID; // ID + packetBuffer[3] = 0x05; // Length + packetBuffer[4] = WRITE_DATA; // Instruction + packetBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address + packetBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen + packetBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen + checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7]; + packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum + + /* + for (int i = 0; i<= elements; i++) { + pc.printf("%c ", packetBuffer[i]); + } + pc.printf("\n"); + */ + m_txEnable = 1; // Enable Tx / Disable Rx + for (int i = 0; i<= elements; i++) { + m_link.putc(packetBuffer[i]); + } + wait_ms(2); // fix this!!! + m_txEnable = 0; // Disable Tx / Enable Rx + } else { + //pc.printf("Dynamixel not writeable\n"); + } + wait_ms(10); + pc.printf("\n Status packet:\n"); + while ( m_link.readable() ) { + pc.printf("0x%x\t", m_link.getc()); // Read status packet if available + } +} + +void Dynamixel::setSpeed(uint16_t speed) +{ + // 0 to 1023 (0x3FF) + uint8_t elements = 9; + uint8_t packetBuffer[elements]; + uint16_t checkSum = 0; + if (m_link.writeable() ) { + packetBuffer[0] = 0xff; + packetBuffer[1] = 0xff; + packetBuffer[2] = m_motorID; // ID + packetBuffer[3] = 0x05; // Length + packetBuffer[4] = WRITE_DATA; // Instruction + packetBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address + packetBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen + packetBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen + checkSum = packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + packetBuffer[7]; + packetBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum + + /* + for (int i = 0; i<= elements; i++) { + pc.printf("%c ", packetBuffer[i]); + } + pc.printf("\n"); + */ + m_txEnable = 1; // Enable Tx / Disable Rx + for (int i = 0; i<= elements; i++) { + m_link.putc(packetBuffer[i]); + } + wait_ms(2); // fix this!!! + m_txEnable = 0; // Disable Tx / Enable Rx + } else { + //pc.printf("Dynamixel not writeable\n"); + } + wait_ms(10); + pc.printf("\n Status packet:\n"); + while ( m_link.readable() ) { + pc.printf("0x%x\t", m_link.getc()); // Read status packet if available + } +} \ No newline at end of file