Diff: Dynamixel.cpp

Revision:

3:61785d105315

Parent:

2:32377cbec534

Child:

4:92ff8b2f3b3f

--- 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