Diff: MX12.cpp

Revision:

1:946a27210553

Parent:

0:29900c3a4a50

Child:

3:624d04c390b8

--- a/MX12.cpp	Tue Nov 25 03:07:36 2014 +0000
+++ b/MX12.cpp	Mon Jan 26 04:02:37 2015 +0000
@@ -1,274 +1,230 @@
-/* mbed AX-12+ Servo Library
- *
- * Copyright (c) 2010, cstyles (http://mbed.org)
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
+/* mbed MX-12 Servo Library
  *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
  */
 
 #include "MX12.h"
 #include "mbed.h"
 
-#ifndef M_PI
-    #define M_PI    3.14159265358979323846  /* pi */
-#endif
-#ifndef M_PI_2
-    #define M_PI_2  1.57079632679489661923  /* pi/2 */
-#endif
+MX12OD_Object MX12_OD[MX12_OD_SIZE];
+bool MX12OD_Object_initalized;
+
+MX12::MX12(PinName tx, PinName rx, int ID, int baud_rate)
+        : mx12_out(tx, NC),
+          mx12_in(NC, rx),
+          profileOut(p12) {
+    mx12_out.baud(baud_rate);
+    mx12_in.baud(baud_rate);
+    _ID = ID;
+}
+
 
-MX12::MX12(PinName tx, PinName rx, int ID)
-        : _mx12(tx,rx) {
-
-    _mx12.baud(1000000);//57600);//1000000);
-    _ID = ID;
+void MX12::Init(void){
+    if(MX12OD_Object_initalized){
+        return;
+    }
+    MX12_OD[MX12_REG_MODEL_NUMBER                   ].Address=0;    MX12_OD[MX12_REG_MODEL_NUMBER                   ].Bytes=2;
+    MX12_OD[MX12_REG_VERSION_OF_FIRMWARE            ].Address=2;    MX12_OD[MX12_REG_VERSION_OF_FIRMWARE            ].Bytes=1;
+    MX12_OD[MX12_REG_ID                             ].Address=3;    MX12_OD[MX12_REG_ID                             ].Bytes=1;
+    MX12_OD[MX12_REG_BAUD_RATE                      ].Address=4;    MX12_OD[MX12_REG_BAUD_RATE                      ].Bytes=1;
+    MX12_OD[MX12_REG_RETURN_DELAY_TIME              ].Address=5;    MX12_OD[MX12_REG_RETURN_DELAY_TIME              ].Bytes=1;
+    MX12_OD[MX12_REG_CW_ANGLE_LIMIT                 ].Address=6;    MX12_OD[MX12_REG_CW_ANGLE_LIMIT                 ].Bytes=2;
+    MX12_OD[MX12_REG_CCW_ANGLE_LIMIT                ].Address=8;    MX12_OD[MX12_REG_CCW_ANGLE_LIMIT                ].Bytes=2;
+    MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE  ].Address=11;   MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE  ].Bytes=1;
+    MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE       ].Address=12;   MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE       ].Bytes=1;
+    MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE      ].Address=13;   MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE      ].Bytes=1;
+    MX12_OD[MX12_REG_MAX_TORQUE                     ].Address=14;   MX12_OD[MX12_REG_MAX_TORQUE                     ].Bytes=2;
+    MX12_OD[MX12_REG_STATUS_RETURN_LEVEL            ].Address=16;   MX12_OD[MX12_REG_STATUS_RETURN_LEVEL            ].Bytes=1;
+    MX12_OD[MX12_REG_ALARM_LED                      ].Address=17;   MX12_OD[MX12_REG_ALARM_LED                      ].Bytes=1;
+    MX12_OD[MX12_REG_ALARM_SHUTDOWN                 ].Address=18;   MX12_OD[MX12_REG_ALARM_SHUTDOWN                 ].Bytes=1;
+    MX12_OD[MX12_REG_MULTI_TURN_OFFSET              ].Address=20;   MX12_OD[MX12_REG_MULTI_TURN_OFFSET              ].Bytes=2;
+    MX12_OD[MX12_REG_RESOLUTION_DIVIDER             ].Address=22;   MX12_OD[MX12_REG_RESOLUTION_DIVIDER             ].Bytes=1;
+    MX12_OD[MX12_REG_TORQUE_ENABLE                  ].Address=24;   MX12_OD[MX12_REG_TORQUE_ENABLE                  ].Bytes=1;
+    MX12_OD[MX12_REG_LED                            ].Address=25;   MX12_OD[MX12_REG_LED                            ].Bytes=1;
+    MX12_OD[MX12_REG_D_GAIN                         ].Address=26;   MX12_OD[MX12_REG_D_GAIN                         ].Bytes=1;
+    MX12_OD[MX12_REG_I_GAIN                         ].Address=27;   MX12_OD[MX12_REG_I_GAIN                         ].Bytes=1;
+    MX12_OD[MX12_REG_P_GAIN                         ].Address=28;   MX12_OD[MX12_REG_P_GAIN                         ].Bytes=1;
+    MX12_OD[MX12_REG_GOAL_POSITION                  ].Address=30;   MX12_OD[MX12_REG_GOAL_POSITION                  ].Bytes=2;
+    MX12_OD[MX12_REG_MOVING_SPEED                   ].Address=32;   MX12_OD[MX12_REG_MOVING_SPEED                   ].Bytes=2;
+    MX12_OD[MX12_REG_TORQUE_LIMIT                   ].Address=34;   MX12_OD[MX12_REG_TORQUE_LIMIT                   ].Bytes=2;
+    MX12_OD[MX12_REG_PRESENT_POSITION               ].Address=36;   MX12_OD[MX12_REG_PRESENT_POSITION               ].Bytes=2;
+    MX12_OD[MX12_REG_PRESENT_SPEED                  ].Address=38;   MX12_OD[MX12_REG_PRESENT_SPEED                  ].Bytes=2;
+    MX12_OD[MX12_REG_PRESENT_LOAD                   ].Address=40;   MX12_OD[MX12_REG_PRESENT_LOAD                   ].Bytes=2;
+    MX12_OD[MX12_REG_PRESENT_VOLTAGE                ].Address=42;   MX12_OD[MX12_REG_PRESENT_VOLTAGE                ].Bytes=1;
+    MX12_OD[MX12_REG_PRESENT_TEMPERATURE            ].Address=43;   MX12_OD[MX12_REG_PRESENT_TEMPERATURE            ].Bytes=1;
+    MX12_OD[MX12_REG_REGISTERED                     ].Address=44;   MX12_OD[MX12_REG_REGISTERED                     ].Bytes=1;
+    MX12_OD[MX12_REG_MOVING                         ].Address=46;   MX12_OD[MX12_REG_MOVING                         ].Bytes=1;
+    MX12_OD[MX12_REG_LOCK                           ].Address=47;   MX12_OD[MX12_REG_LOCK                           ].Bytes=1;
+    MX12_OD[MX12_REG_PUNCH                          ].Address=48;   MX12_OD[MX12_REG_PUNCH                          ].Bytes=2;
+    MX12_OD[MX12_REG_GOAL_ACCELERATION              ].Address=73;   MX12_OD[MX12_REG_GOAL_ACCELERATION              ].Bytes=1;
+    MX12OD_Object_initalized=true;
 }
 
 // Set the mode of the servo
 //  0 = Positional (0-300 degrees)
 //  1 = Rotational -1 to 1 speed
 int MX12::SetMode(int mode) {
-
-    if (mode == 1) { // set CR
-        SetCWLimit(0);
-        SetCCWLimit(0);
-        SetCRSpeed(0.0);
-    } else {
-        SetCWLimit(0);
-        SetCCWLimit(300);
-        SetCRSpeed(0.0);
-    }
+////
+////Need to implement this as per http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_06
+////
+////Maybe also make a read mode?
+////
+//    if (mode == 1) { // set CR
+//        SetCWLimit(0);
+//        SetCCWLimit(0);
+//        SetCRSpeed(0.0);
+//    } else {
+//        SetCWLimit(0);
+//        SetCCWLimit(300);
+//        SetCRSpeed(0.0);
+//    }
     return(0);
 }
 
 
