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