Robotis Dynamixel MX-12W Servo Library
Dependents: SpindleBot_1_5b Utilisatio_MX12_V4
This is my attempt to adapt Chris Styles's AX12 library to work with my Dynamixel MX12 servos. This library is still very much a work in progress, and it may have some/many errors in it, but hopefully I will keep improving it to bring it up to snuff.
Dynamixel aficionados should also check out This MX28 library for a completely separate library that provides very similar functionality, and I wish I had known it existed before I started my work...
minimal example
#include "mbed.h" #include "MX12.h" int main() { MX12 mymx12 (p9, p10, 1); // ID=1 while (1) { mymx12.Set_Goal_Position(0); // go to 0 degrees wait (2.0); mymx12.Set_Goal_Position(300); // go to 300 degrees wait (2.0); } }
MX12.cpp
- Committer:
- labmrd
- Date:
- 2015-01-29
- Revision:
- 3:624d04c390b8
- Parent:
- 1:946a27210553
- Child:
- 4:6e320b7646ff
File content as of revision 3:624d04c390b8:
/* 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, int ID, int baud_rate) : mx12_out(tx, NC), mx12_in(NC, rx), profileOut(LED4) { mx12_out.baud(baud_rate); mx12_in.baud(baud_rate); _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) { //// ////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<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]; 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>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]; data[1]=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(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 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_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); // Skip if the read was to the broadcast address if (_ID != 0xFE) { // Receive the Status packet 6+ number of bytes read #if MX12_READ_DEBUG printf(" Reading Byte:"); #endif Timer serial_timeout; serial_timeout.start (); for (int i=0; i<(6+bytes) ; i++) { serial_timeout.reset (); while (!mx12_in.readable ()) { //Just loop here until you either get a character, or we timeout if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US) { //If we timeout, quit in a panic! printf("Error! Serial Timeout %d\n",i); return(0x00); } } Status[i] = mx12_in.getc(); } if(Status[0]!=0xFF || Status[1]!=0xFF) { printf("Header Error!\n"); //printf("Unexpected header in serial response!\n"); //printf(" Header : 0x%x\n",Status[0]); //printf(" Header : 0x%x\n",Status[1]); return 255; } if(Status[2]!=_ID){ printf("ID Error!\n"); return 255; } if(Status[3]!=bytes+2){ printf("Length Error!\n"); return 255; } char sum=Status[2]+Status[3]+Status[4]+Status[5]; if( bytes==2 ) sum+=Status[6]; if( Status[5+bytes]!=0xff-sum){ printf("Checksum Error!\n"); return 255; } // Copy the data from Status into data for return for (int i=0; i < bytes ; i++) { data[i] = Status[5+i]; } #if MX12_READ_DEBUG printf("\nStatus Packet\n"); printf(" Header : 0x%x\n",Status[0]); printf(" Header : 0x%x\n",Status[1]); printf(" ID : 0x%x\n",Status[2]); printf(" Length : 0x%x\n",Status[3]); printf(" Error Code : 0x%x\n",Status[4]); for (int i=0; i < bytes ; i++) { printf(" Data : 0x%x\n",Status[5+i]); } printf(" Checksum : 0x%x\n",Status[5+bytes]); #endif } // if (ID!=0xFE) 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"); } // Transmit the packet in one burst with no pausing for (int i = 0; i < (7 + bytes) ; i++) { mx12_out.putc(TxBuf[i]); mx12_in.getc(); //printf("Echo: 0x%02x\n",mx12_in.getc()); } // make sure we have a valid return Status[4]=0x00; // we'll only get a reply if it was not broadcast if (_ID!=0xFE) { // response is always 6 bytes // 0xFF, 0xFF, ID, Length, Error, Checksum Timer serial_timeout; serial_timeout.start (); for (int i=0; i < 6 ; i++) { serial_timeout.reset (); while (!mx12_in.readable ()) { //Just loop here until you either get a character, or we timeout if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US) { //If we timeout, quit in a panic! printf("Error! Serial Timeout %d\n",i); return(0x00); } } Status[i] = mx12_in.getc(); } if(Status[0]!=0xFF || Status[1]!=0xFF) { printf("Unexpected header in serial response!\n"); printf(" Header : 0x%x\n",Status[0]); printf(" Header : 0x%x\n",Status[1]); return 255; } if( Status[5]!=0xff-(Status[2]+Status[3]+Status[4])){ printf("Checksum Error!\n"); return 255; } // Build the TxPacket first in RAM, then we'll send in one go #if MX12_WRITE_DEBUG printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); printf(" ID : %d\n",Status[2]); printf(" Length : %d\n",Status[3]); printf(" Error : 0x%x\n",Status[4]); printf(" Checksum : 0x%x\n",Status[5]); #endif } return(Status[4]); // return error code } 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 for (ii = 0; ii<offset ; ii++) { mx12_out.putc(TxBuf[ii]); mx12_in.getc(); //printf("0x%x",TxBuf[ii]); } // Read for (ii = 0; ii<offset ; ii++) { //profileOut=ii%2; } } 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; }