-// if flag[0] is set, were blocking
-// if flag[1] is set, we're registering
-// they are mutually exclusive operations
-int MX12::SetGoal(float radians, int flags) {
+void MX12::Dump_OD_to_Serial(Serial &serialObject){
+    serialObject.printf("CW Angle Limit = %f Degrees\n",Get_CW_Angle_Limit());
+    serialObject.printf("CCW Angle Limit = %f Degrees\n",Get_CCW_Angle_Limit());
+    serialObject.printf("Max Torque = %f Percent\n",Get_Max_Torque());
+    serialObject.printf("Multi Turn Offset = %f Degrees\n",Get_Multi_Turn_Offset());
+    serialObject.printf("Goal Position = %f Degrees\n",Get_Goal_Position());
+    serialObject.printf("Moving Speed = %f Degrees/Second\n",Get_Moving_Speed());
+    serialObject.printf("Torque Limit = %f Percent\n",Get_Torque_Limit());
+    serialObject.printf("Punch = %f Percent\n",Get_Punch());
+    serialObject.printf("ID = %f int\n",Get_ID());
+    serialObject.printf("Baud Rate = %f Lookup\n",Get_Baud_Rate());
+    serialObject.printf("Return Delay Time = %f milliseconds\n",Get_Return_Delay_Time());
+    serialObject.printf("the Highest Limit Temperature = %f Celsius\n",Get_the_Highest_Limit_Temperature());
+    serialObject.printf("the Lowest Limit Voltage = %f Volts\n",Get_the_Lowest_Limit_Voltage());
+    serialObject.printf("the Highest Limit Voltage = %f Volts\n",Get_the_Highest_Limit_Voltage());
+    serialObject.printf("Status Return Level = %f int\n",Get_Status_Return_Level());
+    serialObject.printf("Alarm LED = %f Bitmap\n",Get_Alarm_LED());
+    serialObject.printf("Alarm Shutdown = %f Bitmap\n",Get_Alarm_Shutdown());
+    serialObject.printf("Resolution Divider = %f Ratio\n",Get_Resolution_Divider());
+    serialObject.printf("Torque Enable = %f bool\n",Get_Torque_Enable());
+    serialObject.printf("LED = %f Bitmap\n",Get_LED());
+    serialObject.printf("D Gain = %f Kd\n",Get_D_Gain());
+    serialObject.printf("I Gain = %f Ki\n",Get_I_Gain());
+    serialObject.printf("P Gain = %f Kp\n",Get_P_Gain());
+    serialObject.printf("Lock = %f bool\n",Get_Lock());
+    serialObject.printf("Goal Acceleration = %f Degrees/Second^2\n",Get_Goal_Acceleration());
+    serialObject.printf("Model Number = %f Bitmap\n",Get_Model_Number());
+    serialObject.printf("Present Position = %f Degrees\n",Get_Present_Position());
+    serialObject.printf("Present Speed = %f Degrees/Second\n",Get_Present_Speed());
+    serialObject.printf("Present Load = %f Percent\n",Get_Present_Load());
+    serialObject.printf("Version of Firmware = %f int\n",Get_Version_of_Firmware());
+    serialObject.printf("Present Voltage = %f Volts\n",Get_Present_Voltage());
+    serialObject.printf("Present Temperature = %f Celsius\n",Get_Present_Temperature());
+    serialObject.printf("Registered = %f bool\n",Get_Registered());
+    serialObject.printf("Moving = %f bool\n",Get_Moving());
+}
 
-    char reg_flag = 0;
+void MX12::Scan_For_Dynamixels(bool scan_all_baud_rates,int max_id)
+{
+    char ID_Num;
+    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++){
+            ChangeUARTBaud(baud_rate[ii]);
+            for(ID_Num=0;ID_Num<=max_id;ID_Num++){
+                if(ping(ID_Num)){
+                    printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]);
+                }
+            }
+        }
+    }else{
+        for(ID_Num=0;ID_Num<=max_id;ID_Num++){
+            if(ping(ID_Num)){
+                printf("Found a servo at ID=%#02x\n",ID_Num);
+            }
+        }
+    }
+    
+}
+
+int MX12::SetBaud( int target_baud ) {
+    
+    short baud_int;
+    
+    if(target_baud<=1000000){
+        baud_int=2000000/target_baud-1;
+    }else if(target_baud==2250000){
+        baud_int=250;
+    }else if(target_baud==2500000){
+        baud_int=251;
+    }else if(target_baud==3000000){
+        baud_int=252;
+    }else{
+        printf("Error!  Invalid baud rate of %d!\n",target_baud);
+        return 0;
+    }
+    
+    #if MX12_DEBUG
+        printf("SetBaudRate to 0x%x\n",baud_int);
+    #endif
+    int ret=(write_short(MX12_REG_BAUD_RATE,baud_int));
+    
+    // Now change the UART serial rate to the same?
+    // But to allow changing several servos, allow user
+    // to do this at their liesure.
+    // ChangeUARTBaud( target_baud );
+    
+    return ret;
+}
+
+void MX12::ChangeUARTBaud( int target_baud ) {
+    mx12_out.baud(target_baud);
+    mx12_in.baud(target_baud);
+}
+
+short MX12::GetRawPosition(void) {
+    #if MX12_DEBUG
+        printf("\nGetRawPosition(%d)",_ID);
+    #endif
+    return read_short(MX12_REG_PRESENT_POSITION);
+}
+
+int MX12:: write_short(MX12ODIndex OD,short value)
+{
     char data[2];
 
-    // set the flag is only the register bit is set in the flag
-    if (flags == 0x2) {
-        reg_flag = 1;
-    }
-
-    // 1023 / 300 * degrees
-    short goal = (2048 * radians) / M_PI;
-    if (MX12_DEBUG) {
-        printf("SetGoal to 0x%x\n",goal);
-    }
-
-    data[0] = goal & 0xff; // bottom 8 bits
-    data[1] = goal >> 8;   // top 8 bits
+    data[0] = value & 0xff; // bottom 8 bits
+    data[1] = value >> 8;   // top 8 bits
 
     // write the packet, return the error code
-    int rVal = write(_ID, MX12_REG_GOAL_POSITION, 2, data, reg_flag);
-
-    if (flags == 1) {
-        // block until it comes to a halt
-        while (isMoving()) {}
-    }
-    return(rVal);
-}
-
-
-// Set continuous rotation speed from -1 to 1
-int MX12::SetCRSpeed(float speed) {
-
-    // bit 10     = direction, 0 = CCW, 1=CW
-    // bits 9-0   = Speed
-    char data[2];
-
-    int goal = (0x3ff * abs(speed));
-
-    // Set direction CW if we have a negative speed
-    if (speed < 0) {
-        goal |= (0x1 << 10);
-    }
-
-    data[0] = goal & 0xff; // bottom 8 bits
-    data[1] = goal >> 8;   // top 8 bits
-
-    // write the packet, return the error code
-    int rVal = write(_ID, 0x20, 2, data);
+    int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
 
     return(rVal);
 }
 
