Revision 1:ef90d942ce78, committed 2015-08-19

Comitter:

dmgongora

Date:

Wed Aug 19 05:36:55 2015 +0000

Parent:

0:79e2a8171b16

Child:

2:0ed32411598c

Commit message:

Added method to set the velocity but it's not working.

Changed in this revision

--- a/Dynamixel.cpp	Tue Aug 18 09:15:09 2015 +0000
+++ b/Dynamixel.cpp	Wed Aug 19 05:36:55 2015 +0000
@@ -78,16 +78,58 @@
     // 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[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
-        packetBuffer[8] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + + packetBuffer[7]) & 0xff;   // Check sum
+        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++) {

--- a/Dynamixel.h	Tue Aug 18 09:15:09 2015 +0000
+++ b/Dynamixel.h	Wed Aug 19 05:36:55 2015 +0000
@@ -14,6 +14,7 @@
 const unsigned char ADDRESS_PRESENT_TEMPERATURE = 0x2B;
 const unsigned char ADDRESS_GOAL_POSITION = 0x1E;
 const unsigned char ADDRESS_LED = 0x19;
+const unsigned char ADDRESS_MOVING_SPEED = 0x20;
 
 class Dynamixel
 {
@@ -22,6 +23,7 @@
     void ping();
     void toggleLED(uint8_t ledState);
     void move(uint16_t position);
+    void setSpeed(uint16_t speed);
 private:
     Serial m_link;
     DigitalOut m_txEnable;

--- a/main.cpp	Tue Aug 18 09:15:09 2015 +0000
+++ b/main.cpp	Wed Aug 19 05:36:55 2015 +0000
@@ -1,3 +1,20 @@
+/*****************************************************
+- Description: mbed to Dynamixel connection test using
+    the library
+- Requirements: 
+    Dynamixel (i.e. DX116, RX28)
+    MAX3088/MAX485 (RS485 transceiver)
+- Connections:
+    MAX3088     --  mbed
+    ======================
+    Pin 1       --  Pin 14
+    Pin 2       --  Pin 15
+    Pin 4       --  Pin 13
+    
+- Comments:
+    See schematic for wiring details and class 
+    documentation for available methods.
+*****************************************************/
 #include "mbed.h"
 #include "Dynamixel.h"
 
@@ -8,7 +25,15 @@
     DX116.toggleLED(1);
     wait(0.5);
     DX116.toggleLED(0);
-    DX116.move(0);
+    DX116.setSpeed(0);
+    wait(0.5);
+    DX116.move(0);          // MOVE
     wait(1);
-    DX116.move(1023);
+    DX116.setSpeed(100);
+    wait(0.5);
+    DX116.move(1023);       // MOVE
+    wait(1);
+    DX116.setSpeed(1023);
+    wait(1);
+    DX116.move(0);          // MOVE
 }   
\ No newline at end of file