Revision 5:4c118a827f11, committed 2015-02-10

Comitter:

labmrd

Date:

Tue Feb 10 21:52:40 2015 +0000

Parent:

4:6e320b7646ff

Commit message:

Now with buffer chip!

Changed in this revision

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

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