Robotis Dynamixel MX-12W Servo Library

Dependents:   SpindleBot_1_5b Utilisatio_MX12_V4

/media/uploads/labmrd/mx12.jpg

This is my attempt to adapt Chris Styles's AX12 library to work with my Dynamixel MX12 servos. This library is still very much a work in progress, and it may have some/many errors in it, but hopefully I will keep improving it to bring it up to snuff.

Dynamixel aficionados should also check out This MX28 library for a completely separate library that provides very similar functionality, and I wish I had known it existed before I started my work...

minimal example

#include "mbed.h"
#include "MX12.h"

int main() {

  MX12 mymx12 (p9, p10, 1);           // ID=1

  while (1) {
      mymx12.Set_Goal_Position(0);    // go to 0 degrees
      wait (2.0);
      mymx12.Set_Goal_Position(300);  // go to 300 degrees
      wait (2.0);
  }
}

Files at this revision

API Documentation at this revision

Comitter:
labmrd
Date:
Tue Feb 10 21:52:40 2015 +0000
Parent:
4:6e320b7646ff
Commit message:
Now with buffer chip!

Changed in this revision

MX12.cpp Show annotated file Show diff for this revision Revisions of this file
MX12.h Show annotated file Show diff for this revision Revisions of this file
diff -r 6e320b7646ff -r 4c118a827f11 MX12.cpp
--- a/MX12.cpp	Thu Jan 29 22:24:13 2015 +0000
+++ b/MX12.cpp	Tue Feb 10 21:52:40 2015 +0000
@@ -8,13 +8,16 @@
 MX12OD_Object MX12_OD[MX12_OD_SIZE];
 bool MX12OD_Object_initalized;
 
-MX12::MX12(PinName tx, PinName rx, int ID, int baud_rate)
+MX12::MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate)
         : mx12_out(tx, NC),
           mx12_in(NC, rx),
+          tx_enable(tx_enable_pin),
+          rx_enable(rx_enable_pin),
           profileOut(LED4) {
     mx12_out.baud(baud_rate);
     mx12_in.baud(baud_rate);
     _ID = ID;
+    _baud = baud_rate;
 }
 
 
@@ -59,6 +62,31 @@
     MX12OD_Object_initalized=true;
 }
 
+// We shouldn't need to wait between setting pins, 
+// since the switching time of the chip is ~6 
+// nanoseconds, and a cpu cycle on the LPC1768 
+// is ~10 nanoseconds
+void MX12::ChangeDir(MX12_Direction dir) {
+    if(dir==MX12_DIR_IN){
+        // Turn off transmit first
+        tx_enable=1;
+        // Then enable receive
+        rx_enable=0;
+    }else if(dir==MX12_DIR_OUT){
+        // Turn off receive first
+        rx_enable=1;
+        // Then enable transmit
+        tx_enable=0;
+    }else if(dir==MX12_DIR_NONE){
+        // Turn off both
+        tx_enable=1;
+        rx_enable=1;
+    }else{
+        // This shouldn't be possible!
+        printf("Error: Incorrect Direction choice!\n");
+    }
+}
+
 // Set the mode of the servo
 //  0 = Positional (0-300 degrees)
 //  1 = Rotational -1 to 1 speed