-
-int MX12::SetCWLimit (int degrees) {
-
-    char data[2];
+short MX12:: read_short(MX12ODIndex OD)
+{
     
-    // 1023 / 300 * degrees
-    short limit = (1023 * degrees) / 300;
-
-    if (MX12_DEBUG) {
-        printf("SetCWLimit to 0x%x\n",limit);
-    }
-
-    data[0] = limit & 0xff; // bottom 8 bits
-    data[1] = limit >> 8;   // top 8 bits
-
-    // write the packet, return the error code
-    return (write(_ID, MX12_REG_CW_LIMIT, 2, data));
-
-}
-
-int MX12::SetCCWLimit (int degrees) {
-
     char data[2];
-
-    // 1023 / 300 * degrees
-    short limit = (1023 * degrees) / 300;
+    data[1]=0;
 
-    if (MX12_DEBUG) {
-        printf("SetCCWLimit to 0x%x\n",limit);
-    }
-
-    data[0] = limit & 0xff; // bottom 8 bits
-    data[1] = limit >> 8;   // top 8 bits
-
-    // write the packet, return the error code
-    return (write(_ID, MX12_REG_CCW_LIMIT, 2, data));
-}
-
-
-int MX12::SetID (int CurrentID, int NewID) {
-
-    char data[1];
-    data[0] = NewID;
-    if (MX12_DEBUG) {
-        printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
-    }
-    return (write(CurrentID, MX12_REG_ID, 1, data));
-
-}
-
-
-// return 1 is the servo is still in flight
-int MX12::isMoving(void) {
-
-    char data[1];
-    read(_ID,MX12_REG_MOVING,1,data);
-    return(data[0]);
+    int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
+    short value = data[0] + (data[1] << 8);
+    
+    return (value);
 }
 
 
-void MX12::trigger(void) {
 
-    char TxBuf[16];
-    char sum = 0;
-
-    if (MX12_TRIGGER_DEBUG) {
-        printf("\nTriggered\n");
-    }
-
-    // Build the TxPacket first in RAM, then we'll send in one go
-    if (MX12_TRIGGER_DEBUG) {
-        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
-    }
-
-    TxBuf[0] = 0xFF;
-    TxBuf[1] = 0xFF;
-
-    // ID - Broadcast
-    TxBuf[2] = 0xFE;
-    sum += TxBuf[2];
-
-    if (MX12_TRIGGER_DEBUG) {
-        printf("  ID : %d\n",TxBuf[2]);
-    }
-
-    // Length
-    TxBuf[3] = 0x02;
-    sum += TxBuf[3];
-    if (MX12_TRIGGER_DEBUG) {
-        printf("  Length %d\n",TxBuf[3]);
-    }
-
-    // Instruction - ACTION
-    TxBuf[4] = 0x04;
-    sum += TxBuf[4];
-    if (MX12_TRIGGER_DEBUG) {
-        printf("  Instruction 0x%X\n",TxBuf[5]);
-    }
-
-    // Checksum
-    TxBuf[5] = 0xFF - sum;
-    if (MX12_TRIGGER_DEBUG) {
-        printf("  Checksum 0x%X\n",TxBuf[5]);
-    }
-
-    // Transmit the packet in one burst with no pausing
-    for (int i = 0; i < 6 ; i++) {
-        _mx12.putc(TxBuf[i]);
-    }
-
-    // This is a broadcast packet, so there will be no reply
-
-    return;
-}
+         ////////////////////////////////////////////
+         //                                        //
+         //                                        //
+         //          PRIVATE FUNCTIONS             //
+         //                                        //
+         //                                        //
+         ////////////////////////////////////////////
 
 
-float MX12::GetPosition(void) {
 
-    if (MX12_DEBUG) {
-        printf("\nGetPosition(%d)",_ID);
-    }
-
-    char data[2];
-
-    int ErrorCode = read(_ID, MX12_REG_POSITION, 2, data);
-    short position = data[0] + (data[1] << 8);
-    float angle = (position * M_PI)/2048;
-
-    return (angle);
-}
-
-
-float MX12::GetTemp (void) {
-
-    if (MX12_DEBUG) {
-        printf("\nGetTemp(%d)",_ID);
-    }
-    char data[1];
-    int ErrorCode = read(_ID, MX12_REG_TEMP, 1, data);
-    float temp = data[0];
-    return(temp);
-}
-
-
-float MX12::GetVolts (void) {
-    if (MX12_DEBUG) {
-        printf("\nGetVolts(%d)",_ID);
-    }
-    char data[1];
-    int ErrorCode = read(_ID, MX12_REG_VOLTS, 1, data);
-    float volts = data[0]/10.0;
-    return(volts);
-}
 
 
 int MX12::read(int ID, int start, int bytes, char* data) {
-
+    
     char PacketLength = 0x4;
     char TxBuf[16];
     char sum = 0;
@@ -276,14 +232,14 @@
 
     Status[4] = 0xFE; // return code
 
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
-    }
+    #endif
 
     // Build the TxPacket first in RAM, then we'll send in one go
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
-    }
+    #endif
 
     TxBuf[0] = 0xff;
     TxBuf[1] = 0xff;
