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@3:624d04c390b8, 2015-01-29 (annotated)

Committer:
labmrd
Date:
Thu Jan 29 22:23:20 2015 +0000
Revision:
3:624d04c390b8
Parent:
1:946a27210553
Child:
4:6e320b7646ff
More error checking

Who changed what in which revision?

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