@@ -124,11 +152,15 @@
     if(scan_all_baud_rates){
         int baud_rate[12]={3000000,2500000,2250000,1000000,500000,400000,250000,200000,115200,57600,19200,9600};
         char ii;
-        for(ii=0;ii<12;ii++){
-            printf("baud rate=%d\n",baud_rate[ii]);
+        for(ii=0;ii<4;ii++){
             ChangeUARTBaud(baud_rate[ii]);
             for(ID_Num=0;ID_Num<=max_id;ID_Num++){
-                printf("ID Num=%d\n",ID_Num);
+                //_ID=ID_Num;
+//                char data[2];
+//                data[1]=0;
+//            
+//                int ErrorCode = read(_ID, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Address, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Bytes, data);
+//                if(ErrorCode!=0){
                 if(ping(ID_Num)){
                     printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]);
                 }
@@ -158,7 +190,7 @@
         baud_int=252;
     }else{
         printf("Error!  Invalid baud rate of %d!\n",target_baud);
-        return 0;
+        return MX12_ERROR_RETURN;
     }
     
     #if MX12_DEBUG
@@ -195,6 +227,7 @@
 
     // write the packet, return the error code
     int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
+    if(rVal==MX12_ERROR_RETURN){return rVal;}
     if(rVal>0){
         if(CHECK_BIT(rVal, 0)){printf("Error! Input Voltage Error\n");}
         if(CHECK_BIT(rVal, 1)){printf("Error! Angle Limit Error\n");}
@@ -211,8 +244,7 @@
 short MX12:: read_short(MX12ODIndex OD)
 {
     
-    char data[2];
-    data[1]=0;
+    char data[2]={0,0};
 
     int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
     profileOut = (ErrorCode!=0);
@@ -233,7 +265,71 @@
 
 
 
+int MX12::read_raw(char* Status, int bytes) {
+    // Receive the Status packet 6+ number of bytes read
+    #if MX12_READ_DEBUG
+        printf("  Reading Byte:");
+    #endif
+    Timer serial_timeout;
+    serial_timeout.start ();
+    
+    for (int i=0; i<(6+bytes) ; i++) {
+        serial_timeout.reset ();
+        while (!mx12_in.readable ())
+        {
+            //Just loop here until you either get a character, or we timeout
+            if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
+            {
+                //If we timeout, quit in a panic!
+                //printf("Error! Serial Timeout %d\n",i);
+                return(MX12_ERROR_RETURN);
+            }
+        }
+        Status[i] = mx12_in.getc();
+    }
+    
+    if(Status[0]!=0xFF || Status[1]!=0xFF)
+    {
+        printf("Header Error!\n");
+        //printf("Unexpected header in serial response!\n");
+        //printf("  Header : 0x%x\n",Status[0]);
+        //printf("  Header : 0x%x\n",Status[1]);
+        return MX12_ERROR_RETURN;
+    }
+    if(Status[2]!=_ID){
+        printf("ID Error!\n");
+        return MX12_ERROR_RETURN;
+    }
+    if(Status[3]!=bytes+2){
+        printf("Length Error!\n");
+        return MX12_ERROR_RETURN;
+    }
+    
+    char sum=Status[2]+Status[3]+Status[4]+Status[5];
+    if( bytes==2 )
+        sum+=Status[6];
+    if( Status[5+bytes]!=0xff-sum){
+        printf("Checksum Error!\n");
+        return MX12_ERROR_RETURN;
+    }
+    
+    #if MX12_READ_DEBUG
+        printf("\nStatus Packet\n");
+        printf("  Header : 0x%x\n",Status[0]);
+        printf("  Header : 0x%x\n",Status[1]);
+        printf("  ID : 0x%x\n",Status[2]);
+        printf("  Length : 0x%x\n",Status[3]);
+        printf("  Error Code : 0x%x\n",Status[4]);
 
+        for (int i=0; i < bytes ; i++) {
+            printf("  Data : 0x%x\n",Status[5+i]);
+        }
+
+        printf("  Checksum : 0x%x\n",Status[5+bytes]);
+    #endif
+    
+    return MX12_NORMAL_RETURN;
+}
 
 int MX12::read(int ID, int start, int bytes, char* data) {
     
@@ -298,96 +394,45 @@
     #endif
     
     // Clear in input buffer first
-    while (mx12_in.readable()) {
+    int buffer_purge_count=0;
+    while (mx12_in.readable() && buffer_purge_count < 16 ) {
         mx12_in.getc();
-        printf("Purging one character (read).\n");
+        buffer_purge_count++;
+        //printf("Purging one character (0x%x).\n",c);
     }
+    if(buffer_purge_count > 1){ // One character is normal, I don't know why...
+        printf("Error: Purged %d characters from buffer.\n",buffer_purge_count);
+        return MX12_ERROR_RETURN;
+    }
+    
+    // Change to output
+    ChangeDir(MX12_DIR_OUT);
+    wait_us(1); // Debounce
     
     // Transmit the packet in one burst with no pausing
     for (int i = 0; i<8 ; i++) {
         mx12_out.putc(TxBuf[i]);
     }
     
-    // Read
-    for (int i = 0; i<8 ; i++) {
-        //profileOut=i%2;
-        mx12_in.getc();
+    // Wait for the bytes to be transmitted
+    // This shouldn't overflow with 32 bit ints, but be aware
+    //       us/s    bytes  b/byte   baud
+    wait_us(1000000  *  8  *  8  /  _baud);
+    
+    // Change to input
+    ChangeDir(MX12_DIR_IN);
+    wait_us(1); // Debounce
+
+    if(ID!=0xFE){
+        if(read_raw(Status,bytes)!=MX12_NORMAL_RETURN){
+            return MX12_ERROR_RETURN;
+        }
     }
     
-    // Wait for the bytes to be transmitted
-    //wait (0.00002);
-
-    // Skip if the read was to the broadcast address
-    if (_ID != 0xFE) {
-
-        // Receive the Status packet 6+ number of bytes read
-        #if MX12_READ_DEBUG
-            printf("  Reading Byte:");
-        #endif
-        Timer serial_timeout;
-        serial_timeout.start ();
-        
-        for (int i=0; i<(6+bytes) ; i++) {
-            serial_timeout.reset ();
-            while (!mx12_in.readable ())
-            {
-                //Just loop here until you either get a character, or we timeout
-                if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
-                {
-                    //If we timeout, quit in a panic!
-                    printf("Error! Serial Timeout %d\n",i);
-                    return(0x00);
-                }
-            }
-            Status[i] = mx12_in.getc();
-        }
-        
-        if(Status[0]!=0xFF || Status[1]!=0xFF)
-        {
-            printf("Header Error!\n");
-            //printf("Unexpected header in serial response!\n");
-            //printf("  Header : 0x%x\n",Status[0]);
-            //printf("  Header : 0x%x\n",Status[1]);
-            return 255;
-        }
-        if(Status[2]!=_ID){
-            printf("ID Error!\n");
-            return 255;
-        }
-        if(Status[3]!=bytes+2){
-            printf("Length Error!\n");
-            return 255;
-        }
-        
-        char sum=Status[2]+Status[3]+Status[4]+Status[5];
-        if( bytes==2 )
-            sum+=Status[6];
-        if( Status[5+bytes]!=0xff-sum){
-            printf("Checksum Error!\n");
-            return 255;
-        }
-
-        // Copy the data from Status into data for return
-        for (int i=0; i < bytes ; i++) {
-            data[i] = Status[5+i];
-        }
-
-        #if MX12_READ_DEBUG
-            printf("\nStatus Packet\n");
-            printf("  Header : 0x%x\n",Status[0]);
-            printf("  Header : 0x%x\n",Status[1]);
-            printf("  ID : 0x%x\n",Status[2]);
-            printf("  Length : 0x%x\n",Status[3]);
-            printf("  Error Code : 0x%x\n",Status[4]);
-
-            for (int i=0; i < bytes ; i++) {
-                printf("  Data : 0x%x\n",Status[5+i]);
-            }
-
-            printf("  Checksum : 0x%x\n",Status[5+bytes]);
-        #endif
-
-    } // if (ID!=0xFE)
+    // Copy the data from Status into data for return
+    for (int i=0; i < bytes ; i++) {
+        data[i] = Status[5+i];
+    }
 
     return(Status[4]);
 }
@@ -469,63 +514,33 @@
         mx12_in.getc();
         printf("Purging one character (write).\n");
     }
-
+    
+    // Change to output
+    ChangeDir(MX12_DIR_OUT);
+    wait_us(1);
+    
     // Transmit the packet in one burst with no pausing
-    for (int i = 0; i < (7 + bytes) ; i++) {
+    for (int i = 0; i< (7 + bytes) ; i++) {
         mx12_out.putc(TxBuf[i]);
-        mx12_in.getc();
-        //printf("Echo: 0x%02x\n",mx12_in.getc());
     }
+    
+    // Wait for the bytes to be transmitted
+    // This shouldn't overflow with 32 bit ints, but be aware
+    //       us/s          bytes      b/byte   baud    fudge
+    wait_us(1000000  *  (7 + bytes)  *  8  /  _baud  +  1);
+    
+    // Change to input
+    ChangeDir(MX12_DIR_IN);
+    wait_us(1);
 
-    // make sure we have a valid return
-    Status[4]=0x00;
+    // make sure we have an invalid return
+    Status[4]=MX12_ERROR_RETURN;
 
     // we'll only get a reply if it was not broadcast
-    if (_ID!=0xFE) {
-
-        // response is always 6 bytes
-        // 0xFF, 0xFF, ID, Length, Error, Checksum
-        Timer serial_timeout;
-        serial_timeout.start ();
-        
-        for (int i=0; i < 6 ; i++) {
-            serial_timeout.reset ();
-            while (!mx12_in.readable ())
-            {
-                //Just loop here until you either get a character, or we timeout
-                if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
-                {
-                    //If we timeout, quit in a panic!
-                    printf("Error! Serial Timeout %d\n",i);
-                    return(0x00);
-                }
-            }
-            Status[i] = mx12_in.getc();
+    if(ID!=0xFE){
+        if(read_raw(Status,0)!=MX12_NORMAL_RETURN){
+            return MX12_ERROR_RETURN;
         }
-        
-        if(Status[0]!=0xFF || Status[1]!=0xFF)
-        {
-            printf("Unexpected header in serial response!\n");
-            printf("  Header : 0x%x\n",Status[0]);
-            printf("  Header : 0x%x\n",Status[1]);
-            return 255;
-        }
-        if( Status[5]!=0xff-(Status[2]+Status[3]+Status[4])){
-            printf("Checksum Error!\n");
-            return 255;
-        }
-            
-        
-        // Build the TxPacket first in RAM, then we'll send in one go
-        #if MX12_WRITE_DEBUG
-            printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
-            printf("  ID : %d\n",Status[2]);
-            printf("  Length : %d\n",Status[3]);
-            printf("  Error : 0x%x\n",Status[4]);
-            printf("  Checksum : 0x%x\n",Status[5]);
-        #endif
-
-
     }
 
     return(Status[4]); // return error code
@@ -644,16 +659,21 @@
     
     // Transmit the packet in one burst with no pausing
     offset=7+NumDevices*(DataLength+0x1)+0x1; // one more for the checksum
-    for (ii = 0; ii<offset ; ii++) {
-        mx12_out.putc(TxBuf[ii]);
-        mx12_in.getc();
-        //printf("0x%x",TxBuf[ii]);
+    
+    
+    // Change to output
+    ChangeDir(MX12_DIR_OUT);
+    wait_us(1);
+    
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i< offset ; i++) {
+        mx12_out.putc(TxBuf[i]);
     }
     
-    // Read
-    for (ii = 0; ii<offset ; ii++) {
-        //profileOut=ii%2;
-    }
+    // Wait for the bytes to be transmitted
+    // This shouldn't overflow with 32 bit ints, but be aware
+    //       us/s       bytes    b/byte   baud
+    wait_us(1000000  *  offset  *  8  /  _baud);
 }
 
 void MX12::trigger(void) {
@@ -766,18 +786,31 @@
     #if MX12_TRIGGER_DEBUG
         printf("  Checksum 0x%X\n",TxBuf[5]);
     #endif
+    
+    
+    // Clear in input buffer first
+    while (mx12_in.readable()) {
+        mx12_in.getc();
+        printf("Purging one character (ping).\n");
+    }
 
+    // Change to output
+    ChangeDir(MX12_DIR_OUT);
+    wait_us(1);
+    
     // Transmit the packet in one burst with no pausing
-    for (int i = 0; i < 6 ; i++) {
+    for (int i = 0; i<6 ; i++) {
         mx12_out.putc(TxBuf[i]);
     }
     
-    // Read
-    for (int i = 0; i < 6 ; i++) {
-        //profileOut=i%2;
-        mx12_in.getc();
-    }
-
+    // Wait for the bytes to be transmitted
+    // This shouldn't overflow with 32 bit ints, but be aware
+    //       us/s    bytes  b/byte   baud    fudge
+    wait_us(1000000  *  6  *  8  /  _baud  +  1);
+    
+    // Change to input
+    ChangeDir(MX12_DIR_IN);
+    wait_us(1);
     
     // response is always 6 bytes
     // 0xFF, 0xFF, ID, Length, Error, Checksum
diff -r 6e320b7646ff -r 4c118a827f11 MX12.h
--- a/MX12.h	Thu Jan 29 22:24:13 2015 +0000
+++ b/MX12.h	Tue Feb 10 21:52:40 2015 +0000
@@ -68,17 +68,28 @@
 #define MX12_MODE_POSITION  0
 #define MX12_MODE_ROTATION  1
 
+// For now we don't have a both option, since no
+// one should use that anyways.
+enum MX12_Direction {
+    MX12_DIR_IN,
+    MX12_DIR_OUT,
+    MX12_DIR_NONE
+};
+
 #define MX12_CW 1
 #define MX12_CCW 0
 
 #define MX12_INSTRUCTION_HEADER 0xff
 
+#define MX12_ERROR_RETURN       255
+#define MX12_NORMAL_RETURN        1
+
 // The max delay should be 508us according to:
 // http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_05
 // And a max character should be 833 us according to:
 // (1 byte) / (9600 (bits per second)) = 833 microseconds
 // So 1000 should be a decent value, because 9600 bps is for chumps.
-#define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 500
+#define MAX_DELAY_BETWEEN_CHARCTERS_IN_US 1500
 
 #ifndef M_PI
     #define M_PI    3.14159265358979323846  /* pi */
@@ -119,9 +130,11 @@
      * @param pin rx pin 
      * @param int ID, the Bus ID of the servo 1-255 
      */
-    MX12(PinName tx, PinName rx, int ID, int baud_rate=1000000);
+    MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate=1000000);
 
     void Init(void);
+    
+    void ChangeDir(MX12_Direction dir);
 
 /** clockwise Angle Limit @retval CW Angle Limit in Degrees */  float Get_CW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CW_ANGLE_LIMIT);}
 /** counterclockwise Angle Limit @retval CCW Angle Limit in Degrees */  float Get_CCW_Angle_Limit(void){return 0.087891*read_short(MX12_REG_CCW_ANGLE_LIMIT);}
@@ -289,10 +302,14 @@
     //// Variables
     Serial mx12_out;
     Serial mx12_in;
+    DigitalOut tx_enable;
+    DigitalOut rx_enable;
     DigitalOut profileOut;
     int _ID;
+    int _baud;
 
     //// Functions
+    int read_raw(char* Status, int bytes=0);
     int read(int ID, int start, int length, char* data);
     int write(int ID, int start, int length, char* data);