@@ -291,49 +247,61 @@
     // ID
     TxBuf[2] = ID;
     sum += TxBuf[2];
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  ID : %d\n",TxBuf[2]);
-    }
+    #endif
 
     // Packet Length
     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
     sum += TxBuf[3];            // Accululate the packet sum
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  Length : 0x%x\n",TxBuf[3]);
-    }
+    #endif
 
     // Instruction - Read
     TxBuf[4] = 0x2;
     sum += TxBuf[4];
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  Instruction : 0x%x\n",TxBuf[4]);
-    }
+    #endif
 
     // Start Address
     TxBuf[5] = start;
     sum += TxBuf[5];
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  Start Address : 0x%x\n",TxBuf[5]);
-    }
+    #endif
 
     // Bytes to read
     TxBuf[6] = bytes;
     sum += TxBuf[6];
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  No bytes : 0x%x\n",TxBuf[6]);
-    }
+    #endif
 
     // Checksum
     TxBuf[7] = 0xFF - sum;
-    if (MX12_READ_DEBUG) {
+    #if MX12_READ_DEBUG
         printf("  Checksum : 0x%x\n",TxBuf[7]);
+    #endif
+    
+    // Clear in input buffer first
+    while (mx12_in.readable()) {
+        mx12_in.getc();
+        printf("Purging one character (read).\n");
     }
-
+    
     // Transmit the packet in one burst with no pausing
     for (int i = 0; i<8 ; i++) {
-        _mx12.putc(TxBuf[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
     //wait (0.00002);
 
@@ -341,31 +309,43 @@
     if (_ID != 0xFE) {
 
         // Receive the Status packet 6+ number of bytes read
-        if (MX12_READ_DEBUG) {
+        #if MX12_READ_DEBUG
             printf("  Reading Byte:");
-        }
+        #endif
+        #if MX12_DEBUG
         Timer serial_timeout;
+        serial_timeout.start ();
+        #endif
+        
         for (int i=0; i<(6+bytes) ; i++) {
+            #if MX12_DEBUG
             serial_timeout.reset ();
-            while (!_mx12.readable ())
+            while (!mx12_in.readable ())
             {
                 //Just loop here until you either get a character, or we timeout
-                if (serial_timeout.read_ms () > MAX_DELAY_BETWEEN_CHARCTERS_IN_MS)
+                if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
                 {
                     //If we timeout, quit in a panic!
-                    if (MX12_READ_DEBUG) {
-                        printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
-                    }
+                    printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
                     return(0x00);
                 }
             }
-            Status[i] = _mx12.getc();
-            if (MX12_READ_DEBUG) {
+            #endif
+            Status[i] = mx12_in.getc();
+            #if MX12_READ_DEBUG
                 printf("%d",i);
-            }
+            #endif
         }
-        if (MX12_READ_DEBUG) {
+        
+        #if MX12_READ_DEBUG
             printf("\n");
+        #endif
+        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 0;
         }
 
         // Copy the data from Status into data for return
@@ -373,7 +353,7 @@
             data[i] = Status[5+i];
         }
 
-        if (MX12_READ_DEBUG) {
+        #if MX12_READ_DEBUG
             printf("\nStatus Packet\n");
             printf("  Header : 0x%x\n",Status[0]);
             printf("  Header : 0x%x\n",Status[1]);
@@ -386,7 +366,7 @@
             }
 
             printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
-        }
+        #endif
 
     } // if (ID!=0xFE)
 
@@ -394,84 +374,90 @@
 }
 
 
-int MX12:: write(int ID, int start, int bytes, char* data, int flag) {
+
+
+int MX12:: write(int ID, int start, int bytes, char* data) {
 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
 
     char TxBuf[16];
     char sum = 0;
     char Status[6];
 
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
-    }
+    #endif
 
     // Build the TxPacket first in RAM, then we'll send in one go
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
-    }
+    #endif
 
-    TxBuf[0] = 0xff;
-    TxBuf[1] = 0xff;
+    TxBuf[0] = MX12_INSTRUCTION_HEADER;
+    TxBuf[1] = MX12_INSTRUCTION_HEADER;
 
     // ID
     TxBuf[2] = ID;
     sum += TxBuf[2];
 
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("  ID : %d\n",TxBuf[2]);
-    }
+    #endif
 
     // packet Length
     TxBuf[3] = 3+bytes;
     sum += TxBuf[3];
 
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("  Length : %d\n",TxBuf[3]);
-    }
+    #endif
 
     // Instruction
