MRD Lab / MX12

Robotis Dynamixel MX-12W Servo Library

Dependents:   SpindleBot_1_5b Utilisatio_MX12_V4

/media/uploads/labmrd/mx12.jpg

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@4:6e320b7646ff, 2015-01-29 (annotated)

Committer:
labmrd
Date:
Thu Jan 29 22:24:13 2015 +0000
Revision:
4:6e320b7646ff
Parent:
3:624d04c390b8
Child:
5:4c118a827f11
More output on scan

Who changed what in which revision?

UserRevisionLine numberNew 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 4:6e320b7646ff 128 printf("baud rate=%d\n",baud_rate[ii]);
labmrd 1:946a27210553 129 ChangeUARTBaud(baud_rate[ii]);
labmrd 1:946a27210553 130 for(ID_Num=0;ID_Num<=max_id;ID_Num++){
labmrd 4:6e320b7646ff 131 printf("ID Num=%d\n",ID_Num);
labmrd 1:946a27210553 132 if(ping(ID_Num)){
labmrd 1:946a27210553 133 printf("Found a servo at ID=%#02x, Baud=%d\n",ID_Num,baud_rate[ii]);
labmrd 1:946a27210553 134 }
labmrd 1:946a27210553 135 }
labmrd 1:946a27210553 136 }
labmrd 1:946a27210553 137 }else{
labmrd 1:946a27210553 138 for(ID_Num=0;ID_Num<=max_id;ID_Num++){
labmrd 1:946a27210553 139 if(ping(ID_Num)){
labmrd 1:946a27210553 140 printf("Found a servo at ID=%#02x\n",ID_Num);
labmrd 1:946a27210553 141 }
labmrd 1:946a27210553 142 }
labmrd 1:946a27210553 143 }
labmrd 1:946a27210553 144
labmrd 1:946a27210553 145 }
labmrd 1:946a27210553 146
labmrd 1:946a27210553 147 int MX12::SetBaud( int target_baud ) {
labmrd 1:946a27210553 148
labmrd 1:946a27210553 149 short baud_int;
labmrd 1:946a27210553 150
labmrd 1:946a27210553 151 if(target_baud<=1000000){
labmrd 1:946a27210553 152 baud_int=2000000/target_baud-1;
labmrd 1:946a27210553 153 }else if(target_baud==2250000){
labmrd 1:946a27210553 154 baud_int=250;
labmrd 1:946a27210553 155 }else if(target_baud==2500000){
labmrd 1:946a27210553 156 baud_int=251;
labmrd 1:946a27210553 157 }else if(target_baud==3000000){
labmrd 1:946a27210553 158 baud_int=252;
labmrd 1:946a27210553 159 }else{
labmrd 1:946a27210553 160 printf("Error! Invalid baud rate of %d!\n",target_baud);
labmrd 1:946a27210553 161 return 0;
labmrd 1:946a27210553 162 }
labmrd 1:946a27210553 163
labmrd 1:946a27210553 164 #if MX12_DEBUG
labmrd 1:946a27210553 165 printf("SetBaudRate to 0x%x\n",baud_int);
labmrd 1:946a27210553 166 #endif
labmrd 1:946a27210553 167 int ret=(write_short(MX12_REG_BAUD_RATE,baud_int));
labmrd 1:946a27210553 168
labmrd 1:946a27210553 169 // Now change the UART serial rate to the same?
labmrd 1:946a27210553 170 // But to allow changing several servos, allow user
labmrd 1:946a27210553 171 // to do this at their liesure.
labmrd 1:946a27210553 172 // ChangeUARTBaud( target_baud );
labmrd 1:946a27210553 173
labmrd 1:946a27210553 174 return ret;
labmrd 1:946a27210553 175 }
labmrd 1:946a27210553 176
labmrd 1:946a27210553 177 void MX12::ChangeUARTBaud( int target_baud ) {
labmrd 1:946a27210553 178 mx12_out.baud(target_baud);
labmrd 1:946a27210553 179 mx12_in.baud(target_baud);
labmrd 1:946a27210553 180 }
labmrd 1:946a27210553 181
labmrd 1:946a27210553 182 short MX12::GetRawPosition(void) {
labmrd 1:946a27210553 183 #if MX12_DEBUG
labmrd 1:946a27210553 184 printf("\nGetRawPosition(%d)",_ID);
labmrd 1:946a27210553 185 #endif
labmrd 1:946a27210553 186 return read_short(MX12_REG_PRESENT_POSITION);
labmrd 1:946a27210553 187 }
labmrd 1:946a27210553 188
labmrd 1:946a27210553 189 int MX12:: write_short(MX12ODIndex OD,short value)
labmrd 1:946a27210553 190 {
labmrd 0:29900c3a4a50 191 char data[2];
labmrd 0:29900c3a4a50 192
labmrd 1:946a27210553 193 data[0] = value & 0xff; // bottom 8 bits
labmrd 1:946a27210553 194 data[1] = value >> 8; // top 8 bits
labmrd 0:29900c3a4a50 195
labmrd 0:29900c3a4a50 196 // write the packet, return the error code
labmrd 1:946a27210553 197 int rVal = write(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
labmrd 3:624d04c390b8 198 if(rVal>0){
labmrd 3:624d04c390b8 199 if(CHECK_BIT(rVal, 0)){printf("Error! Input Voltage Error\n");}
labmrd 3:624d04c390b8 200 if(CHECK_BIT(rVal, 1)){printf("Error! Angle Limit Error\n");}
labmrd 3:624d04c390b8 201 if(CHECK_BIT(rVal, 2)){printf("Error! Overheating Error\n");}
labmrd 3:624d04c390b8 202 if(CHECK_BIT(rVal, 3)){printf("Error! Range Error\n");}
labmrd 3:624d04c390b8 203 if(CHECK_BIT(rVal, 4)){printf("Error! Checksum Error\n");}
labmrd 3:624d04c390b8 204 if(CHECK_BIT(rVal, 5)){printf("Error! Overload Error\n");}
labmrd 3:624d04c390b8 205 if(CHECK_BIT(rVal, 6)){printf("Error! Instruction Error\n");}
labmrd 3:624d04c390b8 206 if(CHECK_BIT(rVal, 7)){printf("Error! Undefined Error\n");}
labmrd 3:624d04c390b8 207 }
labmrd 0:29900c3a4a50 208 return(rVal);
labmrd 0:29900c3a4a50 209 }
labmrd 0:29900c3a4a50 210
labmrd 1:946a27210553 211 short MX12:: read_short(MX12ODIndex OD)
labmrd 1:946a27210553 212 {
labmrd 0:29900c3a4a50 213
labmrd 0:29900c3a4a50 214 char data[2];
labmrd 1:946a27210553 215 data[1]=0;
labmrd 0:29900c3a4a50 216
labmrd 1:946a27210553 217 int ErrorCode = read(_ID, MX12_OD[OD].Address, MX12_OD[OD].Bytes, data);
labmrd 3:624d04c390b8 218 profileOut = (ErrorCode!=0);
labmrd 1:946a27210553 219 short value = data[0] + (data[1] << 8);
labmrd 1:946a27210553 220
labmrd 1:946a27210553 221 return (value);
labmrd 0:29900c3a4a50 222 }
labmrd 0:29900c3a4a50 223
labmrd 0:29900c3a4a50 224
labmrd 0:29900c3a4a50 225
labmrd 1:946a27210553 226 ////////////////////////////////////////////
labmrd 1:946a27210553 227 // //
labmrd 1:946a27210553 228 // //
labmrd 1:946a27210553 229 // PRIVATE FUNCTIONS //
labmrd 1:946a27210553 230 // //
labmrd 1:946a27210553 231 // //
labmrd 1:946a27210553 232 ////////////////////////////////////////////
labmrd 0:29900c3a4a50 233
labmrd 0:29900c3a4a50 234
labmrd 0:29900c3a4a50 235
labmrd 0:29900c3a4a50 236
labmrd 0:29900c3a4a50 237
labmrd 0:29900c3a4a50 238 int MX12::read(int ID, int start, int bytes, char* data) {
labmrd 1:946a27210553 239
labmrd 0:29900c3a4a50 240 char PacketLength = 0x4;
labmrd 0:29900c3a4a50 241 char TxBuf[16];
labmrd 0:29900c3a4a50 242 char sum = 0;
labmrd 0:29900c3a4a50 243 char Status[16];
labmrd 0:29900c3a4a50 244
labmrd 0:29900c3a4a50 245 Status[4] = 0xFE; // return code
labmrd 0:29900c3a4a50 246
labmrd 1:946a27210553 247 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 248 printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
labmrd 1:946a27210553 249 #endif
labmrd 0:29900c3a4a50 250
labmrd 0:29900c3a4a50 251 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 252 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 253 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
labmrd 1:946a27210553 254 #endif
labmrd 0:29900c3a4a50 255
labmrd 0:29900c3a4a50 256 TxBuf[0] = 0xff;
labmrd 0:29900c3a4a50 257 TxBuf[1] = 0xff;
labmrd 0:29900c3a4a50 258
labmrd 0:29900c3a4a50 259 // ID
labmrd 0:29900c3a4a50 260 TxBuf[2] = ID;
labmrd 0:29900c3a4a50 261 sum += TxBuf[2];
labmrd 1:946a27210553 262 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 263 printf(" ID : %d\n",TxBuf[2]);
labmrd 1:946a27210553 264 #endif
labmrd 0:29900c3a4a50 265
labmrd 0:29900c3a4a50 266 // Packet Length
labmrd 0:29900c3a4a50 267 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
labmrd 0:29900c3a4a50 268 sum += TxBuf[3]; // Accululate the packet sum
labmrd 1:946a27210553 269 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 270 printf(" Length : 0x%x\n",TxBuf[3]);
labmrd 1:946a27210553 271 #endif
labmrd 0:29900c3a4a50 272
labmrd 0:29900c3a4a50 273 // Instruction - Read
labmrd 0:29900c3a4a50 274 TxBuf[4] = 0x2;
labmrd 0:29900c3a4a50 275 sum += TxBuf[4];
labmrd 1:946a27210553 276 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 277 printf(" Instruction : 0x%x\n",TxBuf[4]);
labmrd 1:946a27210553 278 #endif
labmrd 0:29900c3a4a50 279
labmrd 0:29900c3a4a50 280 // Start Address
labmrd 0:29900c3a4a50 281 TxBuf[5] = start;
labmrd 0:29900c3a4a50 282 sum += TxBuf[5];
labmrd 1:946a27210553 283 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 284 printf(" Start Address : 0x%x\n",TxBuf[5]);
labmrd 1:946a27210553 285 #endif
labmrd 0:29900c3a4a50 286
labmrd 0:29900c3a4a50 287 // Bytes to read
labmrd 0:29900c3a4a50 288 TxBuf[6] = bytes;
labmrd 0:29900c3a4a50 289 sum += TxBuf[6];
labmrd 1:946a27210553 290 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 291 printf(" No bytes : 0x%x\n",TxBuf[6]);
labmrd 1:946a27210553 292 #endif
labmrd 0:29900c3a4a50 293
labmrd 0:29900c3a4a50 294 // Checksum
labmrd 0:29900c3a4a50 295 TxBuf[7] = 0xFF - sum;
labmrd 1:946a27210553 296 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 297 printf(" Checksum : 0x%x\n",TxBuf[7]);
labmrd 1:946a27210553 298 #endif
labmrd 1:946a27210553 299
labmrd 1:946a27210553 300 // Clear in input buffer first
labmrd 1:946a27210553 301 while (mx12_in.readable()) {
labmrd 1:946a27210553 302 mx12_in.getc();
labmrd 1:946a27210553 303 printf("Purging one character (read).\n");
labmrd 0:29900c3a4a50 304 }
labmrd 1:946a27210553 305
labmrd 0:29900c3a4a50 306 // Transmit the packet in one burst with no pausing
labmrd 0:29900c3a4a50 307 for (int i = 0; i<8 ; i++) {
labmrd 1:946a27210553 308 mx12_out.putc(TxBuf[i]);
labmrd 0:29900c3a4a50 309 }
labmrd 1:946a27210553 310
labmrd 1:946a27210553 311 // Read
labmrd 1:946a27210553 312 for (int i = 0; i<8 ; i++) {
labmrd 3:624d04c390b8 313 //profileOut=i%2;
labmrd 1:946a27210553 314 mx12_in.getc();
labmrd 1:946a27210553 315 }
labmrd 1:946a27210553 316
labmrd 0:29900c3a4a50 317 // Wait for the bytes to be transmitted
labmrd 0:29900c3a4a50 318 //wait (0.00002);
labmrd 0:29900c3a4a50 319
labmrd 0:29900c3a4a50 320 // Skip if the read was to the broadcast address
labmrd 0:29900c3a4a50 321 if (_ID != 0xFE) {
labmrd 0:29900c3a4a50 322
labmrd 0:29900c3a4a50 323 // Receive the Status packet 6+ number of bytes read
labmrd 1:946a27210553 324 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 325 printf(" Reading Byte:");
labmrd 1:946a27210553 326 #endif
labmrd 0:29900c3a4a50 327 Timer serial_timeout;
labmrd 1:946a27210553 328 serial_timeout.start ();
labmrd 1:946a27210553 329
labmrd 0:29900c3a4a50 330 for (int i=0; i<(6+bytes) ; i++) {
labmrd 0:29900c3a4a50 331 serial_timeout.reset ();
labmrd 1:946a27210553 332 while (!mx12_in.readable ())
labmrd 0:29900c3a4a50 333 {
labmrd 0:29900c3a4a50 334 //Just loop here until you either get a character, or we timeout
labmrd 1:946a27210553 335 if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
labmrd 0:29900c3a4a50 336 {
labmrd 0:29900c3a4a50 337 //If we timeout, quit in a panic!
labmrd 3:624d04c390b8 338 printf("Error! Serial Timeout %d\n",i);
labmrd 0:29900c3a4a50 339 return(0x00);
labmrd 0:29900c3a4a50 340 }
labmrd 0:29900c3a4a50 341 }
labmrd 1:946a27210553 342 Status[i] = mx12_in.getc();
labmrd 0:29900c3a4a50 343 }
labmrd 1:946a27210553 344
labmrd 1:946a27210553 345 if(Status[0]!=0xFF || Status[1]!=0xFF)
labmrd 1:946a27210553 346 {
labmrd 3:624d04c390b8 347 printf("Header Error!\n");
labmrd 3:624d04c390b8 348 //printf("Unexpected header in serial response!\n");
labmrd 3:624d04c390b8 349 //printf(" Header : 0x%x\n",Status[0]);
labmrd 3:624d04c390b8 350 //printf(" Header : 0x%x\n",Status[1]);
labmrd 3:624d04c390b8 351 return 255;
labmrd 3:624d04c390b8 352 }
labmrd 3:624d04c390b8 353 if(Status[2]!=_ID){
labmrd 3:624d04c390b8 354 printf("ID Error!\n");
labmrd 3:624d04c390b8 355 return 255;
labmrd 3:624d04c390b8 356 }
labmrd 3:624d04c390b8 357 if(Status[3]!=bytes+2){
labmrd 3:624d04c390b8 358 printf("Length Error!\n");
labmrd 3:624d04c390b8 359 return 255;
labmrd 3:624d04c390b8 360 }
labmrd 3:624d04c390b8 361
labmrd 3:624d04c390b8 362 char sum=Status[2]+Status[3]+Status[4]+Status[5];
labmrd 3:624d04c390b8 363 if( bytes==2 )
labmrd 3:624d04c390b8 364 sum+=Status[6];
labmrd 3:624d04c390b8 365 if( Status[5+bytes]!=0xff-sum){
labmrd 3:624d04c390b8 366 printf("Checksum Error!\n");
labmrd 3:624d04c390b8 367 return 255;
labmrd 0:29900c3a4a50 368 }
labmrd 0:29900c3a4a50 369
labmrd 0:29900c3a4a50 370 // Copy the data from Status into data for return
labmrd 3:624d04c390b8 371 for (int i=0; i < bytes ; i++) {
labmrd 0:29900c3a4a50 372 data[i] = Status[5+i];
labmrd 0:29900c3a4a50 373 }
labmrd 0:29900c3a4a50 374
labmrd 1:946a27210553 375 #if MX12_READ_DEBUG
labmrd 0:29900c3a4a50 376 printf("\nStatus Packet\n");
labmrd 0:29900c3a4a50 377 printf(" Header : 0x%x\n",Status[0]);
labmrd 0:29900c3a4a50 378 printf(" Header : 0x%x\n",Status[1]);
labmrd 0:29900c3a4a50 379 printf(" ID : 0x%x\n",Status[2]);
labmrd 0:29900c3a4a50 380 printf(" Length : 0x%x\n",Status[3]);
labmrd 0:29900c3a4a50 381 printf(" Error Code : 0x%x\n",Status[4]);
labmrd 0:29900c3a4a50 382
labmrd 3:624d04c390b8 383 for (int i=0; i < bytes ; i++) {
labmrd 0:29900c3a4a50 384 printf(" Data : 0x%x\n",Status[5+i]);
labmrd 0:29900c3a4a50 385 }
labmrd 0:29900c3a4a50 386
labmrd 3:624d04c390b8 387 printf(" Checksum : 0x%x\n",Status[5+bytes]);
labmrd 1:946a27210553 388 #endif
labmrd 0:29900c3a4a50 389
labmrd 0:29900c3a4a50 390 } // if (ID!=0xFE)
labmrd 0:29900c3a4a50 391
labmrd 0:29900c3a4a50 392 return(Status[4]);
labmrd 0:29900c3a4a50 393 }
labmrd 0:29900c3a4a50 394
labmrd 0:29900c3a4a50 395
labmrd 1:946a27210553 396
labmrd 1:946a27210553 397
labmrd 1:946a27210553 398 int MX12:: write(int ID, int start, int bytes, char* data) {
labmrd 0:29900c3a4a50 399 // 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
labmrd 0:29900c3a4a50 400
labmrd 0:29900c3a4a50 401 char TxBuf[16];
labmrd 0:29900c3a4a50 402 char sum = 0;
labmrd 0:29900c3a4a50 403 char Status[6];
labmrd 0:29900c3a4a50 404
labmrd 1:946a27210553 405 #if MX12_WRITE_DEBUG
labmrd 3:624d04c390b8 406 printf("\nwrite(0x%02x,0x%02x,0x%02x)\n",ID,start,bytes);
labmrd 1:946a27210553 407 #endif
labmrd 0:29900c3a4a50 408
labmrd 0:29900c3a4a50 409 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 410 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 411 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
labmrd 1:946a27210553 412 #endif
labmrd 0:29900c3a4a50 413
labmrd 1:946a27210553 414 TxBuf[0] = MX12_INSTRUCTION_HEADER;
labmrd 1:946a27210553 415 TxBuf[1] = MX12_INSTRUCTION_HEADER;
labmrd 0:29900c3a4a50 416
labmrd 0:29900c3a4a50 417 // ID
labmrd 0:29900c3a4a50 418 TxBuf[2] = ID;
labmrd 0:29900c3a4a50 419 sum += TxBuf[2];
labmrd 0:29900c3a4a50 420
labmrd 1:946a27210553 421 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 422 printf(" ID : %d\n",TxBuf[2]);
labmrd 1:946a27210553 423 #endif
labmrd 0:29900c3a4a50 424
labmrd 0:29900c3a4a50 425 // packet Length
labmrd 0:29900c3a4a50 426 TxBuf[3] = 3+bytes;
labmrd 0:29900c3a4a50 427 sum += TxBuf[3];
labmrd 0:29900c3a4a50 428
labmrd 1:946a27210553 429 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 430 printf(" Length : %d\n",TxBuf[3]);
labmrd 1:946a27210553 431 #endif
labmrd 0:29900c3a4a50 432
labmrd 0:29900c3a4a50 433 // Instruction
labmrd 1:946a27210553 434 // if (flag == 1) {
labmrd 1:946a27210553 435 // TxBuf[4]=0x04;
labmrd 1:946a27210553 436 // } else {
labmrd 0:29900c3a4a50 437 TxBuf[4]=0x03;
labmrd 1:946a27210553 438 // }
labmrd 1:946a27210553 439 sum += TxBuf[4];
labmrd 0:29900c3a4a50 440
labmrd 1:946a27210553 441 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 442 printf(" Instruction : 0x%x\n",TxBuf[4]);
labmrd 1:946a27210553 443 #endif
labmrd 0:29900c3a4a50 444
labmrd 0:29900c3a4a50 445 // Start Address
labmrd 0:29900c3a4a50 446 TxBuf[5] = start;
labmrd 0:29900c3a4a50 447 sum += TxBuf[5];
labmrd 1:946a27210553 448 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 449 printf(" Start : 0x%x\n",TxBuf[5]);
labmrd 1:946a27210553 450 #endif
labmrd 0:29900c3a4a50 451
labmrd 0:29900c3a4a50 452 // data
labmrd 0:29900c3a4a50 453 for (char i=0; i<bytes ; i++) {
labmrd 0:29900c3a4a50 454 TxBuf[6+i] = data[i];
labmrd 0:29900c3a4a50 455 sum += TxBuf[6+i];
labmrd 1:946a27210553 456 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 457 printf(" Data : 0x%x\n",TxBuf[6+i]);
labmrd 1:946a27210553 458 #endif
labmrd 0:29900c3a4a50 459 }
labmrd 0:29900c3a4a50 460
labmrd 0:29900c3a4a50 461 // checksum
labmrd 0:29900c3a4a50 462 TxBuf[6+bytes] = 0xFF - sum;
labmrd 1:946a27210553 463 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 464 printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
labmrd 1:946a27210553 465 #endif
labmrd 1:946a27210553 466
labmrd 1:946a27210553 467 // Clear in input buffer first
labmrd 1:946a27210553 468 while (mx12_in.readable()) {
labmrd 1:946a27210553 469 mx12_in.getc();
labmrd 1:946a27210553 470 printf("Purging one character (write).\n");
labmrd 0:29900c3a4a50 471 }
labmrd 0:29900c3a4a50 472
labmrd 0:29900c3a4a50 473 // Transmit the packet in one burst with no pausing
labmrd 0:29900c3a4a50 474 for (int i = 0; i < (7 + bytes) ; i++) {
labmrd 1:946a27210553 475 mx12_out.putc(TxBuf[i]);
labmrd 1:946a27210553 476 mx12_in.getc();
labmrd 1:946a27210553 477 //printf("Echo: 0x%02x\n",mx12_in.getc());
labmrd 0:29900c3a4a50 478 }
labmrd 0:29900c3a4a50 479
labmrd 0:29900c3a4a50 480 // make sure we have a valid return
labmrd 0:29900c3a4a50 481 Status[4]=0x00;
labmrd 0:29900c3a4a50 482
labmrd 0:29900c3a4a50 483 // we'll only get a reply if it was not broadcast
labmrd 0:29900c3a4a50 484 if (_ID!=0xFE) {
labmrd 0:29900c3a4a50 485
labmrd 0:29900c3a4a50 486 // response is always 6 bytes
labmrd 1:946a27210553 487 // 0xFF, 0xFF, ID, Length, Error, Checksum
labmrd 0:29900c3a4a50 488 Timer serial_timeout;
labmrd 1:946a27210553 489 serial_timeout.start ();
labmrd 1:946a27210553 490
labmrd 0:29900c3a4a50 491 for (int i=0; i < 6 ; i++) {
labmrd 0:29900c3a4a50 492 serial_timeout.reset ();
labmrd 1:946a27210553 493 while (!mx12_in.readable ())
labmrd 0:29900c3a4a50 494 {
labmrd 0:29900c3a4a50 495 //Just loop here until you either get a character, or we timeout
labmrd 1:946a27210553 496 if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
labmrd 0:29900c3a4a50 497 {
labmrd 0:29900c3a4a50 498 //If we timeout, quit in a panic!
labmrd 3:624d04c390b8 499 printf("Error! Serial Timeout %d\n",i);
labmrd 0:29900c3a4a50 500 return(0x00);
labmrd 0:29900c3a4a50 501 }
labmrd 0:29900c3a4a50 502 }
labmrd 1:946a27210553 503 Status[i] = mx12_in.getc();
labmrd 0:29900c3a4a50 504 }
labmrd 1:946a27210553 505
labmrd 3:624d04c390b8 506 if(Status[0]!=0xFF || Status[1]!=0xFF)
labmrd 3:624d04c390b8 507 {
labmrd 3:624d04c390b8 508 printf("Unexpected header in serial response!\n");
labmrd 3:624d04c390b8 509 printf(" Header : 0x%x\n",Status[0]);
labmrd 3:624d04c390b8 510 printf(" Header : 0x%x\n",Status[1]);
labmrd 3:624d04c390b8 511 return 255;
labmrd 3:624d04c390b8 512 }
labmrd 3:624d04c390b8 513 if( Status[5]!=0xff-(Status[2]+Status[3]+Status[4])){
labmrd 3:624d04c390b8 514 printf("Checksum Error!\n");
labmrd 3:624d04c390b8 515 return 255;
labmrd 3:624d04c390b8 516 }
labmrd 3:624d04c390b8 517
labmrd 3:624d04c390b8 518
labmrd 0:29900c3a4a50 519 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 520 #if MX12_WRITE_DEBUG
labmrd 0:29900c3a4a50 521 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
labmrd 0:29900c3a4a50 522 printf(" ID : %d\n",Status[2]);
labmrd 0:29900c3a4a50 523 printf(" Length : %d\n",Status[3]);
labmrd 0:29900c3a4a50 524 printf(" Error : 0x%x\n",Status[4]);
labmrd 0:29900c3a4a50 525 printf(" Checksum : 0x%x\n",Status[5]);
labmrd 1:946a27210553 526 #endif
labmrd 0:29900c3a4a50 527
labmrd 0:29900c3a4a50 528
labmrd 0:29900c3a4a50 529 }
labmrd 0:29900c3a4a50 530
labmrd 0:29900c3a4a50 531 return(Status[4]); // return error code
labmrd 0:29900c3a4a50 532 }
labmrd 1:946a27210553 533
labmrd 1:946a27210553 534 void MX12::coordinated_move(char id0, short pos0, short vel0, char id1, short pos1, short vel1)
labmrd 1:946a27210553 535 {
labmrd 1:946a27210553 536 char NumDevices = 0x2;
labmrd 1:946a27210553 537 char DataLength = 0x4;//!< Hardcoded for now, 2 bytes for pos, 2 bytes for vel
labmrd 1:946a27210553 538 char PacketLength = 0x4+NumDevices*(DataLength+0x1);
labmrd 1:946a27210553 539 char StartAddress = MX12_OD[MX12_REG_GOAL_POSITION].Address;
labmrd 1:946a27210553 540 char TxBuf[20];
labmrd 1:946a27210553 541 char sum = 0;
labmrd 1:946a27210553 542 char ii=0;
labmrd 1:946a27210553 543 char jj=0;
labmrd 1:946a27210553 544 char offset=0;
labmrd 1:946a27210553 545
labmrd 1:946a27210553 546 // Hardcoded for now, 2 devices
labmrd 1:946a27210553 547 char ID_Num[NumDevices];
labmrd 1:946a27210553 548 ID_Num[0]=id0;
labmrd 1:946a27210553 549 ID_Num[1]=id1;
labmrd 1:946a27210553 550
labmrd 1:946a27210553 551 // Hardcoded for now, 2 bytes for pos, 2 bytes for vel, 2 devices
labmrd 1:946a27210553 552 char data[NumDevices][DataLength];
labmrd 1:946a27210553 553 // A faster/better way to do this would be with a fancy memcpy from
labmrd 1:946a27210553 554 // short to char or maybe a union or just raw pointers...
labmrd 1:946a27210553 555 data[0][0] = pos0 & 0xff; // bottom 8 bits
labmrd 1:946a27210553 556 data[0][1] = pos0 >> 8; // top 8 bits
labmrd 1:946a27210553 557 data[0][2] = vel0 & 0xff; // bottom 8 bits
labmrd 1:946a27210553 558 data[0][3] = vel0 >> 8; // top 8 bits
labmrd 1:946a27210553 559 data[1][0] = pos1 & 0xff; // bottom 8 bits
labmrd 1:946a27210553 560 data[1][1] = pos1 >> 8; // top 8 bits
labmrd 1:946a27210553 561 data[1][2] = vel1 & 0xff; // bottom 8 bits
labmrd 1:946a27210553 562 data[1][3] = vel1 >> 8; // top 8 bits
labmrd 1:946a27210553 563
labmrd 1:946a27210553 564 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 565 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 566 printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
labmrd 1:946a27210553 567 #endif
labmrd 1:946a27210553 568
labmrd 1:946a27210553 569 TxBuf[0] = 0xff;
labmrd 1:946a27210553 570 TxBuf[1] = 0xff;
labmrd 1:946a27210553 571
labmrd 1:946a27210553 572 // ID
labmrd 1:946a27210553 573 TxBuf[2] = 0xfe;
labmrd 1:946a27210553 574 sum += TxBuf[2];
labmrd 1:946a27210553 575 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 576 printf(" ID : %d\n",TxBuf[2]);
labmrd 1:946a27210553 577 #endif
labmrd 1:946a27210553 578
labmrd 1:946a27210553 579 // Packet Length
labmrd 1:946a27210553 580 TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
labmrd 1:946a27210553 581 sum += TxBuf[3]; // Accululate the packet sum
labmrd 1:946a27210553 582 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 583 printf(" Length : 0x%x\n",TxBuf[3]);
labmrd 1:946a27210553 584 #endif
labmrd 1:946a27210553 585
labmrd 1:946a27210553 586 // Instruction - Sync Write
labmrd 1:946a27210553 587 TxBuf[4] = 0x83;
labmrd 1:946a27210553 588 sum += TxBuf[4];
labmrd 1:946a27210553 589 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 590 printf(" Instruction : 0x%x\n",TxBuf[4]);
labmrd 1:946a27210553 591 #endif
labmrd 1:946a27210553 592
labmrd 1:946a27210553 593 // Start Address
labmrd 1:946a27210553 594 TxBuf[5] = StartAddress;
labmrd 1:946a27210553 595 sum += TxBuf[5];
labmrd 1:946a27210553 596 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 597 printf(" Start Address : 0x%x\n",TxBuf[5]);
labmrd 1:946a27210553 598 #endif
labmrd 1:946a27210553 599
labmrd 1:946a27210553 600 // Bytes to write to each device
labmrd 1:946a27210553 601 TxBuf[6] = DataLength;
labmrd 1:946a27210553 602 sum += TxBuf[6];
labmrd 1:946a27210553 603 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 604 printf(" No bytes : 0x%x\n",TxBuf[6]);
labmrd 1:946a27210553 605 #endif
labmrd 1:946a27210553 606
labmrd 1:946a27210553 607 // The data itself
labmrd 1:946a27210553 608 for(ii=0;ii<NumDevices;ii++){
labmrd 1:946a27210553 609 // Store this offset in a variable since we use it a lot.
labmrd 1:946a27210553 610 // The 7 comes from the fact that the last write was at 6,
labmrd 1:946a27210553 611 // so this one should start at 7. The 0x1 is for the ID.
labmrd 1:946a27210553 612 offset=7+ii*(DataLength+0x1);
labmrd 1:946a27210553 613 // Write the ID of the device
labmrd 1:946a27210553 614 TxBuf[offset] = ID_Num[ii];
labmrd 1:946a27210553 615 sum += TxBuf[offset];
labmrd 1:946a27210553 616
labmrd 1:946a27210553 617 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 618 printf(" ID #%d : 0x%x\n",ii,TxBuf[offset]);
labmrd 1:946a27210553 619 #endif
labmrd 1:946a27210553 620
labmrd 1:946a27210553 621 // Write each of the bytes of the data
labmrd 3:624d04c390b8 622 for(jj=0;jj<DataLength;jj++){
labmrd 3:624d04c390b8 623 TxBuf[offset+jj+0x1] = data[ii][jj];
labmrd 3:624d04c390b8 624 sum += TxBuf[offset+jj+0x1];
labmrd 1:946a27210553 625 #if MX12_WRITE_DEBUG
labmrd 3:624d04c390b8 626 printf(" Data #%d : 0x%x\n",jj,TxBuf[offset+jj+0x1]);
labmrd 1:946a27210553 627 #endif
labmrd 1:946a27210553 628 }
labmrd 1:946a27210553 629 }
labmrd 1:946a27210553 630
labmrd 1:946a27210553 631
labmrd 1:946a27210553 632 // Checksum
labmrd 1:946a27210553 633 offset=7+NumDevices*(DataLength+0x1);
labmrd 1:946a27210553 634 TxBuf[offset] = 0xFF - sum;
labmrd 1:946a27210553 635 #if MX12_WRITE_DEBUG
labmrd 1:946a27210553 636 printf(" Checksum : 0x%x\n",TxBuf[offset]);
labmrd 1:946a27210553 637 #endif
labmrd 1:946a27210553 638
labmrd 1:946a27210553 639 // Clear in input buffer first
labmrd 1:946a27210553 640 while (mx12_in.readable()) {
labmrd 1:946a27210553 641 mx12_in.getc();
labmrd 1:946a27210553 642 printf("Purging one character (read).\n");
labmrd 1:946a27210553 643 }
labmrd 1:946a27210553 644
labmrd 1:946a27210553 645 // Transmit the packet in one burst with no pausing
labmrd 1:946a27210553 646 offset=7+NumDevices*(DataLength+0x1)+0x1; // one more for the checksum
labmrd 1:946a27210553 647 for (ii = 0; ii<offset ; ii++) {
labmrd 1:946a27210553 648 mx12_out.putc(TxBuf[ii]);
labmrd 3:624d04c390b8 649 mx12_in.getc();
labmrd 3:624d04c390b8 650 //printf("0x%x",TxBuf[ii]);
labmrd 1:946a27210553 651 }
labmrd 1:946a27210553 652
labmrd 1:946a27210553 653 // Read
labmrd 1:946a27210553 654 for (ii = 0; ii<offset ; ii++) {
labmrd 3:624d04c390b8 655 //profileOut=ii%2;
labmrd 1:946a27210553 656 }
labmrd 1:946a27210553 657 }
labmrd 1:946a27210553 658
labmrd 1:946a27210553 659 void MX12::trigger(void) {
labmrd 1:946a27210553 660
labmrd 1:946a27210553 661 char TxBuf[6];
labmrd 1:946a27210553 662 char sum = 0;
labmrd 1:946a27210553 663
labmrd 1:946a27210553 664 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 665 printf("\nTriggered\n");
labmrd 1:946a27210553 666 #endif
labmrd 1:946a27210553 667
labmrd 1:946a27210553 668 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 669 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 670 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
labmrd 1:946a27210553 671 #endif
labmrd 1:946a27210553 672
labmrd 1:946a27210553 673 TxBuf[0] = 0xFF;
labmrd 1:946a27210553 674 TxBuf[1] = 0xFF;
labmrd 1:946a27210553 675
labmrd 1:946a27210553 676 // ID - Broadcast
labmrd 1:946a27210553 677 TxBuf[2] = 0xFE;
labmrd 1:946a27210553 678 sum += TxBuf[2];
labmrd 1:946a27210553 679
labmrd 1:946a27210553 680 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 681 printf(" ID : %d\n",TxBuf[2]);
labmrd 1:946a27210553 682 #endif
labmrd 1:946a27210553 683
labmrd 1:946a27210553 684 // Length
labmrd 1:946a27210553 685 TxBuf[3] = 0x02;
labmrd 1:946a27210553 686 sum += TxBuf[3];
labmrd 1:946a27210553 687 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 688 printf(" Length %d\n",TxBuf[3]);
labmrd 1:946a27210553 689 #endif
labmrd 1:946a27210553 690
labmrd 1:946a27210553 691 // Instruction - ACTION
labmrd 1:946a27210553 692 TxBuf[4] = 0x05;
labmrd 1:946a27210553 693 sum += TxBuf[4];
labmrd 1:946a27210553 694 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 695 printf(" Instruction 0x%X\n",TxBuf[5]);
labmrd 1:946a27210553 696 #endif
labmrd 1:946a27210553 697
labmrd 1:946a27210553 698 // Checksum
labmrd 1:946a27210553 699 TxBuf[5] = 0xFF - sum;
labmrd 1:946a27210553 700 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 701 printf(" Checksum 0x%X\n",TxBuf[5]);
labmrd 1:946a27210553 702 #endif
labmrd 1:946a27210553 703
labmrd 1:946a27210553 704 // Transmit the packet in one burst with no pausing
labmrd 1:946a27210553 705 for (int i = 0; i < 6 ; i++) {
labmrd 1:946a27210553 706 mx12_out.putc(TxBuf[i]);
labmrd 1:946a27210553 707 }
labmrd 1:946a27210553 708
labmrd 1:946a27210553 709 // Read
labmrd 1:946a27210553 710 for (int i = 0; i < 6 ; i++) {
labmrd 3:624d04c390b8 711 //profileOut=i%2;
labmrd 1:946a27210553 712 mx12_in.getc();
labmrd 1:946a27210553 713 }
labmrd 1:946a27210553 714
labmrd 1:946a27210553 715 // This is a broadcast packet, so there will be no reply
labmrd 1:946a27210553 716
labmrd 1:946a27210553 717 return;
labmrd 1:946a27210553 718 }
labmrd 1:946a27210553 719
labmrd 1:946a27210553 720 bool MX12::ping(char ID_Num) {
labmrd 1:946a27210553 721 if(ID_Num==0xFF){
labmrd 1:946a27210553 722 // Default to _ID
labmrd 1:946a27210553 723 ID_Num=_ID;
labmrd 1:946a27210553 724 }
labmrd 1:946a27210553 725
labmrd 1:946a27210553 726 char TxBuf[6];
labmrd 1:946a27210553 727 char sum = 0;
labmrd 1:946a27210553 728 char Status[6];
labmrd 1:946a27210553 729
labmrd 1:946a27210553 730 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 731 printf("\nTriggered\n");
labmrd 1:946a27210553 732 #endif
labmrd 1:946a27210553 733
labmrd 1:946a27210553 734 // Build the TxPacket first in RAM, then we'll send in one go
labmrd 1:946a27210553 735 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 736 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
labmrd 1:946a27210553 737 #endif
labmrd 1:946a27210553 738
labmrd 1:946a27210553 739 TxBuf[0] = 0xFF;
labmrd 1:946a27210553 740 TxBuf[1] = 0xFF;
labmrd 1:946a27210553 741
labmrd 1:946a27210553 742 // ID
labmrd 1:946a27210553 743 TxBuf[2] = ID_Num;
labmrd 1:946a27210553 744 sum += TxBuf[2];
labmrd 1:946a27210553 745
labmrd 1:946a27210553 746 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 747 printf(" ID : %d\n",TxBuf[2]);
labmrd 1:946a27210553 748 #endif
labmrd 1:946a27210553 749
labmrd 1:946a27210553 750 // Length
labmrd 1:946a27210553 751 TxBuf[3] = 0x02;
labmrd 1:946a27210553 752 sum += TxBuf[3];
labmrd 1:946a27210553 753 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 754 printf(" Length %d\n",TxBuf[3]);
labmrd 1:946a27210553 755 #endif
labmrd 1:946a27210553 756
labmrd 1:946a27210553 757 // Instruction - PING
labmrd 1:946a27210553 758 TxBuf[4] = 0x01;
labmrd 1:946a27210553 759 sum += TxBuf[4];
labmrd 1:946a27210553 760 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 761 printf(" Instruction 0x%X\n",TxBuf[5]);
labmrd 1:946a27210553 762 #endif
labmrd 1:946a27210553 763
labmrd 1:946a27210553 764 // Checksum
labmrd 1:946a27210553 765 TxBuf[5] = 0xFF - sum;
labmrd 1:946a27210553 766 #if MX12_TRIGGER_DEBUG
labmrd 1:946a27210553 767 printf(" Checksum 0x%X\n",TxBuf[5]);
labmrd 1:946a27210553 768 #endif
labmrd 1:946a27210553 769
labmrd 1:946a27210553 770 // Transmit the packet in one burst with no pausing
labmrd 1:946a27210553 771 for (int i = 0; i < 6 ; i++) {
labmrd 1:946a27210553 772 mx12_out.putc(TxBuf[i]);
labmrd 1:946a27210553 773 }
labmrd 1:946a27210553 774
labmrd 1:946a27210553 775 // Read
labmrd 1:946a27210553 776 for (int i = 0; i < 6 ; i++) {
labmrd 3:624d04c390b8 777 //profileOut=i%2;
labmrd 1:946a27210553 778 mx12_in.getc();
labmrd 1:946a27210553 779 }
labmrd 1:946a27210553 780
labmrd 1:946a27210553 781
labmrd 1:946a27210553 782 // response is always 6 bytes
labmrd 1:946a27210553 783 // 0xFF, 0xFF, ID, Length, Error, Checksum
labmrd 1:946a27210553 784 Timer serial_timeout;
labmrd 1:946a27210553 785 serial_timeout.start ();
labmrd 1:946a27210553 786
labmrd 1:946a27210553 787 for (int i=0; i < 6 ; i++) {
labmrd 1:946a27210553 788 serial_timeout.reset ();
labmrd 1:946a27210553 789 while (!mx12_in.readable ())
labmrd 1:946a27210553 790 {
labmrd 1:946a27210553 791 //Just loop here until you either get a character, or we timeout
labmrd 1:946a27210553 792 if (serial_timeout.read_us () > MAX_DELAY_BETWEEN_CHARCTERS_IN_US)
labmrd 1:946a27210553 793 {
labmrd 1:946a27210553 794 //If we timeout, quit in a panic!
labmrd 1:946a27210553 795 //printf("\nTimeout waiting for serial response!\nReceived %d characters.\n",i);
labmrd 1:946a27210553 796 return false;
labmrd 1:946a27210553 797 }
labmrd 1:946a27210553 798 }
labmrd 1:946a27210553 799 Status[i] = mx12_in.getc();
labmrd 1:946a27210553 800 }
labmrd 1:946a27210553 801
labmrd 1:946a27210553 802 printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]);
labmrd 1:946a27210553 803 printf(" ID : %d\n",Status[2]);
labmrd 1:946a27210553 804 printf(" Length : %d\n",Status[3]);
labmrd 1:946a27210553 805 printf(" Error : 0x%x\n",Status[4]);
labmrd 1:946a27210553 806 printf(" Checksum : 0x%x\n",Status[5]);
labmrd 1:946a27210553 807
labmrd 1:946a27210553 808 return true;
labmrd 1:946a27210553 809 }