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@3:624d04c390b8, 2015-01-29 (annotated)
- Committer:
- labmrd
- Date:
- Thu Jan 29 22:23:20 2015 +0000
- Revision:
- 3:624d04c390b8
- Parent:
- 1:946a27210553
- Child:
- 4:6e320b7646ff
More error checking
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
labmrd | 1:946a27210553 | 1 | /* mbed MX-12 Servo Library |
labmrd | 0:29900c3a4a50 | 2 | * |
labmrd | 0:29900c3a4a50 | 3 | */ |
labmrd | 0:29900c3a4a50 | 4 | |
labmrd | 0:29900c3a4a50 | 5 | #include "MX12.h" |
labmrd | 0:29900c3a4a50 | 6 | #include "mbed.h" |
labmrd | 0:29900c3a4a50 | 7 | |
labmrd | 1:946a27210553 | 8 | MX12OD_Object MX12_OD[MX12_OD_SIZE]; |
labmrd | 1:946a27210553 | 9 | bool MX12OD_Object_initalized; |
labmrd | 1:946a27210553 | 10 | |
labmrd | 1:946a27210553 | 11 | MX12::MX12(PinName tx, PinName rx, int ID, int baud_rate) |
labmrd | 1:946a27210553 | 12 | : mx12_out(tx, NC), |
labmrd | 1:946a27210553 | 13 | mx12_in(NC, rx), |
labmrd | 3:624d04c390b8 | 14 | profileOut(LED4) { |
labmrd | 1:946a27210553 | 15 | mx12_out.baud(baud_rate); |
labmrd | 1:946a27210553 | 16 | mx12_in.baud(baud_rate); |
labmrd | 1:946a27210553 | 17 | _ID = ID; |
labmrd | 1:946a27210553 | 18 | } |
labmrd | 1:946a27210553 | 19 | |
labmrd | 0:29900c3a4a50 | 20 | |
labmrd | 1:946a27210553 | 21 | void MX12::Init(void){ |
labmrd | 1:946a27210553 | 22 | if(MX12OD_Object_initalized){ |
labmrd | 1:946a27210553 | 23 | return; |
labmrd | 1:946a27210553 | 24 | } |
labmrd | 1:946a27210553 | 25 | MX12_OD[MX12_REG_MODEL_NUMBER ].Address=0; MX12_OD[MX12_REG_MODEL_NUMBER ].Bytes=2; |
labmrd | 1:946a27210553 | 26 | MX12_OD[MX12_REG_VERSION_OF_FIRMWARE ].Address=2; MX12_OD[MX12_REG_VERSION_OF_FIRMWARE ].Bytes=1; |
labmrd | 1:946a27210553 | 27 | MX12_OD[MX12_REG_ID ].Address=3; MX12_OD[MX12_REG_ID ].Bytes=1; |
labmrd | 1:946a27210553 | 28 | MX12_OD[MX12_REG_BAUD_RATE ].Address=4; MX12_OD[MX12_REG_BAUD_RATE ].Bytes=1; |
labmrd | 1:946a27210553 | 29 | MX12_OD[MX12_REG_RETURN_DELAY_TIME ].Address=5; MX12_OD[MX12_REG_RETURN_DELAY_TIME ].Bytes=1; |
labmrd | 1:946a27210553 | 30 | MX12_OD[MX12_REG_CW_ANGLE_LIMIT ].Address=6; MX12_OD[MX12_REG_CW_ANGLE_LIMIT ].Bytes=2; |
labmrd | 1:946a27210553 | 31 | MX12_OD[MX12_REG_CCW_ANGLE_LIMIT ].Address=8; MX12_OD[MX12_REG_CCW_ANGLE_LIMIT ].Bytes=2; |
labmrd | 1:946a27210553 | 32 | MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE ].Address=11; MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_TEMPERATURE ].Bytes=1; |
labmrd | 1:946a27210553 | 33 | MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE ].Address=12; MX12_OD[MX12_REG_THE_LOWEST_LIMIT_VOLTAGE ].Bytes=1; |
labmrd | 1:946a27210553 | 34 | MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE ].Address=13; MX12_OD[MX12_REG_THE_HIGHEST_LIMIT_VOLTAGE ].Bytes=1; |
labmrd | 1:946a27210553 | 35 | MX12_OD[MX12_REG_MAX_TORQUE ].Address=14; MX12_OD[MX12_REG_MAX_TORQUE ].Bytes=2; |
labmrd | 1:946a27210553 | 36 | MX12_OD[MX12_REG_STATUS_RETURN_LEVEL ].Address=16; MX12_OD[MX12_REG_STATUS_RETURN_LEVEL ].Bytes=1; |
labmrd | 1:946a27210553 | 37 | MX12_OD[MX12_REG_ALARM_LED ].Address=17; MX12_OD[MX12_REG_ALARM_LED ].Bytes=1; |
labmrd | 1:946a27210553 | 38 | MX12_OD[MX12_REG_ALARM_SHUTDOWN ].Address=18; MX12_OD[MX12_REG_ALARM_SHUTDOWN ].Bytes=1; |
labmrd | 1:946a27210553 | 39 | MX12_OD[MX12_REG_MULTI_TURN_OFFSET ].Address=20; MX12_OD[MX12_REG_MULTI_TURN_OFFSET ].Bytes=2; |
labmrd | 1:946a27210553 | 40 | MX12_OD[MX12_REG_RESOLUTION_DIVIDER ].Address=22; MX12_OD[MX12_REG_RESOLUTION_DIVIDER ].Bytes=1; |
labmrd | 1:946a27210553 | 41 | MX12_OD[MX12_REG_TORQUE_ENABLE ].Address=24; MX12_OD[MX12_REG_TORQUE_ENABLE ].Bytes=1; |
labmrd | 1:946a27210553 | 42 | MX12_OD[MX12_REG_LED ].Address=25; MX12_OD[MX12_REG_LED ].Bytes=1; |
labmrd | 1:946a27210553 | 43 | MX12_OD[MX12_REG_D_GAIN ].Address=26; MX12_OD[MX12_REG_D_GAIN ].Bytes=1; |
labmrd | 1:946a27210553 | 44 | MX12_OD[MX12_REG_I_GAIN ].Address=27; MX12_OD[MX12_REG_I_GAIN ].Bytes=1; |
labmrd | 1:946a27210553 | 45 | MX12_OD[MX12_REG_P_GAIN ].Address=28; MX12_OD[MX12_REG_P_GAIN ].Bytes=1; |
labmrd | 1:946a27210553 | 46 | MX12_OD[MX12_REG_GOAL_POSITION ].Address=30; MX12_OD[MX12_REG_GOAL_POSITION ].Bytes=2; |
labmrd | 1:946a27210553 | 47 | MX12_OD[MX12_REG_MOVING_SPEED ].Address=32; MX12_OD[MX12_REG_MOVING_SPEED ].Bytes=2; |
labmrd | 1:946a27210553 | 48 | MX12_OD[MX12_REG_TORQUE_LIMIT ].Address=34; MX12_OD[MX12_REG_TORQUE_LIMIT ].Bytes=2; |
labmrd | 1:946a27210553 | 49 | MX12_OD[MX12_REG_PRESENT_POSITION ].Address=36; MX12_OD[MX12_REG_PRESENT_POSITION ].Bytes=2; |
labmrd | 1:946a27210553 | 50 | MX12_OD[MX12_REG_PRESENT_SPEED ].Address=38; MX12_OD[MX12_REG_PRESENT_SPEED ].Bytes=2; |
labmrd | 1:946a27210553 | 51 | MX12_OD[MX12_REG_PRESENT_LOAD ].Address=40; MX12_OD[MX12_REG_PRESENT_LOAD ].Bytes=2; |
labmrd | 1:946a27210553 | 52 | MX12_OD[MX12_REG_PRESENT_VOLTAGE ].Address=42; MX12_OD[MX12_REG_PRESENT_VOLTAGE ].Bytes=1; |
labmrd | 1:946a27210553 | 53 | MX12_OD[MX12_REG_PRESENT_TEMPERATURE ].Address=43; MX12_OD[MX12_REG_PRESENT_TEMPERATURE ].Bytes=1; |
labmrd | 1:946a27210553 | 54 | MX12_OD[MX12_REG_REGISTERED ].Address=44; MX12_OD[MX12_REG_REGISTERED ].Bytes=1; |
labmrd | 1:946a27210553 | 55 | MX12_OD[MX12_REG_MOVING ].Address=46; MX12_OD[MX12_REG_MOVING ].Bytes=1; |
labmrd | 1:946a27210553 | 56 | MX12_OD[MX12_REG_LOCK ].Address=47; MX12_OD[MX12_REG_LOCK ].Bytes=1; |
labmrd | 1:946a27210553 | 57 | MX12_OD[MX12_REG_PUNCH ].Address=48; MX12_OD[MX12_REG_PUNCH ].Bytes=2; |
labmrd | 1:946a27210553 | 58 | MX12_OD[MX12_REG_GOAL_ACCELERATION ].Address=73; MX12_OD[MX12_REG_GOAL_ACCELERATION ].Bytes=1; |
labmrd | 1:946a27210553 | 59 | MX12OD_Object_initalized=true; |
labmrd | 0:29900c3a4a50 | 60 | } |
labmrd | 0:29900c3a4a50 | 61 | |
labmrd | 0:29900c3a4a50 | 62 | // Set the mode of the servo |
labmrd | 0:29900c3a4a50 | 63 | // 0 = Positional (0-300 degrees) |
labmrd | 0:29900c3a4a50 | 64 | // 1 = Rotational -1 to 1 speed |
labmrd | 0:29900c3a4a50 | 65 | int MX12::SetMode(int mode) { |
labmrd | 1:946a27210553 | 66 | //// |
labmrd | 1:946a27210553 | 67 | ////Need to implement this as per http://support.robotis.com/en/product/dynamixel/mx_series/mx-12w.htm#Actuator_Address_06 |
labmrd | 1:946a27210553 | 68 | //// |
labmrd | 1:946a27210553 | 69 | ////Maybe also make a read mode? |
labmrd | 1:946a27210553 | 70 | //// |
labmrd | 1:946a27210553 | 71 | // if (mode == 1) { // set CR |
labmrd | 1:946a27210553 | 72 | // SetCWLimit(0); |
labmrd | 1:946a27210553 | 73 | // SetCCWLimit(0); |
labmrd | 1:946a27210553 | 74 | // SetCRSpeed(0.0); |
labmrd | 1:946a27210553 | 75 | // } else { |
labmrd | 1:946a27210553 | 76 | // SetCWLimit(0); |
labmrd | 1:946a27210553 | 77 | // SetCCWLimit(300); |
labmrd | 1:946a27210553 | 78 | // SetCRSpeed(0.0); |
labmrd | 1:946a27210553 | 79 | // } |
labmrd | 0:29900c3a4a50 | 80 | return(0); |
labmrd | 0:29900c3a4a50 | 81 | } |
labmrd | 0:29900c3a4a50 | 82 | |
labmrd | 0:29900c3a4a50 | 83 | |
labmrd | 1:946a27210553 | 84 | void MX12::Dump_OD_to_Serial(Serial &serialObject){ |
labmrd | 1:946a27210553 | 85 | serialObject.printf("CW Angle Limit = %f Degrees\n",Get_CW_Angle_Limit()); |
labmrd | 1:946a27210553 | 86 | serialObject.printf("CCW Angle Limit = %f Degrees\n",Get_CCW_Angle_Limit()); |
labmrd | 1:946a27210553 | 87 | serialObject.printf("Max Torque = %f Percent\n",Get_Max_Torque()); |
labmrd | 1:946a27210553 | 88 | serialObject.printf("Multi Turn Offset = %f Degrees\n",Get_Multi_Turn_Offset()); |
labmrd | 1:946a27210553 | 89 | serialObject.printf("Goal Position = %f Degrees\n",Get_Goal_Position()); |
labmrd | 1:946a27210553 | 90 | serialObject.printf("Moving Speed = %f Degrees/Second\n",Get_Moving_Speed()); |
labmrd | 1:946a27210553 | 91 | serialObject.printf("Torque Limit = %f Percent\n",Get_Torque_Limit()); |
labmrd | 1:946a27210553 | 92 | serialObject.printf("Punch = %f Percent\n",Get_Punch()); |
labmrd | 1:946a27210553 | 93 | serialObject.printf("ID = %f int\n",Get_ID()); |
labmrd | 1:946a27210553 | 94 | serialObject.printf("Baud Rate = %f Lookup\n",Get_Baud_Rate()); |
labmrd | 1:946a27210553 | 95 | serialObject.printf("Return Delay Time = %f milliseconds\n",Get_Return_Delay_Time()); |
labmrd | 1:946a27210553 | 96 | serialObject.printf("the Highest Limit Temperature = %f Celsius\n",Get_the_Highest_Limit_Temperature()); |
labmrd | 1:946a27210553 | 97 | serialObject.printf("the Lowest Limit Voltage = %f Volts\n",Get_the_Lowest_Limit_Voltage()); |
labmrd | 1:946a27210553 | 98 | serialObject.printf("the Highest Limit Voltage = %f Volts\n",Get_the_Highest_Limit_Voltage()); |
labmrd | 1:946a27210553 | 99 | serialObject.printf("Status Return Level = %f int\n",Get_Status_Return_Level()); |
labmrd | 1:946a27210553 | 100 | serialObject.printf("Alarm LED = %f Bitmap\n",Get_Alarm_LED()); |
labmrd | 1:946a27210553 | 101 | serialObject.printf("Alarm Shutdown = %f Bitmap\n",Get_Alarm_Shutdown()); |
labmrd | 1:946a27210553 | 102 | serialObject.printf("Resolution Divider = %f Ratio\n",Get_Resolution_Divider()); |
labmrd | 1:946a27210553 | 103 | serialObject.printf("Torque Enable = %f bool\n",Get_Torque_Enable()); |
labmrd | 1:946a27210553 | 104 | serialObject.printf("LED = %f Bitmap\n",Get_LED()); |
labmrd | 1:946a27210553 | 105 | serialObject.printf("D Gain = %f Kd\n",Get_D_Gain()); |
labmrd | 1:946a27210553 | 106 | serialObject.printf("I Gain = %f Ki\n",Get_I_Gain()); |
labmrd | 1:946a27210553 | 107 | serialObject.printf("P Gain = %f Kp\n",Get_P_Gain()); |
labmrd | 1:946a27210553 | 108 | serialObject.printf("Lock = %f bool\n",Get_Lock()); |
labmrd | 1:946a27210553 | 109 | serialObject.printf("Goal Acceleration = %f Degrees/Second^2\n",Get_Goal_Acceleration()); |
labmrd | 1:946a27210553 | 110 | serialObject.printf("Model Number = %f Bitmap\n",Get_Model_Number()); |
labmrd | 1:946a27210553 | 111 | serialObject.printf("Present Position = %f Degrees\n",Get_Present_Position()); |
labmrd | 1:946a27210553 | 112 | serialObject.printf("Present Speed = %f Degrees/Second\n",Get_Present_Speed()); |
labmrd | 1:946a27210553 | 113 | serialObject.printf("Present Load = %f Percent\n",Get_Present_Load()); |
labmrd | 1:946a27210553 | 114 | serialObject.printf("Version of Firmware = %f int\n",Get_Version_of_Firmware()); |
labmrd | 1:946a27210553 | 115 | serialObject.printf("Present Voltage = %f Volts\n",Get_Present_Voltage()); |
labmrd | 1:946a27210553 | 116 | serialObject.printf("Present Temperature = %f Celsius\n",Get_Present_Temperature()); |
labmrd | 1:946a27210553 | 117 | serialObject.printf("Registered = %f bool\n",Get_Registered()); |
labmrd | 1:946a27210553 | 118 | serialObject.printf("Moving = %f bool\n",Get_Moving()); |
labmrd | 1:946a27210553 | 119 | } |
labmrd | 0:29900c3a4a50 | 120 | |
labmrd | 1:946a27210553 | 121 | void MX12::Scan_For_Dynamixels(bool scan_all_baud_rates,int max_id) |
labmrd | 1:946a27210553 | 122 | { |
labmrd | 1:946a27210553 | 123 | char ID_Num; |
labmrd | 1:946a27210553 | 124 | if(scan_all_baud_rates){ |
labmrd | 1:946a27210553 | 125 | int baud_rate[12]={3000000,2500000,2250000,1000000,500000,400000,250000,200000,115200,57600,19200,9600}; |
labmrd | 1:946a27210553 | 126 | char ii; |
labmrd | 1:946a27210553 | 127 | for(ii=0;ii<12;ii++){ |
labmrd | 1:946a27210553 | 128 | ChangeUARTBaud(baud_rate[ii]); |
labmrd | 1:946a27210553 | 129 | for(ID_Num=0;ID_Num<=max_id;ID_Num++){ |
labmrd | 1:946a27210553 | 130 | if(ping(ID_Num)){ |
labmrd | 1:946a27210553 | 131 | printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]); |
labmrd | 1:946a27210553 | 132 | } |
labmrd | 1:946a27210553 | 133 | } |
labmrd | 1:946a27210553 | 134 | } |
labmrd | 1:946a27210553 | 135 | }else{ |
labmrd | 1:946a27210553 | 136 | for(ID_Num=0;ID_Num<=max_id;ID_Num++){ |
labmrd | 1:946a27210553 | 137 | if(ping(ID_Num)){ |
labmrd | 1:946a27210553 | 138 | printf("Found a servo at ID=%#02x\n",ID_Num); |
labmrd | 1:946a27210553 | 139 | } |
labmrd | 1:946a27210553 | 140 | } |
labmrd | 1:946a27210553 | 141 | } |
labmrd | 1:946a27210553 | 142 | |
labmrd | 1:946a27210553 | 143 | } |
labmrd | 1:946a27210553 | 144 | |
labmrd | 1:946a27210553 | 145 | int MX12::SetBaud( int target_baud ) { |
labmrd | 1:946a27210553 | 146 | |
labmrd | 1:946a27210553 | 147 | short baud_int; |
labmrd | 1:946a27210553 | 148 | |
labmrd | 1:946a27210553 | 149 | if(target_baud<=1000000){ |
labmrd | 1:946a27210553 | 150 | baud_int=2000000/target_baud-1; |
labmrd | 1:946a27210553 | 151 | }else if(target_baud==2250000){ |
labmrd | 1:946a27210553 | 152 | baud_int=250; |
labmrd | 1:946a27210553 | 153 | }else if(target_baud==2500000){ |
labmrd | 1:946a27210553 | 154 | baud_int=251; |
labmrd | 1:946a27210553 | 155 | }else if(target_baud==3000000){ |
labmrd | 1:946a27210553 | 156 | baud_int=252; |
labmrd | 1:946a27210553 | 157 | }else{ |
labmrd | 1:946a27210553 | 158 | printf("Error! Invalid baud rate of %d!\n",target_baud); |
labmrd | 1:946a27210553 | 159 | return 0; |
labmrd | 1:946a27210553 | 160 | } |
labmrd | 1:946a27210553 | 161 | |
labmrd | 1:946a27210553 | 162 | #if MX12_DEBUG |
labmrd | 1:946a27210553 | 163 | printf("SetBaudRate to 0x%x\n",baud_int); |
labmrd | 1:946a27210553 | 164 | #endif |
labmrd | 1:946a27210553 | 165 | int ret=(write_short(MX12_REG_BAUD_RATE,baud_int)); |
labmrd | 1:946a27210553 | 166 | |
labmrd | 1:946a27210553 | 167 | // Now change the UART serial rate to the same? |
labmrd | 1:946a27210553 | 168 | // But to allow changing several servos, allow user |
labmrd | 1:946a27210553 | 169 | // to do this at their liesure. |
labmrd | 1:946a27210553 | 170 | // ChangeUARTBaud( target_baud ); |
labmrd | 1:946a27210553 | 171 | |
labmrd | 1:946a27210553 | 172 | return ret; |
labmrd | 1:946a27210553 | 173 | } |
labmrd | 1:946a27210553 | 174 | |
labmrd | 1:946a27210553 | 175 | void MX12::ChangeUARTBaud( int target_baud ) { |
labmrd | 1:946a27210553 | 176 | mx12_out.baud(target_baud); |
labmrd | 1:946a27210553 | 177 | mx12_in.baud(target_baud); |
labmrd | 1:946a27210553 | 178 | } |
labmrd | 1:946a27210553 | 179 | |
labmrd | 1:946a27210553 | 180 | short MX12::GetRawPosition(void) { |
labmrd | 1:946a27210553 | 181 | #if MX12_DEBUG |
labmrd | 1:946a27210553 | 182 | printf("\nGetRawPosition(%d)",_ID); |
labmrd | 1:946a27210553 | 183 | #endif |
labmrd | 1:946a27210553 | 184 | return read_short(MX12_REG_PRESENT_POSITION); |
labmrd | 1:946a27210553 | 185 | } |
labmrd | 1:946a27210553 | 186 | |
labmrd | 1:946a27210553 | 187 | int MX12:: write_short(MX12ODIndex OD,short value) |
labmrd | 1:946a27210553 | 188 | { |
labmrd | 0:29900c3a4a50 | 189 | char data[2]; |
labmrd | 0:29900c3a4a50 | 190 | |
labmrd | 1:946a27210553 | 191 | data[0] = value & 0xff; // bottom 8 bits |
labmrd | 1:946a27210553 | 192 | data[1] = value >> 8; // top 8 bits |
labmrd | 0:29900c3a4a50 | 193 | |
labmrd | 0:29900c3a4a50 | 194 | // write the packet, return the error code |
labmrd | 1:946a27210553 | 195 | int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data); |
labmrd | 3:624d04c390b8 | 196 | if(rVal>0){ |
labmrd | 3:624d04c390b8 | 197 | if(CHECK_BIT(rVal, 0)){printf("Error! Input Voltage Error\n");} |
labmrd | 3:624d04c390b8 | 198 | if(CHECK_BIT(rVal, 1)){printf("Error! Angle Limit Error\n");} |
labmrd | 3:624d04c390b8 | 199 | if(CHECK_BIT(rVal, 2)){printf("Error! Overheating Error\n");} |
labmrd | 3:624d04c390b8 | 200 | if(CHECK_BIT(rVal, 3)){printf("Error! Range Error\n");} |
labmrd | 3:624d04c390b8 | 201 | if(CHECK_BIT(rVal, 4)){printf("Error! Checksum Error\n");} |
labmrd | 3:624d04c390b8 | 202 | if(CHECK_BIT(rVal, 5)){printf("Error! Overload Error\n");} |
labmrd | 3:624d04c390b8 | 203 | if(CHECK_BIT(rVal, 6)){printf("Error! Instruction Error\n");} |
labmrd | 3:624d04c390b8 | 204 | if(CHECK_BIT(rVal, 7)){printf("Error! Undefined Error\n");} |
labmrd | 3:624d04c390b8 | 205 | } |
labmrd | 0:29900c3a4a50 | 206 | return(rVal); |
labmrd | 0:29900c3a4a50 | 207 | } |
labmrd | 0:29900c3a4a50 | 208 | |
labmrd | 1:946a27210553 | 209 | short MX12:: read_short(MX12ODIndex OD) |
labmrd | 1:946a27210553 | 210 | { |
labmrd | 0:29900c3a4a50 | 211 | |
labmrd | 0:29900c3a4a50 | 212 | char data[2]; |
labmrd | 1:946a27210553 | 213 | data[1]=0; |
labmrd | 0:29900c3a4a50 | 214 | |
labmrd | 1:946a27210553 | 215 | int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data); |
labmrd | 3:624d04c390b8 | 216 | profileOut = (ErrorCode!=0); |
labmrd | 1:946a27210553 | 217 | short value = data[0] + (data[1] << 8); |
labmrd | 1:946a27210553 | 218 | |
labmrd | 1:946a27210553 | 219 | return (value); |
labmrd | 0:29900c3a4a50 | 220 | } |
labmrd | 0:29900c3a4a50 | 221 | |
labmrd | 0:29900c3a4a50 | 222 | |
labmrd | 0:29900c3a4a50 | 223 | |
labmrd | 1:946a27210553 | 224 | //////////////////////////////////////////// |
labmrd | 1:946a27210553 | 225 | // // |
labmrd | 1:946a27210553 | 226 | // // |
labmrd | 1:946a27210553 | 227 | // PRIVATE FUNCTIONS // |
labmrd | 1:946a27210553 | 228 | // // |
labmrd | 1:946a27210553 | 229 | // // |
labmrd | 1:946a27210553 | 230 | //////////////////////////////////////////// |
labmrd | 0:29900c3a4a50 | 231 | |
labmrd | 0:29900c3a4a50 | 232 | |
labmrd | 0:29900c3a4a50 | 233 | |
labmrd | 0:29900c3a4a50 | 234 | |
labmrd | 0:29900c3a4a50 | 235 | |
labmrd | 0:29900c3a4a50 | 236 | int MX12::read(int ID, int start, int bytes, char* data) { |
labmrd | 1:946a27210553 | 237 | |
labmrd | 0:29900c3a4a50 | 238 | char PacketLength = 0x4; |
labmrd | 0:29900c3a4a50 | 239 | char TxBuf[16]; |
labmrd | 0:29900c3a4a50 | 240 | char sum = 0; |
labmrd | 0:29900c3a4a50 | 241 | char Status[16]; |
labmrd | 0:29900c3a4a50 | 242 | |
labmrd | 0:29900c3a4a50 | 243 | Status[4] = 0xFE; // return code |
labmrd | 0:29900c3a4a50 | 244 | |
labmrd | 1:946a27210553 | 245 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 246 | printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); |
labmrd | 1:946a27210553 | 247 | #endif |
labmrd | 0:29900c3a4a50 | 248 | |
labmrd | 0:29900c3a4a50 | 249 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 250 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 251 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
labmrd | 1:946a27210553 | 252 | #endif |
labmrd | 0:29900c3a4a50 | 253 | |
labmrd | 0:29900c3a4a50 | 254 | TxBuf[0] = 0xff; |
labmrd | 0:29900c3a4a50 | 255 | TxBuf[1] = 0xff; |
labmrd | 0:29900c3a4a50 | 256 | |
labmrd | 0:29900c3a4a50 | 257 | // ID |
labmrd | 0:29900c3a4a50 | 258 | TxBuf[2] = ID; |
labmrd | 0:29900c3a4a50 | 259 | sum += TxBuf[2]; |
labmrd | 1:946a27210553 | 260 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 261 | printf(" ID : %d\n",TxBuf[2]); |
labmrd | 1:946a27210553 | 262 | #endif |
labmrd | 0:29900c3a4a50 | 263 | |
labmrd | 0:29900c3a4a50 | 264 | // Packet Length |
labmrd | 0:29900c3a4a50 | 265 | TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
labmrd | 0:29900c3a4a50 | 266 | sum += TxBuf[3]; // Accululate the packet sum |
labmrd | 1:946a27210553 | 267 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 268 | printf(" Length : 0x%x\n",TxBuf[3]); |
labmrd | 1:946a27210553 | 269 | #endif |
labmrd | 0:29900c3a4a50 | 270 | |
labmrd | 0:29900c3a4a50 | 271 | // Instruction - Read |
labmrd | 0:29900c3a4a50 | 272 | TxBuf[4] = 0x2; |
labmrd | 0:29900c3a4a50 | 273 | sum += TxBuf[4]; |
labmrd | 1:946a27210553 | 274 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 275 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
labmrd | 1:946a27210553 | 276 | #endif |
labmrd | 0:29900c3a4a50 | 277 | |
labmrd | 0:29900c3a4a50 | 278 | // Start Address |
labmrd | 0:29900c3a4a50 | 279 | TxBuf[5] = start; |
labmrd | 0:29900c3a4a50 | 280 | sum += TxBuf[5]; |
labmrd | 1:946a27210553 | 281 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 282 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 283 | #endif |
labmrd | 0:29900c3a4a50 | 284 | |
labmrd | 0:29900c3a4a50 | 285 | // Bytes to read |
labmrd | 0:29900c3a4a50 | 286 | TxBuf[6] = bytes; |
labmrd | 0:29900c3a4a50 | 287 | sum += TxBuf[6]; |
labmrd | 1:946a27210553 | 288 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 289 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
labmrd | 1:946a27210553 | 290 | #endif |
labmrd | 0:29900c3a4a50 | 291 | |
labmrd | 0:29900c3a4a50 | 292 | // Checksum |
labmrd | 0:29900c3a4a50 | 293 | TxBuf[7] = 0xFF - sum; |
labmrd | 1:946a27210553 | 294 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 295 | printf(" Checksum : 0x%x\n",TxBuf[7]); |
labmrd | 1:946a27210553 | 296 | #endif |
labmrd | 1:946a27210553 | 297 | |
labmrd | 1:946a27210553 | 298 | // Clear in input buffer first |
labmrd | 1:946a27210553 | 299 | while (mx12_in.readable()) { |
labmrd | 1:946a27210553 | 300 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 301 | printf("Purging one character (read).\n"); |
labmrd | 0:29900c3a4a50 | 302 | } |
labmrd | 1:946a27210553 | 303 | |
labmrd | 0:29900c3a4a50 | 304 | // Transmit the packet in one burst with no pausing |
labmrd | 0:29900c3a4a50 | 305 | for (int i = 0; i<8 ; i++) { |
labmrd | 1:946a27210553 | 306 | mx12_out.putc(TxBuf[i]); |
labmrd | 0:29900c3a4a50 | 307 | } |
labmrd | 1:946a27210553 | 308 | |
labmrd | 1:946a27210553 | 309 | // Read |
labmrd | 1:946a27210553 | 310 | for (int i = 0; i<8 ; i++) { |
labmrd | 3:624d04c390b8 | 311 | //profileOut=i%2; |
labmrd | 1:946a27210553 | 312 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 313 | } |
labmrd | 1:946a27210553 | 314 | |
labmrd | 0:29900c3a4a50 | 315 | // Wait for the bytes to be transmitted |
labmrd | 0:29900c3a4a50 | 316 | //wait (0.00002); |
labmrd | 0:29900c3a4a50 | 317 | |
labmrd | 0:29900c3a4a50 | 318 | // Skip if the read was to the broadcast address |
labmrd | 0:29900c3a4a50 | 319 | if (_ID != 0xFE) { |
labmrd | 0:29900c3a4a50 | 320 | |
labmrd | 0:29900c3a4a50 | 321 | // Receive the Status packet 6+ number of bytes read |
labmrd | 1:946a27210553 | 322 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 323 | printf(" Reading Byte:"); |
labmrd | 1:946a27210553 | 324 | #endif |
labmrd | 0:29900c3a4a50 | 325 | Timer serial_timeout; |
labmrd | 1:946a27210553 | 326 | serial_timeout.start (); |
labmrd | 1:946a27210553 | 327 | |
labmrd | 0:29900c3a4a50 | 328 | for (int i=0; i<(6+bytes) ; i++) { |
labmrd | 0:29900c3a4a50 | 329 | serial_timeout.reset (); |
labmrd | 1:946a27210553 | 330 | while (!mx12_in.readable ()) |
labmrd | 0:29900c3a4a50 | 331 | { |
labmrd | 0:29900c3a4a50 | 332 | //Just loop here until you either get a character, or we timeout |
labmrd | 1:946a27210553 | 333 | if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US) |
labmrd | 0:29900c3a4a50 | 334 | { |
labmrd | 0:29900c3a4a50 | 335 | //If we timeout, quit in a panic! |
labmrd | 3:624d04c390b8 | 336 | printf("Error! Serial Timeout %d\n",i); |
labmrd | 0:29900c3a4a50 | 337 | return(0x00); |
labmrd | 0:29900c3a4a50 | 338 | } |
labmrd | 0:29900c3a4a50 | 339 | } |
labmrd | 1:946a27210553 | 340 | Status[i] = mx12_in.getc(); |
labmrd | 0:29900c3a4a50 | 341 | } |
labmrd | 1:946a27210553 | 342 | |
labmrd | 1:946a27210553 | 343 | if(Status[0]!=0xFF || Status[1]!=0xFF) |
labmrd | 1:946a27210553 | 344 | { |
labmrd | 3:624d04c390b8 | 345 | printf("Header Error!\n"); |
labmrd | 3:624d04c390b8 | 346 | //printf("Unexpected header in serial response!\n"); |
labmrd | 3:624d04c390b8 | 347 | //printf(" Header : 0x%x\n",Status[0]); |
labmrd | 3:624d04c390b8 | 348 | //printf(" Header : 0x%x\n",Status[1]); |
labmrd | 3:624d04c390b8 | 349 | return 255; |
labmrd | 3:624d04c390b8 | 350 | } |
labmrd | 3:624d04c390b8 | 351 | if(Status[2]!=_ID){ |
labmrd | 3:624d04c390b8 | 352 | printf("ID Error!\n"); |
labmrd | 3:624d04c390b8 | 353 | return 255; |
labmrd | 3:624d04c390b8 | 354 | } |
labmrd | 3:624d04c390b8 | 355 | if(Status[3]!=bytes+2){ |
labmrd | 3:624d04c390b8 | 356 | printf("Length Error!\n"); |
labmrd | 3:624d04c390b8 | 357 | return 255; |
labmrd | 3:624d04c390b8 | 358 | } |
labmrd | 3:624d04c390b8 | 359 | |
labmrd | 3:624d04c390b8 | 360 | char sum=Status[2]+Status[3]+Status[4]+Status[5]; |
labmrd | 3:624d04c390b8 | 361 | if( bytes==2 ) |
labmrd | 3:624d04c390b8 | 362 | sum+=Status[6]; |
labmrd | 3:624d04c390b8 | 363 | if( Status[5+bytes]!=0xff-sum){ |
labmrd | 3:624d04c390b8 | 364 | printf("Checksum Error!\n"); |
labmrd | 3:624d04c390b8 | 365 | return 255; |
labmrd | 0:29900c3a4a50 | 366 | } |
labmrd | 0:29900c3a4a50 | 367 | |
labmrd | 0:29900c3a4a50 | 368 | // Copy the data from Status into data for return |
labmrd | 3:624d04c390b8 | 369 | for (int i=0; i < bytes ; i++) { |
labmrd | 0:29900c3a4a50 | 370 | data[i] = Status[5+i]; |
labmrd | 0:29900c3a4a50 | 371 | } |
labmrd | 0:29900c3a4a50 | 372 | |
labmrd | 1:946a27210553 | 373 | #if MX12_READ_DEBUG |
labmrd | 0:29900c3a4a50 | 374 | printf("\nStatus Packet\n"); |
labmrd | 0:29900c3a4a50 | 375 | printf(" Header : 0x%x\n",Status[0]); |
labmrd | 0:29900c3a4a50 | 376 | printf(" Header : 0x%x\n",Status[1]); |
labmrd | 0:29900c3a4a50 | 377 | printf(" ID : 0x%x\n",Status[2]); |
labmrd | 0:29900c3a4a50 | 378 | printf(" Length : 0x%x\n",Status[3]); |
labmrd | 0:29900c3a4a50 | 379 | printf(" Error Code : 0x%x\n",Status[4]); |
labmrd | 0:29900c3a4a50 | 380 | |
labmrd | 3:624d04c390b8 | 381 | for (int i=0; i < bytes ; i++) { |
labmrd | 0:29900c3a4a50 | 382 | printf(" Data : 0x%x\n",Status[5+i]); |
labmrd | 0:29900c3a4a50 | 383 | } |
labmrd | 0:29900c3a4a50 | 384 | |
labmrd | 3:624d04c390b8 | 385 | printf(" Checksum : 0x%x\n",Status[5+bytes]); |
labmrd | 1:946a27210553 | 386 | #endif |
labmrd | 0:29900c3a4a50 | 387 | |
labmrd | 0:29900c3a4a50 | 388 | } // if (ID!=0xFE) |
labmrd | 0:29900c3a4a50 | 389 | |
labmrd | 0:29900c3a4a50 | 390 | return(Status[4]); |
labmrd | 0:29900c3a4a50 | 391 | } |
labmrd | 0:29900c3a4a50 | 392 | |
labmrd | 0:29900c3a4a50 | 393 | |
labmrd | 1:946a27210553 | 394 | |
labmrd | 1:946a27210553 | 395 | |
labmrd | 1:946a27210553 | 396 | int MX12:: write(int ID, int start, int bytes, char* data) { |
labmrd | 0:29900c3a4a50 | 397 | // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum |
labmrd | 0:29900c3a4a50 | 398 | |
labmrd | 0:29900c3a4a50 | 399 | char TxBuf[16]; |
labmrd | 0:29900c3a4a50 | 400 | char sum = 0; |
labmrd | 0:29900c3a4a50 | 401 | char Status[6]; |
labmrd | 0:29900c3a4a50 | 402 | |
labmrd | 1:946a27210553 | 403 | #if MX12_WRITE_DEBUG |
labmrd | 3:624d04c390b8 | 404 | printf("\nwrite(0x%02x,0x%02x,0x%02x)\n",ID,start,bytes); |
labmrd | 1:946a27210553 | 405 | #endif |
labmrd | 0:29900c3a4a50 | 406 | |
labmrd | 0:29900c3a4a50 | 407 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 408 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 409 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
labmrd | 1:946a27210553 | 410 | #endif |
labmrd | 0:29900c3a4a50 | 411 | |
labmrd | 1:946a27210553 | 412 | TxBuf[0] = MX12_INSTRUCTION_HEADER; |
labmrd | 1:946a27210553 | 413 | TxBuf[1] = MX12_INSTRUCTION_HEADER; |
labmrd | 0:29900c3a4a50 | 414 | |
labmrd | 0:29900c3a4a50 | 415 | // ID |
labmrd | 0:29900c3a4a50 | 416 | TxBuf[2] = ID; |
labmrd | 0:29900c3a4a50 | 417 | sum += TxBuf[2]; |
labmrd | 0:29900c3a4a50 | 418 | |
labmrd | 1:946a27210553 | 419 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 420 | printf(" ID : %d\n",TxBuf[2]); |
labmrd | 1:946a27210553 | 421 | #endif |
labmrd | 0:29900c3a4a50 | 422 | |
labmrd | 0:29900c3a4a50 | 423 | // packet Length |
labmrd | 0:29900c3a4a50 | 424 | TxBuf[3] = 3+bytes; |
labmrd | 0:29900c3a4a50 | 425 | sum += TxBuf[3]; |
labmrd | 0:29900c3a4a50 | 426 | |
labmrd | 1:946a27210553 | 427 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 428 | printf(" Length : %d\n",TxBuf[3]); |
labmrd | 1:946a27210553 | 429 | #endif |
labmrd | 0:29900c3a4a50 | 430 | |
labmrd | 0:29900c3a4a50 | 431 | // Instruction |
labmrd | 1:946a27210553 | 432 | // if (flag == 1) { |
labmrd | 1:946a27210553 | 433 | // TxBuf[4]=0x04; |
labmrd | 1:946a27210553 | 434 | // } else { |
labmrd | 0:29900c3a4a50 | 435 | TxBuf[4]=0x03; |
labmrd | 1:946a27210553 | 436 | // } |
labmrd | 1:946a27210553 | 437 | sum += TxBuf[4]; |
labmrd | 0:29900c3a4a50 | 438 | |
labmrd | 1:946a27210553 | 439 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 440 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
labmrd | 1:946a27210553 | 441 | #endif |
labmrd | 0:29900c3a4a50 | 442 | |
labmrd | 0:29900c3a4a50 | 443 | // Start Address |
labmrd | 0:29900c3a4a50 | 444 | TxBuf[5] = start; |
labmrd | 0:29900c3a4a50 | 445 | sum += TxBuf[5]; |
labmrd | 1:946a27210553 | 446 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 447 | printf(" Start : 0x%x\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 448 | #endif |
labmrd | 0:29900c3a4a50 | 449 | |
labmrd | 0:29900c3a4a50 | 450 | // data |
labmrd | 0:29900c3a4a50 | 451 | for (char i=0; i<bytes ; i++) { |
labmrd | 0:29900c3a4a50 | 452 | TxBuf[6+i] = data[i]; |
labmrd | 0:29900c3a4a50 | 453 | sum += TxBuf[6+i]; |
labmrd | 1:946a27210553 | 454 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 455 | printf(" Data : 0x%x\n",TxBuf[6+i]); |
labmrd | 1:946a27210553 | 456 | #endif |
labmrd | 0:29900c3a4a50 | 457 | } |
labmrd | 0:29900c3a4a50 | 458 | |
labmrd | 0:29900c3a4a50 | 459 | // checksum |
labmrd | 0:29900c3a4a50 | 460 | TxBuf[6+bytes] = 0xFF - sum; |
labmrd | 1:946a27210553 | 461 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 462 | printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); |
labmrd | 1:946a27210553 | 463 | #endif |
labmrd | 1:946a27210553 | 464 | |
labmrd | 1:946a27210553 | 465 | // Clear in input buffer first |
labmrd | 1:946a27210553 | 466 | while (mx12_in.readable()) { |
labmrd | 1:946a27210553 | 467 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 468 | printf("Purging one character (write).\n"); |
labmrd | 0:29900c3a4a50 | 469 | } |
labmrd | 0:29900c3a4a50 | 470 | |
labmrd | 0:29900c3a4a50 | 471 | // Transmit the packet in one burst with no pausing |
labmrd | 0:29900c3a4a50 | 472 | for (int i = 0; i < (7 + bytes) ; i++) { |
labmrd | 1:946a27210553 | 473 | mx12_out.putc(TxBuf[i]); |
labmrd | 1:946a27210553 | 474 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 475 | //printf("Echo: 0x%02x\n",mx12_in.getc()); |
labmrd | 0:29900c3a4a50 | 476 | } |
labmrd | 0:29900c3a4a50 | 477 | |
labmrd | 0:29900c3a4a50 | 478 | // make sure we have a valid return |
labmrd | 0:29900c3a4a50 | 479 | Status[4]=0x00; |
labmrd | 0:29900c3a4a50 | 480 | |
labmrd | 0:29900c3a4a50 | 481 | // we'll only get a reply if it was not broadcast |
labmrd | 0:29900c3a4a50 | 482 | if (_ID!=0xFE) { |
labmrd | 0:29900c3a4a50 | 483 | |
labmrd | 0:29900c3a4a50 | 484 | // response is always 6 bytes |
labmrd | 1:946a27210553 | 485 | // 0xFF, 0xFF, ID, Length, Error, Checksum |
labmrd | 0:29900c3a4a50 | 486 | Timer serial_timeout; |
labmrd | 1:946a27210553 | 487 | serial_timeout.start (); |
labmrd | 1:946a27210553 | 488 | |
labmrd | 0:29900c3a4a50 | 489 | for (int i=0; i < 6 ; i++) { |
labmrd | 0:29900c3a4a50 | 490 | serial_timeout.reset (); |
labmrd | 1:946a27210553 | 491 | while (!mx12_in.readable ()) |
labmrd | 0:29900c3a4a50 | 492 | { |
labmrd | 0:29900c3a4a50 | 493 | //Just loop here until you either get a character, or we timeout |
labmrd | 1:946a27210553 | 494 | if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US) |
labmrd | 0:29900c3a4a50 | 495 | { |
labmrd | 0:29900c3a4a50 | 496 | //If we timeout, quit in a panic! |
labmrd | 3:624d04c390b8 | 497 | printf("Error! Serial Timeout %d\n",i); |
labmrd | 0:29900c3a4a50 | 498 | return(0x00); |
labmrd | 0:29900c3a4a50 | 499 | } |
labmrd | 0:29900c3a4a50 | 500 | } |
labmrd | 1:946a27210553 | 501 | Status[i] = mx12_in.getc(); |
labmrd | 0:29900c3a4a50 | 502 | } |
labmrd | 1:946a27210553 | 503 | |
labmrd | 3:624d04c390b8 | 504 | if(Status[0]!=0xFF || Status[1]!=0xFF) |
labmrd | 3:624d04c390b8 | 505 | { |
labmrd | 3:624d04c390b8 | 506 | printf("Unexpected header in serial response!\n"); |
labmrd | 3:624d04c390b8 | 507 | printf(" Header : 0x%x\n",Status[0]); |
labmrd | 3:624d04c390b8 | 508 | printf(" Header : 0x%x\n",Status[1]); |
labmrd | 3:624d04c390b8 | 509 | return 255; |
labmrd | 3:624d04c390b8 | 510 | } |
labmrd | 3:624d04c390b8 | 511 | if( Status[5]!=0xff-(Status[2]+Status[3]+Status[4])){ |
labmrd | 3:624d04c390b8 | 512 | printf("Checksum Error!\n"); |
labmrd | 3:624d04c390b8 | 513 | return 255; |
labmrd | 3:624d04c390b8 | 514 | } |
labmrd | 3:624d04c390b8 | 515 | |
labmrd | 3:624d04c390b8 | 516 | |
labmrd | 0:29900c3a4a50 | 517 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 518 | #if MX12_WRITE_DEBUG |
labmrd | 0:29900c3a4a50 | 519 | printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); |
labmrd | 0:29900c3a4a50 | 520 | printf(" ID : %d\n",Status[2]); |
labmrd | 0:29900c3a4a50 | 521 | printf(" Length : %d\n",Status[3]); |
labmrd | 0:29900c3a4a50 | 522 | printf(" Error : 0x%x\n",Status[4]); |
labmrd | 0:29900c3a4a50 | 523 | printf(" Checksum : 0x%x\n",Status[5]); |
labmrd | 1:946a27210553 | 524 | #endif |
labmrd | 0:29900c3a4a50 | 525 | |
labmrd | 0:29900c3a4a50 | 526 | |
labmrd | 0:29900c3a4a50 | 527 | } |
labmrd | 0:29900c3a4a50 | 528 | |
labmrd | 0:29900c3a4a50 | 529 | return(Status[4]); // return error code |
labmrd | 0:29900c3a4a50 | 530 | } |
labmrd | 1:946a27210553 | 531 | |
labmrd | 1:946a27210553 | 532 | void MX12::coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1) |
labmrd | 1:946a27210553 | 533 | { |
labmrd | 1:946a27210553 | 534 | char NumDevices = 0x2; |
labmrd | 1:946a27210553 | 535 | char DataLength = 0x4;//!< Hardcoded for now, 2 bytes for pos, 2 bytes for vel |
labmrd | 1:946a27210553 | 536 | char PacketLength = 0x4+NumDevices*(DataLength+0x1); |
labmrd | 1:946a27210553 | 537 | char StartAddress = MX12_OD[MX12_REG_GOAL_POSITION].Address; |
labmrd | 1:946a27210553 | 538 | char TxBuf[20]; |
labmrd | 1:946a27210553 | 539 | char sum = 0; |
labmrd | 1:946a27210553 | 540 | char ii=0; |
labmrd | 1:946a27210553 | 541 | char jj=0; |
labmrd | 1:946a27210553 | 542 | char offset=0; |
labmrd | 1:946a27210553 | 543 | |
labmrd | 1:946a27210553 | 544 | // Hardcoded for now, 2 devices |
labmrd | 1:946a27210553 | 545 | char ID_Num[NumDevices]; |
labmrd | 1:946a27210553 | 546 | ID_Num[0]=id0; |
labmrd | 1:946a27210553 | 547 | ID_Num[1]=id1; |
labmrd | 1:946a27210553 | 548 | |
labmrd | 1:946a27210553 | 549 | // Hardcoded for now, 2 bytes for pos, 2 bytes for vel, 2 devices |
labmrd | 1:946a27210553 | 550 | char data[NumDevices][DataLength]; |
labmrd | 1:946a27210553 | 551 | // A faster/better way to do this would be with a fancy memcpy from |
labmrd | 1:946a27210553 | 552 | // short to char or maybe a union or just raw pointers... |
labmrd | 1:946a27210553 | 553 | data[0][0] = pos0 & 0xff; // bottom 8 bits |
labmrd | 1:946a27210553 | 554 | data[0][1] = pos0 >> 8; // top 8 bits |
labmrd | 1:946a27210553 | 555 | data[0][2] = vel0 & 0xff; // bottom 8 bits |
labmrd | 1:946a27210553 | 556 | data[0][3] = vel0 >> 8; // top 8 bits |
labmrd | 1:946a27210553 | 557 | data[1][0] = pos1 & 0xff; // bottom 8 bits |
labmrd | 1:946a27210553 | 558 | data[1][1] = pos1 >> 8; // top 8 bits |
labmrd | 1:946a27210553 | 559 | data[1][2] = vel1 & 0xff; // bottom 8 bits |
labmrd | 1:946a27210553 | 560 | data[1][3] = vel1 >> 8; // top 8 bits |
labmrd | 1:946a27210553 | 561 | |
labmrd | 1:946a27210553 | 562 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 563 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 564 | printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); |
labmrd | 1:946a27210553 | 565 | #endif |
labmrd | 1:946a27210553 | 566 | |
labmrd | 1:946a27210553 | 567 | TxBuf[0] = 0xff; |
labmrd | 1:946a27210553 | 568 | TxBuf[1] = 0xff; |
labmrd | 1:946a27210553 | 569 | |
labmrd | 1:946a27210553 | 570 | // ID |
labmrd | 1:946a27210553 | 571 | TxBuf[2] = 0xfe; |
labmrd | 1:946a27210553 | 572 | sum += TxBuf[2]; |
labmrd | 1:946a27210553 | 573 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 574 | printf(" ID : %d\n",TxBuf[2]); |
labmrd | 1:946a27210553 | 575 | #endif |
labmrd | 1:946a27210553 | 576 | |
labmrd | 1:946a27210553 | 577 | // Packet Length |
labmrd | 1:946a27210553 | 578 | TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) |
labmrd | 1:946a27210553 | 579 | sum += TxBuf[3]; // Accululate the packet sum |
labmrd | 1:946a27210553 | 580 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 581 | printf(" Length : 0x%x\n",TxBuf[3]); |
labmrd | 1:946a27210553 | 582 | #endif |
labmrd | 1:946a27210553 | 583 | |
labmrd | 1:946a27210553 | 584 | // Instruction - Sync Write |
labmrd | 1:946a27210553 | 585 | TxBuf[4] = 0x83; |
labmrd | 1:946a27210553 | 586 | sum += TxBuf[4]; |
labmrd | 1:946a27210553 | 587 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 588 | printf(" Instruction : 0x%x\n",TxBuf[4]); |
labmrd | 1:946a27210553 | 589 | #endif |
labmrd | 1:946a27210553 | 590 | |
labmrd | 1:946a27210553 | 591 | // Start Address |
labmrd | 1:946a27210553 | 592 | TxBuf[5] = StartAddress; |
labmrd | 1:946a27210553 | 593 | sum += TxBuf[5]; |
labmrd | 1:946a27210553 | 594 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 595 | printf(" Start Address : 0x%x\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 596 | #endif |
labmrd | 1:946a27210553 | 597 | |
labmrd | 1:946a27210553 | 598 | // Bytes to write to each device |
labmrd | 1:946a27210553 | 599 | TxBuf[6] = DataLength; |
labmrd | 1:946a27210553 | 600 | sum += TxBuf[6]; |
labmrd | 1:946a27210553 | 601 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 602 | printf(" No bytes : 0x%x\n",TxBuf[6]); |
labmrd | 1:946a27210553 | 603 | #endif |
labmrd | 1:946a27210553 | 604 | |
labmrd | 1:946a27210553 | 605 | // The data itself |
labmrd | 1:946a27210553 | 606 | for(ii=0;ii<NumDevices;ii++){ |
labmrd | 1:946a27210553 | 607 | // Store this offset in a variable since we use it a lot. |
labmrd | 1:946a27210553 | 608 | // The 7 comes from the fact that the last write was at 6, |
labmrd | 1:946a27210553 | 609 | // so this one should start at 7. The 0x1 is for the ID. |
labmrd | 1:946a27210553 | 610 | offset=7+ii*(DataLength+0x1); |
labmrd | 1:946a27210553 | 611 | // Write the ID of the device |
labmrd | 1:946a27210553 | 612 | TxBuf[offset] = ID_Num[ii]; |
labmrd | 1:946a27210553 | 613 | sum += TxBuf[offset]; |
labmrd | 1:946a27210553 | 614 | |
labmrd | 1:946a27210553 | 615 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 616 | printf(" ID #%d : 0x%x\n",ii,TxBuf[offset]); |
labmrd | 1:946a27210553 | 617 | #endif |
labmrd | 1:946a27210553 | 618 | |
labmrd | 1:946a27210553 | 619 | // Write each of the bytes of the data |
labmrd | 3:624d04c390b8 | 620 | for(jj=0;jj<DataLength;jj++){ |
labmrd | 3:624d04c390b8 | 621 | TxBuf[offset+jj+0x1] = data[ii][jj]; |
labmrd | 3:624d04c390b8 | 622 | sum += TxBuf[offset+jj+0x1]; |
labmrd | 1:946a27210553 | 623 | #if MX12_WRITE_DEBUG |
labmrd | 3:624d04c390b8 | 624 | printf(" Data #%d : 0x%x\n",jj,TxBuf[offset+jj+0x1]); |
labmrd | 1:946a27210553 | 625 | #endif |
labmrd | 1:946a27210553 | 626 | } |
labmrd | 1:946a27210553 | 627 | } |
labmrd | 1:946a27210553 | 628 | |
labmrd | 1:946a27210553 | 629 | |
labmrd | 1:946a27210553 | 630 | // Checksum |
labmrd | 1:946a27210553 | 631 | offset=7+NumDevices*(DataLength+0x1); |
labmrd | 1:946a27210553 | 632 | TxBuf[offset] = 0xFF - sum; |
labmrd | 1:946a27210553 | 633 | #if MX12_WRITE_DEBUG |
labmrd | 1:946a27210553 | 634 | printf(" Checksum : 0x%x\n",TxBuf[offset]); |
labmrd | 1:946a27210553 | 635 | #endif |
labmrd | 1:946a27210553 | 636 | |
labmrd | 1:946a27210553 | 637 | // Clear in input buffer first |
labmrd | 1:946a27210553 | 638 | while (mx12_in.readable()) { |
labmrd | 1:946a27210553 | 639 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 640 | printf("Purging one character (read).\n"); |
labmrd | 1:946a27210553 | 641 | } |
labmrd | 1:946a27210553 | 642 | |
labmrd | 1:946a27210553 | 643 | // Transmit the packet in one burst with no pausing |
labmrd | 1:946a27210553 | 644 | offset=7+NumDevices*(DataLength+0x1)+0x1; // one more for the checksum |
labmrd | 1:946a27210553 | 645 | for (ii = 0; ii<offset ; ii++) { |
labmrd | 1:946a27210553 | 646 | mx12_out.putc(TxBuf[ii]); |
labmrd | 3:624d04c390b8 | 647 | mx12_in.getc(); |
labmrd | 3:624d04c390b8 | 648 | //printf("0x%x",TxBuf[ii]); |
labmrd | 1:946a27210553 | 649 | } |
labmrd | 1:946a27210553 | 650 | |
labmrd | 1:946a27210553 | 651 | // Read |
labmrd | 1:946a27210553 | 652 | for (ii = 0; ii<offset ; ii++) { |
labmrd | 3:624d04c390b8 | 653 | //profileOut=ii%2; |
labmrd | 1:946a27210553 | 654 | } |
labmrd | 1:946a27210553 | 655 | } |
labmrd | 1:946a27210553 | 656 | |
labmrd | 1:946a27210553 | 657 | void MX12::trigger(void) { |
labmrd | 1:946a27210553 | 658 | |
labmrd | 1:946a27210553 | 659 | char TxBuf[6]; |
labmrd | 1:946a27210553 | 660 | char sum = 0; |
labmrd | 1:946a27210553 | 661 | |
labmrd | 1:946a27210553 | 662 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 663 | printf("\nTriggered\n"); |
labmrd | 1:946a27210553 | 664 | #endif |
labmrd | 1:946a27210553 | 665 | |
labmrd | 1:946a27210553 | 666 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 667 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 668 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
labmrd | 1:946a27210553 | 669 | #endif |
labmrd | 1:946a27210553 | 670 | |
labmrd | 1:946a27210553 | 671 | TxBuf[0] = 0xFF; |
labmrd | 1:946a27210553 | 672 | TxBuf[1] = 0xFF; |
labmrd | 1:946a27210553 | 673 | |
labmrd | 1:946a27210553 | 674 | // ID - Broadcast |
labmrd | 1:946a27210553 | 675 | TxBuf[2] = 0xFE; |
labmrd | 1:946a27210553 | 676 | sum += TxBuf[2]; |
labmrd | 1:946a27210553 | 677 | |
labmrd | 1:946a27210553 | 678 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 679 | printf(" ID : %d\n",TxBuf[2]); |
labmrd | 1:946a27210553 | 680 | #endif |
labmrd | 1:946a27210553 | 681 | |
labmrd | 1:946a27210553 | 682 | // Length |
labmrd | 1:946a27210553 | 683 | TxBuf[3] = 0x02; |
labmrd | 1:946a27210553 | 684 | sum += TxBuf[3]; |
labmrd | 1:946a27210553 | 685 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 686 | printf(" Length %d\n",TxBuf[3]); |
labmrd | 1:946a27210553 | 687 | #endif |
labmrd | 1:946a27210553 | 688 | |
labmrd | 1:946a27210553 | 689 | // Instruction - ACTION |
labmrd | 1:946a27210553 | 690 | TxBuf[4] = 0x05; |
labmrd | 1:946a27210553 | 691 | sum += TxBuf[4]; |
labmrd | 1:946a27210553 | 692 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 693 | printf(" Instruction 0x%X\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 694 | #endif |
labmrd | 1:946a27210553 | 695 | |
labmrd | 1:946a27210553 | 696 | // Checksum |
labmrd | 1:946a27210553 | 697 | TxBuf[5] = 0xFF - sum; |
labmrd | 1:946a27210553 | 698 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 699 | printf(" Checksum 0x%X\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 700 | #endif |
labmrd | 1:946a27210553 | 701 | |
labmrd | 1:946a27210553 | 702 | // Transmit the packet in one burst with no pausing |
labmrd | 1:946a27210553 | 703 | for (int i = 0; i < 6 ; i++) { |
labmrd | 1:946a27210553 | 704 | mx12_out.putc(TxBuf[i]); |
labmrd | 1:946a27210553 | 705 | } |
labmrd | 1:946a27210553 | 706 | |
labmrd | 1:946a27210553 | 707 | // Read |
labmrd | 1:946a27210553 | 708 | for (int i = 0; i < 6 ; i++) { |
labmrd | 3:624d04c390b8 | 709 | //profileOut=i%2; |
labmrd | 1:946a27210553 | 710 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 711 | } |
labmrd | 1:946a27210553 | 712 | |
labmrd | 1:946a27210553 | 713 | // This is a broadcast packet, so there will be no reply |
labmrd | 1:946a27210553 | 714 | |
labmrd | 1:946a27210553 | 715 | return; |
labmrd | 1:946a27210553 | 716 | } |
labmrd | 1:946a27210553 | 717 | |
labmrd | 1:946a27210553 | 718 | bool MX12::ping(char ID_Num) { |
labmrd | 1:946a27210553 | 719 | if(ID_Num==0xFF){ |
labmrd | 1:946a27210553 | 720 | // Default to _ID |
labmrd | 1:946a27210553 | 721 | ID_Num=_ID; |
labmrd | 1:946a27210553 | 722 | } |
labmrd | 1:946a27210553 | 723 | |
labmrd | 1:946a27210553 | 724 | char TxBuf[6]; |
labmrd | 1:946a27210553 | 725 | char sum = 0; |
labmrd | 1:946a27210553 | 726 | char Status[6]; |
labmrd | 1:946a27210553 | 727 | |
labmrd | 1:946a27210553 | 728 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 729 | printf("\nTriggered\n"); |
labmrd | 1:946a27210553 | 730 | #endif |
labmrd | 1:946a27210553 | 731 | |
labmrd | 1:946a27210553 | 732 | // Build the TxPacket first in RAM, then we'll send in one go |
labmrd | 1:946a27210553 | 733 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 734 | printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); |
labmrd | 1:946a27210553 | 735 | #endif |
labmrd | 1:946a27210553 | 736 | |
labmrd | 1:946a27210553 | 737 | TxBuf[0] = 0xFF; |
labmrd | 1:946a27210553 | 738 | TxBuf[1] = 0xFF; |
labmrd | 1:946a27210553 | 739 | |
labmrd | 1:946a27210553 | 740 | // ID |
labmrd | 1:946a27210553 | 741 | TxBuf[2] = ID_Num; |
labmrd | 1:946a27210553 | 742 | sum += TxBuf[2]; |
labmrd | 1:946a27210553 | 743 | |
labmrd | 1:946a27210553 | 744 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 745 | printf(" ID : %d\n",TxBuf[2]); |
labmrd | 1:946a27210553 | 746 | #endif |
labmrd | 1:946a27210553 | 747 | |
labmrd | 1:946a27210553 | 748 | // Length |
labmrd | 1:946a27210553 | 749 | TxBuf[3] = 0x02; |
labmrd | 1:946a27210553 | 750 | sum += TxBuf[3]; |
labmrd | 1:946a27210553 | 751 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 752 | printf(" Length %d\n",TxBuf[3]); |
labmrd | 1:946a27210553 | 753 | #endif |
labmrd | 1:946a27210553 | 754 | |
labmrd | 1:946a27210553 | 755 | // Instruction - PING |
labmrd | 1:946a27210553 | 756 | TxBuf[4] = 0x01; |
labmrd | 1:946a27210553 | 757 | sum += TxBuf[4]; |
labmrd | 1:946a27210553 | 758 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 759 | printf(" Instruction 0x%X\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 760 | #endif |
labmrd | 1:946a27210553 | 761 | |
labmrd | 1:946a27210553 | 762 | // Checksum |
labmrd | 1:946a27210553 | 763 | TxBuf[5] = 0xFF - sum; |
labmrd | 1:946a27210553 | 764 | #if MX12_TRIGGER_DEBUG |
labmrd | 1:946a27210553 | 765 | printf(" Checksum 0x%X\n",TxBuf[5]); |
labmrd | 1:946a27210553 | 766 | #endif |
labmrd | 1:946a27210553 | 767 | |
labmrd | 1:946a27210553 | 768 | // Transmit the packet in one burst with no pausing |
labmrd | 1:946a27210553 | 769 | for (int i = 0; i < 6 ; i++) { |
labmrd | 1:946a27210553 | 770 | mx12_out.putc(TxBuf[i]); |
labmrd | 1:946a27210553 | 771 | } |
labmrd | 1:946a27210553 | 772 | |
labmrd | 1:946a27210553 | 773 | // Read |
labmrd | 1:946a27210553 | 774 | for (int i = 0; i < 6 ; i++) { |
labmrd | 3:624d04c390b8 | 775 | //profileOut=i%2; |
labmrd | 1:946a27210553 | 776 | mx12_in.getc(); |
labmrd | 1:946a27210553 | 777 | } |
labmrd | 1:946a27210553 | 778 | |
labmrd | 1:946a27210553 | 779 | |
labmrd | 1:946a27210553 | 780 | // response is always 6 bytes |
labmrd | 1:946a27210553 | 781 | // 0xFF, 0xFF, ID, Length, Error, Checksum |
labmrd | 1:946a27210553 | 782 | Timer serial_timeout; |
labmrd | 1:946a27210553 | 783 | serial_timeout.start (); |
labmrd | 1:946a27210553 | 784 | |
labmrd | 1:946a27210553 | 785 | for (int i=0; i < 6 ; i++) { |
labmrd | 1:946a27210553 | 786 | serial_timeout.reset (); |
labmrd | 1:946a27210553 | 787 | while (!mx12_in.readable ()) |
labmrd | 1:946a27210553 | 788 | { |
labmrd | 1:946a27210553 | 789 | //Just loop here until you either get a character, or we timeout |
labmrd | 1:946a27210553 | 790 | if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US) |
labmrd | 1:946a27210553 | 791 | { |
labmrd | 1:946a27210553 | 792 | //If we timeout, quit in a panic! |
labmrd | 1:946a27210553 | 793 | //printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i); |
labmrd | 1:946a27210553 | 794 | return false; |
labmrd | 1:946a27210553 | 795 | } |
labmrd | 1:946a27210553 | 796 | } |
labmrd | 1:946a27210553 | 797 | Status[i] = mx12_in.getc(); |
labmrd | 1:946a27210553 | 798 | } |
labmrd | 1:946a27210553 | 799 | |
labmrd | 1:946a27210553 | 800 | printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); |
labmrd | 1:946a27210553 | 801 | printf(" ID : %d\n",Status[2]); |
labmrd | 1:946a27210553 | 802 | printf(" Length : %d\n",Status[3]); |
labmrd | 1:946a27210553 | 803 | printf(" Error : 0x%x\n",Status[4]); |
labmrd | 1:946a27210553 | 804 | printf(" Checksum : 0x%x\n",Status[5]); |
labmrd | 1:946a27210553 | 805 | |
labmrd | 1:946a27210553 | 806 | return true; |
labmrd | 1:946a27210553 | 807 | } |