MX12.cpp

00001 /* mbed MX-12 Servo Library
00002  *
00003  */
00004 
00005 #include "MX12.h"
00006 #include "mbed.h"
00007 
00008 MX12OD_Object MX12_OD[MX12_OD_SIZE];
00009 bool MX12OD_Object_initalized;
00010 
00011 MX12::MX12(PinName tx, PinName rx, PinName tx_enable_pin, PinName rx_enable_pin, int ID, int baud_rate)
00012         : mx12_out(tx, NC),
00013           mx12_in(NC, rx),
00014           tx_enable(tx_enable_pin),
00015           rx_enable(rx_enable_pin),
00016           profileOut(LED4) {
00017     mx12_out.baud(baud_rate);
00018     mx12_in.baud(baud_rate);
00019     _ID = ID;
00020     _baud = baud_rate;
00021 }
00022 
00023 
00024 void MX12::Init(void){
00025     if(MX12OD_Object_initalized){
00026         return;
00027     }
00028     MX12_OD[MX12_REG_MODEL_NUMBER                   ].Address=0;    MX12_OD[MX12_REG_MODEL_NUMBER                   ].Bytes=2;
00029     MX12_OD[MX12_REG_VERSION_OF_FIRMWARE            ].Address=2;    MX12_OD[MX12_REG_VERSION_OF_FIRMWARE            ].Bytes=1;
00030     MX12_OD[MX12_REG_ID                             ].Address=3;    MX12_OD[MX12_REG_ID                             ].Bytes=1;
00031     MX12_OD[MX12_REG_BAUD_RATE                      ].Address=4;    MX12_OD[MX12_REG_BAUD_RATE                      ].Bytes=1;
00032     MX12_OD[MX12_REG_RETURN_DELAY_TIME              ].Address=5;    MX12_OD[MX12_REG_RETURN_DELAY_TIME              ].Bytes=1;
00033     MX12_OD[MX12_REG_CW_ANGLE_LIMIT                 ].Address=6;    MX12_OD[MX12_REG_CW_ANGLE_LIMIT                 ].Bytes=2;
00034     MX12_OD[MX12_REG_CCW_ANGLE_LIMIT                ].Address=8;    MX12_OD[MX12_REG_CCW_ANGLE_LIMIT                ].Bytes=2;
00035     MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE  ].Address=11;   MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE  ].Bytes=1;
00036     MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE       ].Address=12;   MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE       ].Bytes=1;
00037     MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE      ].Address=13;   MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE      ].Bytes=1;
00038     MX12_OD[MX12_REG_MAX_TORQUE                     ].Address=14;   MX12_OD[MX12_REG_MAX_TORQUE                     ].Bytes=2;
00039     MX12_OD[MX12_REG_STATUS_RETURN_LEVEL            ].Address=16;   MX12_OD[MX12_REG_STATUS_RETURN_LEVEL            ].Bytes=1;
00040     MX12_OD[MX12_REG_ALARM_LED                      ].Address=17;   MX12_OD[MX12_REG_ALARM_LED                      ].Bytes=1;
00041     MX12_OD[MX12_REG_ALARM_SHUTDOWN                 ].Address=18;   MX12_OD[MX12_REG_ALARM_SHUTDOWN                 ].Bytes=1;
00042     MX12_OD[MX12_REG_MULTI_TURN_OFFSET              ].Address=20;   MX12_OD[MX12_REG_MULTI_TURN_OFFSET              ].Bytes=2;
00043     MX12_OD[MX12_REG_RESOLUTION_DIVIDER             ].Address=22;   MX12_OD[MX12_REG_RESOLUTION_DIVIDER             ].Bytes=1;
00044     MX12_OD[MX12_REG_TORQUE_ENABLE                  ].Address=24;   MX12_OD[MX12_REG_TORQUE_ENABLE                  ].Bytes=1;
00045     MX12_OD[MX12_REG_LED                            ].Address=25;   MX12_OD[MX12_REG_LED                            ].Bytes=1;
00046     MX12_OD[MX12_REG_D_GAIN                         ].Address=26;   MX12_OD[MX12_REG_D_GAIN                         ].Bytes=1;
00047     MX12_OD[MX12_REG_I_GAIN                         ].Address=27;   MX12_OD[MX12_REG_I_GAIN                         ].Bytes=1;
00048     MX12_OD[MX12_REG_P_GAIN                         ].Address=28;   MX12_OD[MX12_REG_P_GAIN                         ].Bytes=1;
00049     MX12_OD[MX12_REG_GOAL_POSITION                  ].Address=30;   MX12_OD[MX12_REG_GOAL_POSITION                  ].Bytes=2;
00050     MX12_OD[MX12_REG_MOVING_SPEED                   ].Address=32;   MX12_OD[MX12_REG_MOVING_SPEED                   ].Bytes=2;
00051     MX12_OD[MX12_REG_TORQUE_LIMIT                   ].Address=34;   MX12_OD[MX12_REG_TORQUE_LIMIT                   ].Bytes=2;
00052     MX12_OD[MX12_REG_PRESENT_POSITION               ].Address=36;   MX12_OD[MX12_REG_PRESENT_POSITION               ].Bytes=2;
00053     MX12_OD[MX12_REG_PRESENT_SPEED                  ].Address=38;   MX12_OD[MX12_REG_PRESENT_SPEED                  ].Bytes=2;
00054     MX12_OD[MX12_REG_PRESENT_LOAD                   ].Address=40;   MX12_OD[MX12_REG_PRESENT_LOAD                   ].Bytes=2;
00055     MX12_OD[MX12_REG_PRESENT_VOLTAGE                ].Address=42;   MX12_OD[MX12_REG_PRESENT_VOLTAGE                ].Bytes=1;
00056     MX12_OD[MX12_REG_PRESENT_TEMPERATURE            ].Address=43;   MX12_OD[MX12_REG_PRESENT_TEMPERATURE            ].Bytes=1;
00057     MX12_OD[MX12_REG_REGISTERED                     ].Address=44;   MX12_OD[MX12_REG_REGISTERED                     ].Bytes=1;
00058     MX12_OD[MX12_REG_MOVING                         ].Address=46;   MX12_OD[MX12_REG_MOVING                         ].Bytes=1;
00059     MX12_OD[MX12_REG_LOCK                           ].Address=47;   MX12_OD[MX12_REG_LOCK                           ].Bytes=1;
00060     MX12_OD[MX12_REG_PUNCH                          ].Address=48;   MX12_OD[MX12_REG_PUNCH                          ].Bytes=2;
00061     MX12_OD[MX12_REG_GOAL_ACCELERATION              ].Address=73;   MX12_OD[MX12_REG_GOAL_ACCELERATION              ].Bytes=1;
00062     MX12OD_Object_initalized=true;
00063 }
00064 
00065 // We shouldn't need to wait between setting pins, 
00066 // since the switching time of the chip is ~6 
00067 // nanoseconds, and a cpu cycle on the LPC1768 
00068 // is ~10 nanoseconds
00069 void MX12::ChangeDir(MX12_Direction dir) {
00070     if(dir==MX12_DIR_IN){
00071         // Turn off transmit first
00072         tx_enable=1;
00073         // Then enable receive
00074         rx_enable=0;
00075     }else if(dir==MX12_DIR_OUT){
00076         // Turn off receive first
00077         rx_enable=1;
00078         // Then enable transmit
00079         tx_enable=0;
00080     }else if(dir==MX12_DIR_NONE){
00081         // Turn off both
00082         tx_enable=1;
00083         rx_enable=1;
00084     }else{
00085         // This shouldn't be possible!
00086         printf("Error: Incorrect Direction choice!\n");
00087     }
00088 }
00089 
00090 // Set the mode of the servo
00091 //  0 = Positional (0-300 degrees)
00092 //  1 = Rotational -1 to 1 speed
00093 int MX12::SetMode(int mode) {
00094 ////
00095 ////Need to implement this as per http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_06
00096 ////
00097 ////Maybe also make a read mode?
00098 ////
00099 //    if (mode == 1) { // set CR
00100 //        SetCWLimit(0);
00101 //        SetCCWLimit(0);
00102 //        SetCRSpeed(0.0);
00103 //    } else {
00104 //        SetCWLimit(0);
00105 //        SetCCWLimit(300);
00106 //        SetCRSpeed(0.0);
00107 //    }
00108     return(0);
00109 }
00110 
00111 
00112 void MX12::Dump_OD_to_Serial(Serial &serialObject){
00113     serialObject.printf("CW Angle Limit = %f Degrees\n",Get_CW_Angle_Limit());
00114     serialObject.printf("CCW Angle Limit = %f Degrees\n",Get_CCW_Angle_Limit());
00115     serialObject.printf("Max Torque = %f Percent\n",Get_Max_Torque());
00116     serialObject.printf("Multi Turn Offset = %f Degrees\n",Get_Multi_Turn_Offset());
00117     serialObject.printf("Goal Position = %f Degrees\n",Get_Goal_Position());
00118     serialObject.printf("Moving Speed = %f Degrees/Second\n",Get_Moving_Speed());
00119     serialObject.printf("Torque Limit = %f Percent\n",Get_Torque_Limit());
00120     serialObject.printf("Punch = %f Percent\n",Get_Punch());
00121     serialObject.printf("ID = %f int\n",Get_ID());
00122     serialObject.printf("Baud Rate = %f Lookup\n",Get_Baud_Rate());
00123     serialObject.printf("Return Delay Time = %f milliseconds\n",Get_Return_Delay_Time());
00124     serialObject.printf("the Highest Limit Temperature = %f Celsius\n",Get_the_Highest_Limit_Temperature());
00125     serialObject.printf("the Lowest Limit Voltage = %f Volts\n",Get_the_Lowest_Limit_Voltage());
00126     serialObject.printf("the Highest Limit Voltage = %f Volts\n",Get_the_Highest_Limit_Voltage());
00127     serialObject.printf("Status Return Level = %f int\n",Get_Status_Return_Level());
00128     serialObject.printf("Alarm LED = %f Bitmap\n",Get_Alarm_LED());
00129     serialObject.printf("Alarm Shutdown = %f Bitmap\n",Get_Alarm_Shutdown());
00130     serialObject.printf("Resolution Divider = %f Ratio\n",Get_Resolution_Divider());
00131     serialObject.printf("Torque Enable = %f bool\n",Get_Torque_Enable());
00132     serialObject.printf("LED = %f Bitmap\n",Get_LED());
00133     serialObject.printf("D Gain = %f Kd\n",Get_D_Gain());
00134     serialObject.printf("I Gain = %f Ki\n",Get_I_Gain());
00135     serialObject.printf("P Gain = %f Kp\n",Get_P_Gain());
00136     serialObject.printf("Lock = %f bool\n",Get_Lock());
00137     serialObject.printf("Goal Acceleration = %f Degrees/Second^2\n",Get_Goal_Acceleration());
00138     serialObject.printf("Model Number = %f Bitmap\n",Get_Model_Number());
00139     serialObject.printf("Present Position = %f Degrees\n",Get_Present_Position());
00140     serialObject.printf("Present Speed = %f Degrees/Second\n",Get_Present_Speed());
00141     serialObject.printf("Present Load = %f Percent\n",Get_Present_Load());
00142     serialObject.printf("Version of Firmware = %f int\n",Get_Version_of_Firmware());
00143     serialObject.printf("Present Voltage = %f Volts\n",Get_Present_Voltage());
00144     serialObject.printf("Present Temperature = %f Celsius\n",Get_Present_Temperature());
00145     serialObject.printf("Registered = %f bool\n",Get_Registered());
00146     serialObject.printf("Moving = %f bool\n",Get_Moving());
00147 }
00148 
00149 void MX12::Scan_For_Dynamixels(bool scan_all_baud_rates,int max_id)
00150 {
00151     char ID_Num;
00152     if(scan_all_baud_rates){
00153         int baud_rate[12]={3000000,2500000,2250000,1000000,500000,400000,250000,200000,115200,57600,19200,9600};
00154         char ii;
00155         for(ii=0;ii<4;ii++){
00156             ChangeUARTBaud(baud_rate[ii]);
00157             for(ID_Num=0;ID_Num<=max_id;ID_Num++){
00158                 //_ID=ID_Num;
00159 //                char data[2];
00160 //                data[1]=0;
00161 //            
00162 //                int ErrorCode = read(_ID, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Address, MX12_OD[MX12_REG_VERSION_OF_FIRMWARE].Bytes, data);
00163 //                if(ErrorCode!=0){
00164                 if(ping(ID_Num)){
00165                     printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]);
00166                 }
00167             }
00168         }
00169     }else{
00170         for(ID_Num=0;ID_Num<=max_id;ID_Num++){
00171             if(ping(ID_Num)){
00172                 printf("Found a servo at ID=%#02x\n",ID_Num);
00173             }
00174         }
00175     }
00176     
00177 }
00178 
00179 int MX12::SetBaud( int target_baud ) {
00180     
00181     short baud_int;
00182     
00183     if(target_baud<=1000000){
00184         baud_int=2000000/target_baud-1;
00185     }else if(target_baud==2250000){
00186         baud_int=250;
00187     }else if(target_baud==2500000){
00188         baud_int=251;
00189     }else if(target_baud==3000000){
00190         baud_int=252;
00191     }else{
00192         printf("Error!  Invalid baud rate of %d!\n",target_baud);
00193         return MX12_ERROR_RETURN;
00194     }
00195     
00196     #if MX12_DEBUG
00197         printf("SetBaudRate to 0x%x\n",baud_int);
00198     #endif
00199     int ret=(write_short(MX12_REG_BAUD_RATE,baud_int));
00200     
00201     // Now change the UART serial rate to the same?
00202     // But to allow changing several servos, allow user
00203     // to do this at their liesure.
00204     // ChangeUARTBaud( target_baud );
00205     
00206     return ret;
00207 }
00208 
00209 void MX12::ChangeUARTBaud( int target_baud ) {
00210     mx12_out.baud(target_baud);
00211     mx12_in.baud(target_baud);
00212 }
00213 
00214 short MX12::GetRawPosition(void) {
00215     #if MX12_DEBUG
00216         printf("\nGetRawPosition(%d)",_ID);
00217     #endif
00218     return read_short(MX12_REG_PRESENT_POSITION);
00219 }
00220 
00221 int MX12:: write_short(MX12ODIndex OD,short value)
00222 {
00223     char data[2];
00224 
00225     data[0] = value & 0xff; // bottom 8 bits
00226     data[1] = value >> 8;   // top 8 bits
00227 
00228     // write the packet, return the error code
00229     int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
00230     if(rVal==MX12_ERROR_RETURN){return rVal;}
00231     if(rVal>0){
00232         if(CHECK_BIT(rVal, 0)){printf("Error! Input Voltage Error\n");}
00233         if(CHECK_BIT(rVal, 1)){printf("Error! Angle Limit Error\n");}
00234         if(CHECK_BIT(rVal, 2)){printf("Error! Overheating Error\n");}
00235         if(CHECK_BIT(rVal, 3)){printf("Error! Range Error\n");}
00236         if(CHECK_BIT(rVal, 4)){printf("Error! Checksum Error\n");}
00237         if(CHECK_BIT(rVal, 5)){printf("Error! Overload Error\n");}
00238         if(CHECK_BIT(rVal, 6)){printf("Error! Instruction Error\n");}
00239         if(CHECK_BIT(rVal, 7)){printf("Error! Undefined Error\n");}
00240     }
00241     return(rVal);
00242 }
00243 
00244 short MX12:: read_short(MX12ODIndex OD)
00245 {
00246     
00247     char data[2]={0,0};
00248 
00249     int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
00250     profileOut = (ErrorCode!=0);
00251     short value = data[0] + (data[1] << 8);
00252     
00253     return (value);
00254 }
00255 
00256 
00257 
00258          ////////////////////////////////////////////
00259          //                                        //
00260          //                                        //
00261          //          PRIVATE FUNCTIONS             //
00262          //                                        //
00263          //                                        //
00264          ////////////////////////////////////////////
00265 
00266 
00267 
00268 int MX12::read_raw(char* Status, int bytes) {
00269     // Receive the Status packet 6+ number of bytes read
00270     #if MX12_READ_DEBUG
00271         printf("  Reading Byte:");
00272     #endif
00273     Timer serial_timeout;
00274     serial_timeout.start ();
00275     
00276     for (int i=0; i<(6+bytes) ; i++) {
00277         serial_timeout.reset ();
00278         while (!mx12_in.readable ())
00279         {
00280             //Just loop here until you either get a character, or we timeout
00281             if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
00282             {
00283                 //If we timeout, quit in a panic!
00284                 //printf("Error! Serial Timeout %d\n",i);
00285                 return(MX12_ERROR_RETURN);
00286             }
00287         }
00288         Status[i] = mx12_in.getc();
00289     }
00290     
00291     if(Status[0]!=0xFF || Status[1]!=0xFF)
00292     {
00293         printf("Header Error!\n");
00294         //printf("Unexpected header in serial response!\n");
00295         //printf("  Header : 0x%x\n",Status[0]);
00296         //printf("  Header : 0x%x\n",Status[1]);
00297         return MX12_ERROR_RETURN;
00298     }
00299     if(Status[2]!=_ID){
00300         printf("ID Error!\n");
00301         return MX12_ERROR_RETURN;
00302     }
00303     if(Status[3]!=bytes+2){
00304         printf("Length Error!\n");
00305         return MX12_ERROR_RETURN;
00306     }
00307     
00308     char sum=Status[2]+Status[3]+Status[4]+Status[5];
00309     if( bytes==2 )
00310         sum+=Status[6];
00311     if( Status[5+bytes]!=0xff-sum){
00312         printf("Checksum Error!\n");
00313         return MX12_ERROR_RETURN;
00314     }
00315     
00316     #if MX12_READ_DEBUG
00317         printf("\nStatus Packet\n");
00318         printf("  Header : 0x%x\n",Status[0]);
00319         printf("  Header : 0x%x\n",Status[1]);
00320         printf("  ID : 0x%x\n",Status[2]);
00321         printf("  Length : 0x%x\n",Status[3]);
00322         printf("  Error Code : 0x%x\n",Status[4]);
00323 
00324         for (int i=0; i < bytes ; i++) {
00325             printf("  Data : 0x%x\n",Status[5+i]);
00326         }
00327 
00328         printf("  Checksum : 0x%x\n",Status[5+bytes]);
00329     #endif
00330     
00331     return MX12_NORMAL_RETURN;
00332 }
00333 
00334 int MX12::read(int ID, int start, int bytes, char* data) {
00335     
00336     char PacketLength = 0x4;
00337     char TxBuf[16];
00338     char sum = 0;
00339     char Status[16];
00340 
00341     Status[4] = 0xFE; // return code
00342 
00343     #if MX12_READ_DEBUG
00344         printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
00345     #endif
00346 
00347     // Build the TxPacket first in RAM, then we'll send in one go
00348     #if MX12_READ_DEBUG
00349         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00350     #endif
00351 
00352     TxBuf[0] = 0xff;
00353     TxBuf[1] = 0xff;
00354 
00355     // ID
00356     TxBuf[2] = ID;
00357     sum += TxBuf[2];
00358     #if MX12_READ_DEBUG
00359         printf("  ID : %d\n",TxBuf[2]);
00360     #endif
00361 
00362     // Packet Length
00363     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00364     sum += TxBuf[3];            // Accululate the packet sum
00365     #if MX12_READ_DEBUG
00366         printf("  Length : 0x%x\n",TxBuf[3]);
00367     #endif
00368 
00369     // Instruction - Read
00370     TxBuf[4] = 0x2;
00371     sum += TxBuf[4];
00372     #if MX12_READ_DEBUG
00373         printf("  Instruction : 0x%x\n",TxBuf[4]);
00374     #endif
00375 
00376     // Start Address
00377     TxBuf[5] = start;
00378     sum += TxBuf[5];
00379     #if MX12_READ_DEBUG
00380         printf("  Start Address : 0x%x\n",TxBuf[5]);
00381     #endif
00382 
00383     // Bytes to read
00384     TxBuf[6] = bytes;
00385     sum += TxBuf[6];
00386     #if MX12_READ_DEBUG
00387         printf("  No bytes : 0x%x\n",TxBuf[6]);
00388     #endif
00389 
00390     // Checksum
00391     TxBuf[7] = 0xFF - sum;
00392     #if MX12_READ_DEBUG
00393         printf("  Checksum : 0x%x\n",TxBuf[7]);
00394     #endif
00395     
00396     // Clear in input buffer first
00397     int buffer_purge_count=0;
00398     while (mx12_in.readable() && buffer_purge_count < 16 ) {
00399         mx12_in.getc();
00400         buffer_purge_count++;
00401         //printf("Purging one character (0x%x).\n",c);
00402     }
00403     if(buffer_purge_count > 1){ // One character is normal, I don't know why...
00404         printf("Error: Purged %d characters from buffer.\n",buffer_purge_count);
00405         return MX12_ERROR_RETURN;
00406     }
00407     
00408     // Change to output
00409     ChangeDir(MX12_DIR_OUT);
00410     wait_us(1); // Debounce
00411     
00412     // Transmit the packet in one burst with no pausing
00413     for (int i = 0; i<8 ; i++) {
00414         mx12_out.putc(TxBuf[i]);
00415     }
00416     
00417     // Wait for the bytes to be transmitted
00418     // This shouldn't overflow with 32 bit ints, but be aware
00419     //       us/s    bytes  b/byte   baud
00420     wait_us(1000000  *  8  *  8  /  _baud);
00421     
00422     // Change to input
00423     ChangeDir(MX12_DIR_IN);
00424     wait_us(1); // Debounce
00425 
00426     if(ID!=0xFE){
00427         if(read_raw(Status,bytes)!=MX12_NORMAL_RETURN){
00428             return MX12_ERROR_RETURN;
00429         }
00430     }
00431     
00432     // Copy the data from Status into data for return
00433     for (int i=0; i < bytes ; i++) {
00434         data[i] = Status[5+i];
00435     }
00436 
00437     return(Status[4]);
00438 }
00439 
00440 
00441 
00442 
00443 int MX12:: write(int ID, int start, int bytes, char* data) {
00444 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
00445 
00446     char TxBuf[16];
00447     char sum = 0;
00448     char Status[6];
00449 
00450     #if MX12_WRITE_DEBUG
00451         printf("\nwrite(0x%02x,0x%02x,0x%02x)\n",ID,start,bytes);
00452     #endif
00453 
00454     // Build the TxPacket first in RAM, then we'll send in one go
00455     #if MX12_WRITE_DEBUG
00456         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00457     #endif
00458 
00459     TxBuf[0] = MX12_INSTRUCTION_HEADER;
00460     TxBuf[1] = MX12_INSTRUCTION_HEADER;
00461 
00462     // ID
00463     TxBuf[2] = ID;
00464     sum += TxBuf[2];
00465 
00466     #if MX12_WRITE_DEBUG
00467         printf("  ID : %d\n",TxBuf[2]);
00468     #endif
00469 
00470     // packet Length
00471     TxBuf[3] = 3+bytes;
00472     sum += TxBuf[3];
00473 
00474     #if MX12_WRITE_DEBUG
00475         printf("  Length : %d\n",TxBuf[3]);
00476     #endif
00477 
00478     // Instruction
00479 //    if (flag == 1) {
00480 //        TxBuf[4]=0x04;
00481 //    } else {
00482         TxBuf[4]=0x03;
00483 //    }
00484     sum += TxBuf[4];
00485 
00486     #if MX12_WRITE_DEBUG
00487         printf("  Instruction : 0x%x\n",TxBuf[4]);
00488     #endif
00489 
00490     // Start Address
00491     TxBuf[5] = start;
00492     sum += TxBuf[5];
00493     #if MX12_WRITE_DEBUG
00494         printf("  Start : 0x%x\n",TxBuf[5]);
00495     #endif
00496 
00497     // data
00498     for (char i=0; i<bytes ; i++) {
00499         TxBuf[6+i] = data[i];
00500         sum += TxBuf[6+i];
00501         #if MX12_WRITE_DEBUG
00502             printf("  Data : 0x%x\n",TxBuf[6+i]);
00503         #endif
00504     }
00505 
00506     // checksum
00507     TxBuf[6+bytes] = 0xFF - sum;
00508     #if MX12_WRITE_DEBUG
00509         printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
00510     #endif
00511     
00512     // Clear in input buffer first
00513     while (mx12_in.readable()) {
00514         mx12_in.getc();
00515         printf("Purging one character (write).\n");
00516     }
00517     
00518     // Change to output
00519     ChangeDir(MX12_DIR_OUT);
00520     wait_us(1);
00521     
00522     // Transmit the packet in one burst with no pausing
00523     for (int i = 0; i< (7 + bytes) ; i++) {
00524         mx12_out.putc(TxBuf[i]);
00525     }
00526     
00527     // Wait for the bytes to be transmitted
00528     // This shouldn't overflow with 32 bit ints, but be aware
00529     //       us/s          bytes      b/byte   baud    fudge
00530     wait_us(1000000  *  (7 + bytes)  *  8  /  _baud  +  1);
00531     
00532     // Change to input
00533     ChangeDir(MX12_DIR_IN);
00534     wait_us(1);
00535 
00536     // make sure we have an invalid return
00537     Status[4]=MX12_ERROR_RETURN;
00538 
00539     // we'll only get a reply if it was not broadcast
00540     if(ID!=0xFE){
00541         if(read_raw(Status,0)!=MX12_NORMAL_RETURN){
00542             return MX12_ERROR_RETURN;
00543         }
00544     }
00545 
00546     return(Status[4]); // return error code
00547 }
00548 
00549 void MX12::coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1)
00550 {
00551     char NumDevices   = 0x2;
00552     char DataLength   = 0x4;//!< Hardcoded for now, 2 bytes for pos, 2 bytes for vel
00553     char PacketLength = 0x4+NumDevices*(DataLength+0x1);
00554     char StartAddress = MX12_OD[MX12_REG_GOAL_POSITION].Address;
00555     char TxBuf[20];
00556     char sum = 0;
00557     char ii=0;
00558     char jj=0;
00559     char offset=0;
00560     
00561     // Hardcoded for now, 2 devices
00562     char ID_Num[NumDevices];
00563     ID_Num[0]=id0;
00564     ID_Num[1]=id1;
00565     
00566     // Hardcoded for now, 2 bytes for pos, 2 bytes for vel, 2 devices
00567     char data[NumDevices][DataLength];
00568     // A faster/better way to do this would be with a fancy memcpy from 
00569     // short to char or maybe a union or just raw pointers...
00570     data[0][0] = pos0 & 0xff; // bottom 8 bits
00571     data[0][1] = pos0 >> 8;   // top 8 bits
00572     data[0][2] = vel0 & 0xff; // bottom 8 bits
00573     data[0][3] = vel0 >> 8;   // top 8 bits
00574     data[1][0] = pos1 & 0xff; // bottom 8 bits
00575     data[1][1] = pos1 >> 8;   // top 8 bits
00576     data[1][2] = vel1 & 0xff; // bottom 8 bits
00577     data[1][3] = vel1 >> 8;   // top 8 bits
00578     
00579     // Build the TxPacket first in RAM, then we'll send in one go
00580     #if MX12_WRITE_DEBUG
00581         printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
00582     #endif
00583 
00584     TxBuf[0] = 0xff;
00585     TxBuf[1] = 0xff;
00586 
00587     // ID
00588     TxBuf[2] = 0xfe;
00589     sum += TxBuf[2];
00590     #if MX12_WRITE_DEBUG
00591         printf("  ID : %d\n",TxBuf[2]);
00592     #endif
00593 
00594     // Packet Length
00595     TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
00596     sum += TxBuf[3];            // Accululate the packet sum
00597     #if MX12_WRITE_DEBUG
00598         printf("  Length : 0x%x\n",TxBuf[3]);
00599     #endif
00600 
00601     // Instruction - Sync Write
00602     TxBuf[4] = 0x83;
00603     sum += TxBuf[4];
00604     #if MX12_WRITE_DEBUG
00605         printf("  Instruction : 0x%x\n",TxBuf[4]);
00606     #endif
00607 
00608     // Start Address
00609     TxBuf[5] = StartAddress;
00610     sum += TxBuf[5];
00611     #if MX12_WRITE_DEBUG
00612         printf("  Start Address : 0x%x\n",TxBuf[5]);
00613     #endif
00614 
00615     // Bytes to write to each device
00616     TxBuf[6] = DataLength;
00617     sum += TxBuf[6];
00618     #if MX12_WRITE_DEBUG
00619         printf("  No bytes : 0x%x\n",TxBuf[6]);
00620     #endif
00621 
00622     // The data itself
00623     for(ii=0;ii<NumDevices;ii++){
00624         // Store this offset in a variable since we use it a lot.
00625         // The 7 comes from the fact that the last write was at 6,
00626         // so this one should start at 7.  The 0x1 is for the ID.
00627         offset=7+ii*(DataLength+0x1);
00628         // Write the ID of the device
00629         TxBuf[offset] = ID_Num[ii];
00630         sum += TxBuf[offset];
00631         
00632         #if MX12_WRITE_DEBUG
00633             printf("  ID #%d : 0x%x\n",ii,TxBuf[offset]);
00634         #endif
00635     
00636         // Write each of the bytes of the data
00637         for(jj=0;jj<DataLength;jj++){
00638             TxBuf[offset+jj+0x1] = data[ii][jj];
00639             sum += TxBuf[offset+jj+0x1];
00640             #if MX12_WRITE_DEBUG
00641                 printf("  Data #%d : 0x%x\n",jj,TxBuf[offset+jj+0x1]);
00642             #endif
00643         }
00644     }
00645     
00646 
00647     // Checksum
00648     offset=7+NumDevices*(DataLength+0x1);
00649     TxBuf[offset] = 0xFF - sum;
00650     #if MX12_WRITE_DEBUG
00651         printf("  Checksum : 0x%x\n",TxBuf[offset]);
00652     #endif
00653     
00654     // Clear in input buffer first
00655     while (mx12_in.readable()) {
00656         mx12_in.getc();
00657         printf("Purging one character (read).\n");
00658     }
00659     
00660     // Transmit the packet in one burst with no pausing
00661     offset=7+NumDevices*(DataLength+0x1)+0x1; // one more for the checksum
00662     
00663     
00664     // Change to output
00665     ChangeDir(MX12_DIR_OUT);
00666     wait_us(1);
00667     
00668     // Transmit the packet in one burst with no pausing
00669     for (int i = 0; i< offset ; i++) {
00670         mx12_out.putc(TxBuf[i]);
00671     }
00672     
00673     // Wait for the bytes to be transmitted
00674     // This shouldn't overflow with 32 bit ints, but be aware
00675     //       us/s       bytes    b/byte   baud
00676     wait_us(1000000  *  offset  *  8  /  _baud);
00677 }
00678 
00679 void MX12::trigger(void) {
00680 
00681     char TxBuf[6];
00682     char sum = 0;
00683 
00684     #if MX12_TRIGGER_DEBUG
00685         printf("\nTriggered\n");
00686     #endif
00687 
00688     // Build the TxPacket first in RAM, then we'll send in one go
00689     #if MX12_TRIGGER_DEBUG
00690         printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
00691     #endif
00692 
00693     TxBuf[0] = 0xFF;
00694     TxBuf[1] = 0xFF;
00695 
00696     // ID - Broadcast
00697     TxBuf[2] = 0xFE;
00698     sum += TxBuf[2];
00699 
00700     #if MX12_TRIGGER_DEBUG
00701         printf("  ID : %d\n",TxBuf[2]);
00702     #endif
00703 
00704     // Length
00705     TxBuf[3] = 0x02;
00706     sum += TxBuf[3];
00707     #if MX12_TRIGGER_DEBUG
00708         printf("  Length %d\n",TxBuf[3]);
00709     #endif
00710 
00711     // Instruction - ACTION
00712     TxBuf[4] = 0x05;
00713     sum += TxBuf[4];
00714     #if MX12_TRIGGER_DEBUG
00715         printf("  Instruction 0x%X\n",TxBuf[5]);
00716     #endif
00717 
00718     // Checksum
00719     TxBuf[5] = 0xFF - sum;
00720     #if MX12_TRIGGER_DEBUG
00721         printf("  Checksum 0x%X\n",TxBuf[5]);
00722     #endif
00723 
00724     // Transmit the packet in one burst with no pausing
00725     for (int i = 0; i < 6 ; i++) {
00726         mx12_out.putc(TxBuf[i]);
00727     }
00728     
00729     // Read
00730     for (int i = 0; i < 6 ; i++) {
00731         //profileOut=i%2;
00732         mx12_in.getc();
00733     }
00734 
00735     // This is a broadcast packet, so there will be no reply
00736 
00737     return;
00738 }
00739 
00740 bool MX12::ping(char ID_Num) {
00741     if(ID_Num==0xFF){
00742         // Default to _ID
00743         ID_Num=_ID;
00744     }
00745     
00746     char TxBuf[6];
00747     char sum = 0;
00748     char Status[6];
00749 
00750     #if MX12_TRIGGER_DEBUG
00751         printf("\nTriggered\n");
00752     #endif
00753 
00754     // Build the TxPacket first in RAM, then we'll send in one go
00755     #if MX12_TRIGGER_DEBUG
00756         printf("\nTrigger Packet\n  Header : 0xFF, 0xFF\n");
00757     #endif
00758 
00759     TxBuf[0] = 0xFF;
00760     TxBuf[1] = 0xFF;
00761 
00762     // ID
00763     TxBuf[2] = ID_Num;
00764     sum += TxBuf[2];
00765 
00766     #if MX12_TRIGGER_DEBUG
00767         printf("  ID : %d\n",TxBuf[2]);
00768     #endif
00769 
00770     // Length
00771     TxBuf[3] = 0x02;
00772     sum += TxBuf[3];
00773     #if MX12_TRIGGER_DEBUG
00774         printf("  Length %d\n",TxBuf[3]);
00775     #endif
00776 
00777     // Instruction - PING
00778     TxBuf[4] = 0x01;
00779     sum += TxBuf[4];
00780     #if MX12_TRIGGER_DEBUG
00781         printf("  Instruction 0x%X\n",TxBuf[5]);
00782     #endif
00783 
00784     // Checksum
00785     TxBuf[5] = 0xFF - sum;
00786     #if MX12_TRIGGER_DEBUG
00787         printf("  Checksum 0x%X\n",TxBuf[5]);
00788     #endif
00789     
00790     
00791     // Clear in input buffer first
00792     while (mx12_in.readable()) {
00793         mx12_in.getc();
00794         printf("Purging one character (ping).\n");
00795     }
00796 
00797     // Change to output
00798     ChangeDir(MX12_DIR_OUT);
00799     wait_us(1);
00800     
00801     // Transmit the packet in one burst with no pausing
00802     for (int i = 0; i<6 ; i++) {
00803         mx12_out.putc(TxBuf[i]);
00804     }
00805     
00806     // Wait for the bytes to be transmitted
00807     // This shouldn't overflow with 32 bit ints, but be aware
00808     //       us/s    bytes  b/byte   baud    fudge
00809     wait_us(1000000  *  6  *  8  /  _baud  +  1);
00810     
00811     // Change to input
00812     ChangeDir(MX12_DIR_IN);
00813     wait_us(1);
00814     
00815     // response is always 6 bytes
00816     // 0xFF, 0xFF, ID, Length, Error, Checksum
00817     Timer serial_timeout;
00818     serial_timeout.start ();
00819     
00820     for (int i=0; i < 6 ; i++) {
00821         serial_timeout.reset ();
00822         while (!mx12_in.readable ())
00823         {
00824             //Just loop here until you either get a character, or we timeout
00825             if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
00826             {
00827                 //If we timeout, quit in a panic!
00828                 //printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
00829                 return false;
00830             }
00831         }
00832         Status[i] = mx12_in.getc();
00833     }
00834     
00835     printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
00836     printf("  ID : %d\n",Status[2]);
00837     printf("  Length : %d\n",Status[3]);
00838     printf("  Error : 0x%x\n",Status[4]);
00839     printf("  Checksum : 0x%x\n",Status[5]);
00840 
00841     return true;
00842 }