-    if (flag == 1) {
-        TxBuf[4]=0x04;
-        sum += TxBuf[4];
-    } else {
+//    if (flag == 1) {
+//        TxBuf[4]=0x04;
+//    } else {
         TxBuf[4]=0x03;
-        sum += TxBuf[4];
-    }
+//    }
+    sum += TxBuf[4];
 
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("  Instruction : 0x%x\n",TxBuf[4]);
-    }
+    #endif
 
     // Start Address
     TxBuf[5] = start;
     sum += TxBuf[5];
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("  Start : 0x%x\n",TxBuf[5]);
-    }
+    #endif
 
     // data
     for (char i=0; i<bytes ; i++) {
         TxBuf[6+i] = data[i];
         sum += TxBuf[6+i];
-        if (MX12_WRITE_DEBUG) {
+        #if MX12_WRITE_DEBUG
             printf("  Data : 0x%x\n",TxBuf[6+i]);
-        }
+        #endif
     }
 
     // checksum
     TxBuf[6+bytes] = 0xFF - sum;
-    if (MX12_WRITE_DEBUG) {
+    #if MX12_WRITE_DEBUG
         printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
+    #endif
+    
+    // Clear in input buffer first
+    while (mx12_in.readable()) {
+        mx12_in.getc();
+        printf("Purging one character (write).\n");
     }
 
     // Transmit the packet in one burst with no pausing
     for (int i = 0; i < (7 + bytes) ; i++) {
-        _mx12.putc(TxBuf[i]);
+        mx12_out.putc(TxBuf[i]);
+        mx12_in.getc();
+        //printf("Echo: 0x%02x\n",mx12_in.getc());
     }
 
-    // Wait for data to transmit
-    wait (0.00002);
-
     // make sure we have a valid return
     Status[4]=0x00;
 
@@ -479,36 +465,316 @@
     if (_ID!=0xFE) {
 
         // response is always 6 bytes
-        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // 0xFF, 0xFF, ID, Length, Error, Checksum
+        #if MX12_DEBUG
         Timer serial_timeout;
+        serial_timeout.start ();
+        #endif
+        
         for (int i=0; i < 6 ; i++) {
+            #if MX12_DEBUG
             serial_timeout.reset ();
-            while (!_mx12.readable ())
+            while (!mx12_in.readable ())
             {
                 //Just loop here until you either get a character, or we timeout
-                if (serial_timeout.read_ms () > MAX_DELAY_BETWEEN_CHARCTERS_IN_MS)
+                if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
                 {
                     //If we timeout, quit in a panic!
-                    if (MX12_WRITE_DEBUG) {
-                        printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
-                    }
+                    printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
                     return(0x00);
                 }
             }
-            Status[i] = _mx12.getc();
+            #endif
+            Status[i] = mx12_in.getc();
         }
-
+        
         // Build the TxPacket first in RAM, then we'll send in one go
-        if (MX12_WRITE_DEBUG) {
+        #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
 }
+
+void MX12::coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1)
+{
+    char NumDevices   = 0x2;
+    char DataLength   = 0x4;//!< Hardcoded for now, 2 bytes for pos, 2 bytes for vel
+    char PacketLength = 0x4+NumDevices*(DataLength+0x1);
+    char StartAddress = MX12_OD[MX12_REG_GOAL_POSITION].Address;
+    char TxBuf[20];
+    char sum = 0;
+    char ii=0;
+    char jj=0;
+    char offset=0;
+    
+    // Hardcoded for now, 2 devices
+    char ID_Num[NumDevices];
+    ID_Num[0]=id0;
+    ID_Num[1]=id1;
+    
+    // Hardcoded for now, 2 bytes for pos, 2 bytes for vel, 2 devices
+    char data[NumDevices][DataLength];
+    // A faster/better way to do this would be with a fancy memcpy from 
+    // short to char or maybe a union or just raw pointers...
+    data[0][0] = pos0 & 0xff; // bottom 8 bits
+    data[0][1] = pos0 >> 8;   // top 8 bits
+    data[0][2] = vel0 & 0xff; // bottom 8 bits
+    data[0][3] = vel0 >> 8;   // top 8 bits
+    data[1][0] = pos1 & 0xff; // bottom 8 bits
+    data[1][1] = pos1 >> 8;   // top 8 bits
+    data[1][2] = vel1 & 0xff; // bottom 8 bits
+    data[1][3] = vel1 >> 8;   // top 8 bits
+    
+    // Build the TxPacket first in RAM, then we'll send in one go
+    #if MX12_WRITE_DEBUG
+        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+    #endif
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = 0xfe;
+    sum += TxBuf[2];
+    #if MX12_WRITE_DEBUG
+        printf("  ID : %d\n",TxBuf[2]);
+    #endif
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+    #if MX12_WRITE_DEBUG
+        printf("  Length : 0x%x\n",TxBuf[3]);
+    #endif
+
+    // Instruction - Sync Write
+    TxBuf[4] = 0x83;
+    sum += TxBuf[4];
+    #if MX12_WRITE_DEBUG
+        printf("  Instruction : 0x%x\n",TxBuf[4]);
+    #endif
+
+    // Start Address
+    TxBuf[5] = StartAddress;
+    sum += TxBuf[5];
+    #if MX12_WRITE_DEBUG
+        printf("  Start Address : 0x%x\n",TxBuf[5]);
+    #endif
+
+    // Bytes to write to each device
+    TxBuf[6] = DataLength;
+    sum += TxBuf[6];
+    #if MX12_WRITE_DEBUG
+        printf("  No bytes : 0x%x\n",TxBuf[6]);
+    #endif
+
+    // The data itself
+    for(ii=0;ii<NumDevices;ii++){
+        // Store this offset in a variable since we use it a lot.
+        // The 7 comes from the fact that the last write was at 6,
+        // so this one should start at 7.  The 0x1 is for the ID.
+        offset=7+ii*(DataLength+0x1);
+        // Write the ID of the device
+        TxBuf[offset] = ID_Num[ii];
+        sum += TxBuf[offset];
+        
+        #if MX12_WRITE_DEBUG
+            printf("  ID #%d : 0x%x\n",ii,TxBuf[offset]);
+        #endif
+    
+        // Write each of the bytes of the data
+        for(jj=0;jj<DataLength;ii++){
+            TxBuf[offset+jj] = data[ii][jj];
+            sum += TxBuf[offset+jj];
+            #if MX12_WRITE_DEBUG
+                printf("  Data #%d : 0x%x\n",jj,TxBuf[offset+jj]);
+            #endif
+        }
+    }
+    
+
+    // Checksum
+    offset=7+NumDevices*(DataLength+0x1);
+    TxBuf[offset] = 0xFF - sum;
+    #if MX12_WRITE_DEBUG
+        printf("  Checksum : 0x%x\n",TxBuf[offset]);
+    #endif
+    
+    // Clear in input buffer first
+    while (mx12_in.readable()) {
+        mx12_in.getc();
+        printf("Purging one character (read).\n");
+    }
+    
+    // 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]);
+    }
+    
+    // Read
+    for (ii = 0; ii<offset ; ii++) {
+        profileOut=ii%2;
+        mx12_in.getc();
+    }
+}
+
+void MX12::trigger(void) {
+
+    char TxBuf[6];
+    char sum = 0;
+
+    #if MX12_TRIGGER_DEBUG
+        printf("\nTriggered\n");
+    #endif
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    #if MX12_TRIGGER_DEBUG
+        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
+    #endif
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID - Broadcast
+    TxBuf[2] = 0xFE;
+    sum += TxBuf[2];
+
+    #if MX12_TRIGGER_DEBUG
+        printf("  ID : %d\n",TxBuf[2]);
+    #endif
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+    #if MX12_TRIGGER_DEBUG
+        printf("  Length %d\n",TxBuf[3]);
+    #endif
+
+    // Instruction - ACTION
+    TxBuf[4] = 0x05;
+    sum += TxBuf[4];
+    #if MX12_TRIGGER_DEBUG
+        printf("  Instruction 0x%X\n",TxBuf[5]);
+    #endif
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+    #if MX12_TRIGGER_DEBUG
+        printf("  Checksum 0x%X\n",TxBuf[5]);
+    #endif
+
+    // Transmit the packet in one burst with no pausing
+    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();
+    }
+
+    // This is a broadcast packet, so there will be no reply
+
+    return;
+}
+
+bool MX12::ping(char ID_Num) {
+    if(ID_Num==0xFF){
+        // Default to _ID
+        ID_Num=_ID;
+    }
+    
+    char TxBuf[6];
+    char sum = 0;
+    char Status[6];
+
+    #if MX12_TRIGGER_DEBUG
+        printf("\nTriggered\n");
+    #endif
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+    #if MX12_TRIGGER_DEBUG
+        printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
+    #endif
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID
+    TxBuf[2] = ID_Num;
+    sum += TxBuf[2];
+
+    #if MX12_TRIGGER_DEBUG
+        printf("  ID : %d\n",TxBuf[2]);
+    #endif
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+    #if MX12_TRIGGER_DEBUG
+        printf("  Length %d\n",TxBuf[3]);
+    #endif
+
+    // Instruction - PING
+    TxBuf[4] = 0x01;
+    sum += TxBuf[4];
+    #if MX12_TRIGGER_DEBUG
+        printf("  Instruction 0x%X\n",TxBuf[5]);
+    #endif
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+    #if MX12_TRIGGER_DEBUG
+        printf("  Checksum 0x%X\n",TxBuf[5]);
+    #endif
+
+    // Transmit the packet in one burst with no pausing
+    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();
+    }
+
+    
+    // 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("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
+                return false;
+            }
+        }
+        Status[i] = mx12_in.getc();
+    }
+    
+    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]);
+
+    return true;
+}
\ No newline at end of file