Sim Youngwoo / Mbed 2 deprecated torque_calibration_ywsim

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed HX711 MotorModuleExample_copy

main.cpp@7:0408cbd898f5, 2021-02-12 (annotated)

Committer:
ywsim
Date:
Fri Feb 12 22:52:54 2021 +0000
Revision:
7:0408cbd898f5
Parent:
6:09738d84a005 
ygg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ywsim 6:09738d84a005 1 #include "HX711.h"
benkatz 2:36a254d3dbf3 2 #define CAN_ID 0x0
benkatz 0:d6186b8990c5 3 #include "mbed.h"
benkatz 0:d6186b8990c5 4 #include "math_ops.h"
benkatz 2:36a254d3dbf3 5 #include "MotorModule.h"
benkatz 2:36a254d3dbf3 6
benkatz 0:d6186b8990c5 7
ywsim 6:09738d84a005 8 typedef union _data {
ywsim 6:09738d84a005 9 float f;
ywsim 6:09738d84a005 10 char s[4];
ywsim 6:09738d84a005 11 } myDatafs;
benkatz 0:d6186b8990c5 12
ywsim 6:09738d84a005 13
ywsim 6:09738d84a005 14
ywsim 6:09738d84a005 15
ywsim 6:09738d84a005 16
ywsim 6:09738d84a005 17
ywsim 6:09738d84a005 18 HX711 loadCell( PB_10, PA_8 );
benkatz 2:36a254d3dbf3 19 Serial pc(PA_2, PA_3); // Serial port to the computer
benkatz 1:d24fd64d1fcb 20 CAN can(PB_8, PB_9, 1000000); // CAN Rx pin name, CAN Tx pin name
benkatz 0:d6186b8990c5 21
ywsim 6:09738d84a005 22 myDatafs dat1;
ywsim 6:09738d84a005 23 myDatafs dat2;
ywsim 6:09738d84a005 24 myDatafs dat3;
ywsim 6:09738d84a005 25 myDatafs dat4;
ywsim 6:09738d84a005 26
ywsim 6:09738d84a005 27 Ticker loop_ctrl; // Control loop interrupt handler
ywsim 6:09738d84a005 28 Ticker loop_msg;
ywsim 6:09738d84a005 29 Ticker newReading;
benkatz 0:d6186b8990c5 30
ywsim 6:09738d84a005 31 DigitalOut myled1(LED1);
ywsim 6:09738d84a005 32 DigitalOut myled2(PB_3);
ywsim 6:09738d84a005 33 DigitalOut myled3(PA_10);
ywsim 6:09738d84a005 34
ywsim 6:09738d84a005 35 HX711::HX711_status_t aux;
ywsim 6:09738d84a005 36 HX711::Vector_count_t myData;
ywsim 6:09738d84a005 37 HX711::Vector_mass_t calcMass;
ywsim 6:09738d84a005 38 HX711::Vector_voltage_t calcVolt;
ywsim 6:09738d84a005 39
ywsim 6:09738d84a005 40 #define DT_ctrl .002f // Control loop period
ywsim 6:09738d84a005 41 #define DT_msg .2f // msg loop period
ywsim 6:09738d84a005 42 #define N_MOTORS 1 // Number of motors on the can bus
ywsim 6:09738d84a005 43
benkatz 2:36a254d3dbf3 44 MotorStruct motors[N_MOTORS]; // Create a list of the motors attached
benkatz 2:36a254d3dbf3 45
benkatz 2:36a254d3dbf3 46 /* Communication functions. Do not touch */
benkatz 2:36a254d3dbf3 47 void onMsgReceived();
benkatz 2:36a254d3dbf3 48 void init_motors(int ids[N_MOTORS]);
benkatz 0:d6186b8990c5 49
ywsim 6:09738d84a005 50 int loop_counter = 0;
ywsim 6:09738d84a005 51 int newprint = 1;
ywsim 6:09738d84a005 52 int nskip = 5;
ywsim 6:09738d84a005 53 int skip_counter = 0;
benkatz 4:0ce97b9fde37 54
ywsim 6:09738d84a005 55 float t = 0.0;
ywsim 6:09738d84a005 56 float A= 0.5;
ywsim 6:09738d84a005 57 float f = 1.0;
ywsim 6:09738d84a005 58 float mycur = 0.1;
ywsim 6:09738d84a005 59 bool flagCtrl = true;
benkatz 4:0ce97b9fde37 60
ywsim 6:09738d84a005 61 void readDATA ( void )
ywsim 6:09738d84a005 62 {
ywsim 6:09738d84a005 63 myled2 = 1;
ywsim 6:09738d84a005 64
ywsim 6:09738d84a005 65 aux = loadCell.HX711_ReadRawData ( HX711::CHANNEL_A_GAIN_128, &myData, 1 );
ywsim 6:09738d84a005 66 calcMass = loadCell.HX711_CalculateMass ( &myData, 0.500, HX711::HX711_SCALE_g );
ywsim 6:09738d84a005 67 calcVolt = loadCell.HX711_CalculateVoltage ( &myData, 5.0 );
ywsim 6:09738d84a005 68
ywsim 6:09738d84a005 69 // pc.printf( "RawData: %ld Mass: %0.3f g Voltage: %0.5f mV\r\n", (uint32_t)myData.myRawValue, calcMass.myMass, 1000*calcVolt.myVoltage );
ywsim 6:09738d84a005 70
ywsim 6:09738d84a005 71 myled2 = 0;
ywsim 6:09738d84a005 72 }
ywsim 6:09738d84a005 73
ywsim 6:09738d84a005 74 void printnew(){
ywsim 6:09738d84a005 75 newprint = 1;
benkatz 0:d6186b8990c5 76 }
benkatz 0:d6186b8990c5 77
ywsim 6:09738d84a005 78 void control(){
ywsim 6:09738d84a005 79 t = DT_ctrl*loop_counter; //time in seconds
ywsim 6:09738d84a005 80 if (flagCtrl) {
ywsim 6:09738d84a005 81 // if control is ON
ywsim 6:09738d84a005 82 if (t<1) {
ywsim 6:09738d84a005 83 motors[0].control.p_des = 0;
ywsim 6:09738d84a005 84 motors[0].control.v_des = 0;
ywsim 6:09738d84a005 85 motors[0].control.kp = 0;
ywsim 6:09738d84a005 86 motors[0].control.kd = 0;
ywsim 6:09738d84a005 87 }
ywsim 6:09738d84a005 88 if (1<=t){
ywsim 6:09738d84a005 89 motors[0].control.i_ff = mycur;
ywsim 6:09738d84a005 90 motors[0].control.kd = 0;
ywsim 6:09738d84a005 91 motors[0].control.kp = 0;
ywsim 6:09738d84a005 92 }
ywsim 6:09738d84a005 93 for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
ywsim 6:09738d84a005 94 pack_cmd(&motors[i]);
ywsim 6:09738d84a005 95 can.write(motors[i].txMsg);
ywsim 6:09738d84a005 96 }
ywsim 6:09738d84a005 97 } else {
ywsim 6:09738d84a005 98 // if control is OFF
ywsim 6:09738d84a005 99 motors[0].control.i_ff = 0.0f; //all set to zero (no control)
ywsim 6:09738d84a005 100 motors[0].control.kp = 0.0f;
ywsim 6:09738d84a005 101 motors[0].control.kd = 0.0f;
ywsim 6:09738d84a005 102
ywsim 6:09738d84a005 103 loop_counter = 0.0f; //re-zero loop timing
ywsim 6:09738d84a005 104
ywsim 6:09738d84a005 105 for(int i = 0; i<N_MOTORS; i++) { //send msg to driver
ywsim 6:09738d84a005 106 pack_cmd(&motors[i]);
ywsim 6:09738d84a005 107 can.write(motors[i].txMsg);
ywsim 6:09738d84a005 108 }
ywsim 6:09738d84a005 109 }
ywsim 6:09738d84a005 110 loop_counter++; // increase loop counter
ywsim 6:09738d84a005 111 }
benkatz 0:d6186b8990c5 112
benkatz 2:36a254d3dbf3 113 int main()
benkatz 2:36a254d3dbf3 114 {
ywsim 6:09738d84a005 115 /* Setup & Initialization */
benkatz 2:36a254d3dbf3 116 pc.baud(921600); // Set baud rate for communication over USB serial
benkatz 2:36a254d3dbf3 117 can.attach(&onMsgReceived); // attach 'CAN receive-complete' interrupt handler
benkatz 2:36a254d3dbf3 118 can.filter(CAN_ID , 0xFFF, CANStandard, 0); // Set up can filter so it interrups only for messages with ID CAN_ID
ywsim 6:09738d84a005 119
ywsim 6:09738d84a005 120 myled1 = 0; myled2 = 0; myled3 = 0;
ywsim 6:09738d84a005 121 myled2 = 1; myled3 = 1; wait(0.5); myled2 = 0; myled3 = 0; wait(0.5);
ywsim 6:09738d84a005 122 myled2 = 1; myled3 = 1; wait(0.5); myled2 = 0; myled3 = 0; wait(0.5);
benkatz 2:36a254d3dbf3 123
ywsim 6:09738d84a005 124 //Reset and wake the device up
ywsim 6:09738d84a005 125 aux = loadCell.HX711_PowerDown();
ywsim 6:09738d84a005 126 aux = loadCell.HX711_Reset();
ywsim 6:09738d84a005 127
ywsim 6:09738d84a005 128 myled2 =1; wait(0.2); myled2 =0; wait(0.2); myled2 =1; wait(0.2); myled2 =0; wait(0.2);
ywsim 6:09738d84a005 129 myled2 =1; wait(0.2); myled2 =0; wait(0.2); myled2 =1; wait(0.2); myled2 =0; wait(0.2);
ywsim 6:09738d84a005 130
ywsim 6:09738d84a005 131 //CALIBRATION time start!
ywsim 6:09738d84a005 132 // 1. REMOVE THE MASS ON THE LOAD CELL ( ALL LEDs OFF ). Read data without any mass on the load cell
ywsim 6:09738d84a005 133 pc.printf("1. Self Mass Measurement beginning, remove all\r\n");
ywsim 6:09738d84a005 134 pc.printf("count 2\r\n");
ywsim 6:09738d84a005 135 wait(1);
ywsim 6:09738d84a005 136 pc.printf("count 1\r\n");
ywsim 6:09738d84a005 137 wait(1);
ywsim 6:09738d84a005 138 pc.printf("GO\r\n");
ywsim 6:09738d84a005 139
ywsim 6:09738d84a005 140 aux = loadCell.HX711_ReadData_WithoutMass( HX711::CHANNEL_A_GAIN_128, &myData, 20);
ywsim 6:09738d84a005 141
ywsim 6:09738d84a005 142 pc.printf("1. calib done.\r\n");
ywsim 6:09738d84a005 143 wait(1.5);
ywsim 6:09738d84a005 144 myled3 =1; wait(0.2); myled3 =0; wait(0.2); myled3 =1; wait(0.2); myled3 =0; wait(0.2);
ywsim 6:09738d84a005 145 myled3 =1; wait(0.2); myled3 =0; wait(0.2); myled3 =1; wait(0.2); myled3 =0; wait(0.2);
ywsim 6:09738d84a005 146
ywsim 6:09738d84a005 147 //2. PUT A KNOWN MASS ON THE LOAD CELL ( JUST LED1 ON ). Read data with an user-specified calibration mass
ywsim 6:09738d84a005 148 pc.printf("2. Put on a Calibrated Mass, Beginning, \r\n");
ywsim 6:09738d84a005 149 pc.printf("count 3\r\n");
ywsim 6:09738d84a005 150 wait(1);
ywsim 6:09738d84a005 151 pc.printf("count 2\r\n");
ywsim 6:09738d84a005 152 wait(1);
ywsim 6:09738d84a005 153 pc.printf("count 1\r\n");
ywsim 6:09738d84a005 154 wait(1);
ywsim 6:09738d84a005 155 pc.printf("GO\r\n");
ywsim 6:09738d84a005 156 wait(1);
ywsim 6:09738d84a005 157
ywsim 6:09738d84a005 158 aux = loadCell.HX711_ReadData_WithCalibratedMass ( HX711::CHANNEL_A_GAIN_128, &myData, 20 );
ywsim 6:09738d84a005 159
ywsim 6:09738d84a005 160 pc.printf("2. Calibration is done.\r\n");
ywsim 6:09738d84a005 161 wait(1);
ywsim 6:09738d84a005 162
ywsim 6:09738d84a005 163 // //OPTIONAL - TARING
ywsim 6:09738d84a005 164 // myled3 = 1;
ywsim 6:09738d84a005 165 // aux = loadCell.HX711_SetAutoTare ( HX711::CHANNEL_A_GAIN_128, 0.48, HX711::HX711_SCALE_kg, &myData, 5 );
ywsim 6:09738d84a005 166 // myled3 = 0;
ywsim 6:09738d84a005 167
ywsim 6:09738d84a005 168 myled1 = 0; myled2 = 0; myled3 = 0;
ywsim 6:09738d84a005 169 myled2 = 1; myled3 = 1; wait(0.5); myled2 = 0; myled3 = 0; wait(0.5);
ywsim 6:09738d84a005 170 myled2 = 1; myled3 = 1; wait(0.5); myled2 = 0; myled3 = 0; wait(0.5);
ywsim 6:09738d84a005 171
ywsim 6:09738d84a005 172 // All set up, and ready to go. Measure what ever you want
ywsim 6:09738d84a005 173 pc.printf("3. RT Measure start.\r\n");
ywsim 6:09738d84a005 174 pc.printf("count 3\r\n");
ywsim 6:09738d84a005 175 wait(1);
ywsim 6:09738d84a005 176 pc.printf("count 2\r\n");
ywsim 6:09738d84a005 177 wait(1);
ywsim 6:09738d84a005 178 pc.printf("count 1\r\n");
ywsim 6:09738d84a005 179 wait(1);
ywsim 6:09738d84a005 180 pc.printf("GO\r\n");
ywsim 6:09738d84a005 181
ywsim 6:09738d84a005 182 newReading.attach( &readDATA, DT_msg ); // the address of the function to be attached ( readDATA ) and the interval ( 0.5s ) ( JUST LED4 BLINKING )
ywsim 6:09738d84a005 183 pc.printf("LoadCell Interrupt Enabled\r\n");
ywsim 6:09738d84a005 184 //
ywsim 6:09738d84a005 185
ywsim 6:09738d84a005 186 int ids[N_MOTORS] = {1}; // List of motor CAN ID's
benkatz 2:36a254d3dbf3 187 init_motors(ids); // Initialize the list of motors
benkatz 0:d6186b8990c5 188
kfmurph2 5:a2e3d0213315 189 enable_motor(&motors[0], &can); // Enable motors
ywsim 6:09738d84a005 190 //enable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 191
kfmurph2 5:a2e3d0213315 192 zero_motor(&motors[0], &can); //Zero motors
ywsim 6:09738d84a005 193 //zero_motor(&motors[1], &can);
ywsim 6:09738d84a005 194 pc.printf("Motor enabled \r\n");
benkatz 4:0ce97b9fde37 195 wait(1); // Wait 1 second
kfmurph2 5:a2e3d0213315 196
benkatz 3:f0d054d896f9 197 //disable_motor(&motors[0], &can); // Disable first motor
benkatz 4:0ce97b9fde37 198 //disable_motor(&motors[1], &can);
kfmurph2 5:a2e3d0213315 199
ywsim 6:09738d84a005 200 /* Interrupt Enables */
ywsim 6:09738d84a005 201 loop_ctrl.attach(&control, DT_ctrl); // Start running the contorl interrupt at 1/DT Hz
ywsim 6:09738d84a005 202 loop_msg.attach(&printnew, DT_msg);
ywsim 6:09738d84a005 203
ywsim 6:09738d84a005 204 while(1) {
ywsim 6:09738d84a005 205 if (pc.readable()) {
ywsim 6:09738d84a005 206 char c = pc.getc();
ywsim 6:09738d84a005 207 if(c == 's') {
ywsim 6:09738d84a005 208 flagCtrl = false;
ywsim 6:09738d84a005 209 printf("s");
ywsim 6:09738d84a005 210 }
ywsim 6:09738d84a005 211 if(c == 'g') {
ywsim 6:09738d84a005 212 flagCtrl = true;
ywsim 6:09738d84a005 213 printf("g");
ywsim 6:09738d84a005 214 }
ywsim 6:09738d84a005 215 if(c == 'e') {
ywsim 6:09738d84a005 216 mycur = mycur +0.02;
ywsim 6:09738d84a005 217 }
ywsim 6:09738d84a005 218 if(c == 'r') {
ywsim 6:09738d84a005 219 mycur = mycur - 0.02;
ywsim 6:09738d84a005 220 }
ywsim 6:09738d84a005 221 if(c == 'd') {
ywsim 6:09738d84a005 222 mycur = mycur +0.1;
ywsim 6:09738d84a005 223 }
ywsim 6:09738d84a005 224 if(c == 'f') {
ywsim 6:09738d84a005 225 mycur = mycur - 0.1;
ywsim 6:09738d84a005 226 }
ywsim 6:09738d84a005 227
ywsim 6:09738d84a005 228 }
ywsim 6:09738d84a005 229 if(newprint == 1){
ywsim 6:09738d84a005 230 pc.printf("%.3f %.3f %.3f\r\n", mycur, motors[0].state.current, calcMass.myMass);
ywsim 6:09738d84a005 231 //pc.printf( "RawData: %ld Mass: %0.3f g Voltage: %0.5f mV\r\n", (uint32_t)myData.myRawValue, , 1000*calcVolt.myVoltage );
ywsim 6:09738d84a005 232
ywsim 6:09738d84a005 233 //pc.putc(motors[0].state.position);
ywsim 6:09738d84a005 234 //pc.write(&motors[0].state.position);
ywsim 6:09738d84a005 235 //dat1.f = motors[0].state.position;
ywsim 6:09738d84a005 236 //pc.printf("%c%c%c%c", dat1.s[0], dat1.s[1], dat1.s[2], dat1.s[3]);
ywsim 6:09738d84a005 237 newprint = 0;
ywsim 6:09738d84a005 238 }
benkatz 0:d6186b8990c5 239 }
benkatz 2:36a254d3dbf3 240 }
benkatz 0:d6186b8990c5 241
benkatz 2:36a254d3dbf3 242 /* low-level communication functoins below. Do not touch */
benkatz 0:d6186b8990c5 243
benkatz 2:36a254d3dbf3 244 void onMsgReceived()
benkatz 2:36a254d3dbf3 245 /* This interrupt gets called when a CAN message with ID CAN_ID shows up */
benkatz 2:36a254d3dbf3 246 {
benkatz 2:36a254d3dbf3 247 CANMessage rxMsg;
benkatz 2:36a254d3dbf3 248 rxMsg.len = 6;
benkatz 2:36a254d3dbf3 249 can.read(rxMsg); // read message into Rx message storage
benkatz 2:36a254d3dbf3 250 int id = rxMsg.data[0];
benkatz 2:36a254d3dbf3 251 for (int i = 0; i< N_MOTORS; i++)
benkatz 2:36a254d3dbf3 252 {
benkatz 2:36a254d3dbf3 253 if(motors[i].control.id == id)
benkatz 2:36a254d3dbf3 254 {
benkatz 2:36a254d3dbf3 255 memcpy(&motors[i].rxMsg, &rxMsg, sizeof(motors[i].rxMsg));
benkatz 2:36a254d3dbf3 256 unpack_reply(&motors[i]);
benkatz 2:36a254d3dbf3 257 }
benkatz 2:36a254d3dbf3 258 }
benkatz 2:36a254d3dbf3 259 }
benkatz 0:d6186b8990c5 260
benkatz 2:36a254d3dbf3 261 void init_motors(int ids[N_MOTORS])
benkatz 2:36a254d3dbf3 262 /* Initialize buffer lengths and IDs of the motors in the list */
benkatz 2:36a254d3dbf3 263 {
benkatz 2:36a254d3dbf3 264 for(int i = 0; i<N_MOTORS; i++)
benkatz 2:36a254d3dbf3 265 {
benkatz 2:36a254d3dbf3 266 motors[i].txMsg.len = 8;
benkatz 2:36a254d3dbf3 267 motors[i].rxMsg.len = 6;
benkatz 2:36a254d3dbf3 268 motors[i].control.id = ids[i];
benkatz 2:36a254d3dbf3 269 motors[i].txMsg.id = ids[i];
benkatz 2:36a254d3dbf3 270 motors[i].control.p_des = 0;
benkatz 2:36a254d3dbf3 271 motors[i].control.v_des = 0;
benkatz 2:36a254d3dbf3 272 motors[i].control.kp = 0;
benkatz 2:36a254d3dbf3 273 motors[i].control.kd = 0;
benkatz 2:36a254d3dbf3 274 motors[i].control.i_ff = 0;
benkatz 2:36a254d3dbf3 275 }
benkatz 2:36a254d3dbf3 276 }