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@1:946a27210553, 2015-01-26 (annotated)

Committer:
labmrd
Date:
Mon Jan 26 04:02:37 2015 +0000
Revision:
1:946a27210553
Parent:
0:29900c3a4a50
Child:
3:624d04c390b8
Re-made to include all possible Object Dictionary entries

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