File content as of revision 5:4c118a827f11:

/* mbed MX-12 Servo Library
 *
 */

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

MX12OD_Object MX12_OD[MX12_OD_SIZE];
bool MX12OD_Object_initalized;

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


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

// 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
int MX12::SetMode(int mode) {
////
////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);
}


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

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<4;ii++){
            ChangeUARTBaud(baud_rate[ii]);
            for(ID_Num=0;ID_Num<=max_id;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]);
                }
            }
        }
    }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 MX12_ERROR_RETURN;
    }
    
    #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];

    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_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");}
        if(CHECK_BIT(rVal, 2)){printf("Error! Overheating Error\n");}
        if(CHECK_BIT(rVal, 3)){printf("Error! Range Error\n");}
        if(CHECK_BIT(rVal, 4)){printf("Error! Checksum Error\n");}
        if(CHECK_BIT(rVal, 5)){printf("Error! Overload Error\n");}
        if(CHECK_BIT(rVal, 6)){printf("Error! Instruction Error\n");}
        if(CHECK_BIT(rVal, 7)){printf("Error! Undefined Error\n");}
    }
    return(rVal);
}

short MX12:: read_short(MX12ODIndex OD)
{
    
    char data[2]={0,0};

    int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
    profileOut = (ErrorCode!=0);
    short value = data[0] + (data[1] << 8);
    
    return (value);
}



         ////////////////////////////////////////////
         //                                        //
         //                                        //
         //          PRIVATE FUNCTIONS             //
         //                                        //
         //                                        //
         ////////////////////////////////////////////



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) {
    
    char PacketLength = 0x4;
    char TxBuf[16];
    char sum = 0;
    char Status[16];

    Status[4] = 0xFE; // return code

    #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
        printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
    #endif

    TxBuf[0] = 0xff;
    TxBuf[1] = 0xff;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];
    #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
        printf("  Length : 0x%x\n",TxBuf[3]);
    #endif

    // Instruction - Read
    TxBuf[4] = 0x2;
    sum += TxBuf[4];
    #if MX12_READ_DEBUG
        printf("  Instruction : 0x%x\n",TxBuf[4]);
    #endif

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];
    #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
        printf("  No bytes : 0x%x\n",TxBuf[6]);
    #endif

    // Checksum
    TxBuf[7] = 0xFF - sum;
    #if MX12_READ_DEBUG
        printf("  Checksum : 0x%x\n",TxBuf[7]);
    #endif
    
    // Clear in input buffer first
    int buffer_purge_count=0;
    while (mx12_in.readable() && buffer_purge_count < 16 ) {
        mx12_in.getc();
        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]);
    }
    
    // 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;
        }
    }
    
    // Copy the data from Status into data for return
    for (int i=0; i < bytes ; i++) {
        data[i] = Status[5+i];
    }

    return(Status[4]);
}




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
        printf("\nwrite(0x%02x,0x%02x,0x%02x)\n",ID,start,bytes);
    #endif

    // 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] = MX12_INSTRUCTION_HEADER;
    TxBuf[1] = MX12_INSTRUCTION_HEADER;

    // ID
    TxBuf[2] = ID;
    sum += TxBuf[2];

    #if MX12_WRITE_DEBUG
        printf("  ID : %d\n",TxBuf[2]);
    #endif

    // packet Length
    TxBuf[3] = 3+bytes;
    sum += TxBuf[3];

    #if MX12_WRITE_DEBUG
        printf("  Length : %d\n",TxBuf[3]);
    #endif

    // Instruction
//    if (flag == 1) {
//        TxBuf[4]=0x04;
//    } else {
        TxBuf[4]=0x03;
//    }
    sum += TxBuf[4];

    #if MX12_WRITE_DEBUG
        printf("  Instruction : 0x%x\n",TxBuf[4]);
    #endif

    // Start Address
    TxBuf[5] = start;
    sum += TxBuf[5];
    #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
            printf("  Data : 0x%x\n",TxBuf[6+i]);
        #endif
    }

    // checksum
    TxBuf[6+bytes] = 0xFF - sum;
    #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");
    }
    
    // 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++) {
        mx12_out.putc(TxBuf[i]);
    }
    
    // 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 an invalid return
    Status[4]=MX12_ERROR_RETURN;

    // we'll only get a reply if it was not broadcast
    if(ID!=0xFE){
        if(read_raw(Status,0)!=MX12_NORMAL_RETURN){
            return MX12_ERROR_RETURN;
        }
    }

    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;jj++){
            TxBuf[offset+jj+0x1] = data[ii][jj];
            sum += TxBuf[offset+jj+0x1];
            #if MX12_WRITE_DEBUG
                printf("  Data #%d : 0x%x\n",jj,TxBuf[offset+jj+0x1]);
            #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
    
    
    // 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]);
    }
    
    // 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) {

    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
    
    
    // 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++) {
        mx12_out.putc(TxBuf[i]);
    }
    
    // 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
    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;
}