Revision 0:79e2a8171b16, committed 2015-08-18

Comitter:

dmgongora

Date:

Tue Aug 18 09:15:09 2015 +0000

Child:

1:ef90d942ce78

Commit message:

First working version of the Dynamixel class

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel.cpp	Tue Aug 18 09:15:09 2015 +0000
@@ -0,0 +1,112 @@
+#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];
+    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
+        packetBuffer[8] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4] + packetBuffer[5] + packetBuffer[6] + + packetBuffer[7]) & 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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Dynamixel.h	Tue Aug 18 09:15:09 2015 +0000
@@ -0,0 +1,31 @@
+#ifndef DEF_DYNAMIXEL
+#define DEF_DYNAMIXEL
+
+#include "mbed.h"
+// Instruction set
+const unsigned char PING = 0x01;
+const unsigned char READ_DATA = 0x02;
+const unsigned char WRITE_DATA = 0x03;
+const unsigned char REG_WRITE = 0x04;
+const unsigned char ACTION = 0x05;
+const unsigned char RESET = 0x06;
+const unsigned char SYNC_WRITE = 0x83;
+// Control table
+const unsigned char ADDRESS_PRESENT_TEMPERATURE = 0x2B;
+const unsigned char ADDRESS_GOAL_POSITION = 0x1E;
+const unsigned char ADDRESS_LED = 0x19;
+
+class Dynamixel
+{
+public:
+    Dynamixel(PinName tx, PinName rx, PinName txEnable, uint8_t motorID, int baudrate);
+    void ping();
+    void toggleLED(uint8_t ledState);
+    void move(uint16_t position);
+private:
+    Serial m_link;
+    DigitalOut m_txEnable;
+    uint8_t m_motorID;
+    const int m_baudrate;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 18 09:15:09 2015 +0000
@@ -0,0 +1,14 @@
+#include "mbed.h"
+#include "Dynamixel.h"
+
+int main()
+{
+    Dynamixel DX116(p13, p14, p15, 9, 57600);
+    
+    DX116.toggleLED(1);
+    wait(0.5);
+    DX116.toggleLED(0);
+    DX116.move(0);
+    wait(1);
+    DX116.move(1023);
+}   
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Aug 18 09:15:09 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b9ad9a133dc7
\ No newline at end of file