mbed to Dynamixel servo communication.
Dependents: dynamixel_wheel_test
Diff: Dynamixel.cpp
- Revision:
- 3:61785d105315
- Parent:
- 2:32377cbec534
- Child:
- 4:92ff8b2f3b3f
diff -r 32377cbec534 -r 61785d105315 Dynamixel.cpp --- a/Dynamixel.cpp Wed Aug 19 06:31:45 2015 +0000 +++ b/Dynamixel.cpp Wed Aug 19 08:59:16 2015 +0000 @@ -1,6 +1,8 @@ #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); @@ -8,9 +10,10 @@ uint8_t Dynamixel::ping() { + pc.printf("\n ---Ping--- \n"); uint8_t elements = 6; uint8_t instructionBuffer[elements]; - uint8_t statusBuffer[STATUS_PACKET_LENGTH]; + uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0}; if (m_link.writeable() ) { instructionBuffer[0] = 0xff; @@ -30,16 +33,24 @@ //pc.printf("Dynamixel not writeable\n"); } wait_ms(10); // fix this!!! - if ( m_link.readable() && m_motorID != BROADCAST_ID ) { - for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + + if ( m_motorID != BROADCAST_ID ) { + int i = 0; + while( m_link.readable() && i<STATUS_PACKET_LENGTH) { statusBuffer[i] = m_link.getc(); // Read status packet + i++; } } + // Check status packet content + for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + pc.printf("%x ", statusBuffer[i]); + } return statusBuffer[4]; // Return error } uint8_t Dynamixel::toggleLED(uint8_t ledState) { + pc.printf("\n ---Toggle--- \n"); uint8_t elements = 8; uint8_t instructionBuffer[elements]; uint8_t statusBuffer[STATUS_PACKET_LENGTH]; @@ -69,15 +80,17 @@ statusBuffer[i] = m_link.getc(); // Read status packet } } + return statusBuffer[4]; // Return error } uint8_t Dynamixel::move(uint16_t position) { + pc.printf("\n ---Move--- \n"); // 0 to 1023 (0x3FF) uint8_t elements = 9; uint8_t instructionBuffer[elements]; - uint8_t statusBuffer[STATUS_PACKET_LENGTH]; + uint8_t statusBuffer[STATUS_PACKET_LENGTH] = {0,0,0,0,0,0}; uint16_t checkSum = 0; if (m_link.writeable() ) { @@ -89,15 +102,14 @@ instructionBuffer[5] = ADDRESS_GOAL_POSITION; // Parameter 1: Starting address instructionBuffer[6] = (uint8_t) position & 0xff; // Parameter 2: First value to be writen instructionBuffer[7] = (uint8_t) (position >> 8); // Parameter 3: Second value to be writen - checkSum = instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]; - instructionBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum - - /* - for (int i = 0; i<= elements; i++) { - pc.printf("%c ", instructionBuffer[i]); + checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]); + instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum + + for (int i = 0; i < elements; i++) { + pc.printf("%x ", instructionBuffer[i]); } pc.printf("\n"); - */ + m_txEnable = 1; // Enable Tx / Disable Rx for (int i = 0; i<= elements; i++) { m_link.putc(instructionBuffer[i]); @@ -105,19 +117,31 @@ wait_ms(2); // fix this!!! m_txEnable = 0; // Disable Tx / Enable Rx } else { - //pc.printf("Dynamixel not writeable\n"); + pc.printf("Dynamixel not writeable\n"); } + wait_ms(10); - if ( m_link.readable() && m_motorID != BROADCAST_ID ) { - for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + if ( m_motorID != BROADCAST_ID ) { + pc.printf("Trying to receiv status packet.\n"); + int i = 0; + while( m_link.readable() && i<STATUS_PACKET_LENGTH) { statusBuffer[i] = m_link.getc(); // Read status packet + i++; + //wait_ms(100); } + pc.printf("Read %d bytes.\n", i); } + // Check status packet content + for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + pc.printf("%x ", statusBuffer[i]); + } + return statusBuffer[4]; // Return error } uint8_t Dynamixel::setSpeed(uint16_t speed) { + pc.printf("\n --- Set speed--- \n"); // 0 to 1023 (0x3FF) uint8_t elements = 9; uint8_t instructionBuffer[elements]; @@ -133,15 +157,9 @@ instructionBuffer[5] = ADDRESS_MOVING_SPEED; // Parameter 1: Starting address instructionBuffer[6] = (uint8_t) speed & 0xff; // Parameter 2: First value to be writen instructionBuffer[7] = (uint8_t) (speed >> 8); // Parameter 3: Second value to be writen - checkSum = instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]; - instructionBuffer[8] = (uint8_t) (~checkSum & 0xff); // Check sum + checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6] + instructionBuffer[7]); + instructionBuffer[8] = (uint8_t) (checkSum & 0xff); // Check sum - /* - for (int i = 0; i<= elements; i++) { - pc.printf("%c ", instructionBuffer[i]); - } - pc.printf("\n"); - */ m_txEnable = 1; // Enable Tx / Disable Rx for (int i = 0; i<= elements; i++) { m_link.putc(instructionBuffer[i]); @@ -152,10 +170,74 @@ //pc.printf("Dynamixel not writeable\n"); } wait_ms(10); - if ( m_link.readable() && m_motorID != BROADCAST_ID ) { - for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + if ( m_motorID != BROADCAST_ID ) { + pc.printf("Trying to receiv status packet.\n"); + int i = 0; + while( m_link.readable() && i<STATUS_PACKET_LENGTH) { statusBuffer[i] = m_link.getc(); // Read status packet + i++; + //wait_ms(100); } + pc.printf("Read %d bytes.\n", i); + } + // Check status packet content + for (int i = 0; i < STATUS_PACKET_LENGTH; i++) { + pc.printf("%x ", statusBuffer[i]); } return statusBuffer[4]; // Return error +} + +uint8_t Dynamixel::getReturnDelayTime() +{ + // 0 to 254 (0xFE) + uint8_t elements = 8; + uint8_t instructionBuffer[elements]; + uint8_t statusBuffer[STATUS_PACKET_LENGTH+1] = {0,0,0,0,0,0,0}; // usual length plus the value read + + uint16_t checkSum = 0; + if (m_link.writeable() ) { + instructionBuffer[0] = 0xff; + instructionBuffer[1] = 0xff; + instructionBuffer[2] = m_motorID; // ID + instructionBuffer[3] = 0x04; // Length + instructionBuffer[4] = READ_DATA; // Instruction + instructionBuffer[5] = ADDRESS_RETURN_DELAY_TIME; // Parameter 1: Starting address + instructionBuffer[6] = 0x01; // Parameter 2: Number of bytes to read + + checkSum = ~(instructionBuffer[2] + instructionBuffer[3] + instructionBuffer[4] + instructionBuffer[5] + instructionBuffer[6]); + instructionBuffer[7] = (uint8_t) (checkSum & 0xff); // Check sum + + for (int i = 0; i < elements; i++) { + pc.printf("%x ", instructionBuffer[i]); + } + pc.printf("\n"); + + m_txEnable = 1; // Enable Tx / Disable Rx + for (int i = 0; i<= elements; i++) { + m_link.putc(instructionBuffer[i]); + } + wait_ms(2); // fix this!!! + m_txEnable = 0; // Disable Tx / Enable Rx + } else { + pc.printf("Dynamixel not writeable\n"); + } + + wait_ms(10); + if ( m_motorID != BROADCAST_ID ) { + pc.printf("Trying to receiv status packet.\n"); + int i = 0; + while( m_link.readable() && i<STATUS_PACKET_LENGTH + 1) { + statusBuffer[i] = m_link.getc(); // Read status packet + i++; + //wait_ms(100); + } + pc.printf("Read %d bytes.\n", i); + } + // Check status packet content + for (int i = 0; i < STATUS_PACKET_LENGTH + 1; i++) { + pc.printf("%x ", statusBuffer[i]); + } + pc.printf("\n Delay time: %d [us]\n",statusBuffer[5]*2); + + return statusBuffer[5]; // Return parameter } \ No newline at end of file