Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@29:dd2450e6eb57, 2019-10-29 (annotated)

Committer:
joostbonekamp
Date:
Tue Oct 29 11:47:49 2019 +0000
Revision:
29:dd2450e6eb57
Parent:
28:f08665a5ef6c
Child:
30:eda0a0591580
added calibration for high values of the EMG signal, fixed reverse motor polarity logic

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joostbonekamp 18:dddc8d9f7638 1 /*
joostbonekamp 18:dddc8d9f7638 2 To-do:
joostbonekamp 29:dd2450e6eb57 3 Encoder calibration with reverse kinematics
joostbonekamp 28:f08665a5ef6c 4
joostbonekamp 18:dddc8d9f7638 5 Homing
joostbonekamp 26:3456b03d5bce 6 Turning the magnet on/off (reading magnet status?)
joostbonekamp 26:3456b03d5bce 7 Gravity compensation (if necessary)
joostbonekamp 28:f08665a5ef6c 8 PID values (too large)
joostbonekamp 26:3456b03d5bce 9 Boundaries (after verification of the PID values)
joostbonekamp 18:dddc8d9f7638 10 */
RobertoO 0:67c50348f842 11 #include "mbed.h"
RobertoO 1:b862262a9d14 12 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 13 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 14 #include "QEI.h"
joostbonekamp 17:615c5d8b3710 15 #include "HIDScope.h"
joostbonekamp 17:615c5d8b3710 16 #include "BiQuad.h"
hidde1104 13:51ae2da8da55 17 #define PI 3.14159265
RobertoO 0:67c50348f842 18
joostbonekamp 12:88cbc65f2563 19 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 24:710d7d99b915 20 HIDScope scope(4); //HIDScope instance
joostbonekamp 28:f08665a5ef6c 21 DigitalOut motor0_direction(D7); //rotation motor 1 on shield (always D6)
joostbonekamp 28:f08665a5ef6c 22 FastPWM motor0_pwm(D6); //pwm 1 on shield (always D7)
joostbonekamp 28:f08665a5ef6c 23 DigitalOut motor1_direction(D4); //rotation motor 2 on shield (always D4)
joostbonekamp 28:f08665a5ef6c 24 FastPWM motor1_pwm(D5); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 25 Ticker loop_ticker; //used in main()
joostbonekamp 20:4424d447f3cd 26 Ticker scope_ticker;
joostbonekamp 26:3456b03d5bce 27 InterruptIn but1(SW2); //button on mbed board
joostbonekamp 12:88cbc65f2563 28 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 28:f08665a5ef6c 29 AnalogIn EMG0_sig(A0);
joostbonekamp 28:f08665a5ef6c 30 AnalogIn EMG0_ref(A1);
joostbonekamp 28:f08665a5ef6c 31 AnalogIn EMG1_sig(A2);
joostbonekamp 28:f08665a5ef6c 32 AnalogIn EMG1_ref(A3);
joostbonekamp 12:88cbc65f2563 33
joostbonekamp 17:615c5d8b3710 34 void check_failure();
joostbonekamp 23:9eeac9d1ecbe 35 int schmitt_trigger(float i);
joostbonekamp 10:b8c60fd468f1 36
joostbonekamp 17:615c5d8b3710 37 QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 17:615c5d8b3710 38 QEI enc2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 17:615c5d8b3710 39
joostbonekamp 24:710d7d99b915 40 BiQuad bq1_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 24:710d7d99b915 41 BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
joostbonekamp 24:710d7d99b915 42 BiQuad bq2_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 24:710d7d99b915 43 BiQuad bq2_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
joostbonekamp 3:e3d12393adb1 44
joostbonekamp 5:aa8b5d5e632f 45 //variables
joostbonekamp 29:dd2450e6eb57 46 enum States {s_idle, s_cali_EMG_low, s_cali_EMG_high, s_cali_enc, s_moving_magnet_off, s_moving_magnet_on, s_homing, s_failure};
joostbonekamp 12:88cbc65f2563 47 States state; //using the States enum
joostbonekamp 14:4cf17b10e504 48 struct actuator_state {
joostbonekamp 24:710d7d99b915 49 float duty_cycle[2]; //pwm of 1st motor
joostbonekamp 24:710d7d99b915 50 int direction[2]; //direction of 1st motor
joostbonekamp 29:dd2450e6eb57 51 bool swap_direction[2];
joostbonekamp 12:88cbc65f2563 52 bool magnet; //state of the magnet
joostbonekamp 17:615c5d8b3710 53 } actuator;
joostbonekamp 12:88cbc65f2563 54
joostbonekamp 14:4cf17b10e504 55 struct EMG_params {
joostbonekamp 24:710d7d99b915 56 float max[2]; //params of the emg, tbd during calibration
joostbonekamp 24:710d7d99b915 57 float min[2];
joostbonekamp 24:710d7d99b915 58 } EMG;
joostbonekamp 5:aa8b5d5e632f 59
joostbonekamp 24:710d7d99b915 60 struct PID_struct {
joostbonekamp 24:710d7d99b915 61 float P[2];
joostbonekamp 24:710d7d99b915 62 float I[2];
joostbonekamp 24:710d7d99b915 63 float D[2];
joostbonekamp 24:710d7d99b915 64 float I_counter[2];
joostbonekamp 24:710d7d99b915 65 } PID;
joostbonekamp 18:dddc8d9f7638 66
joostbonekamp 18:dddc8d9f7638 67 float dt = 0.001;
joostbonekamp 25:e1577c9e8c7e 68 float theta[2]; //theta0 = rot, theta1 = lin
joostbonekamp 24:710d7d99b915 69 int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
joostbonekamp 26:3456b03d5bce 70 float EMG_raw[2][2];
joostbonekamp 24:710d7d99b915 71 float EMG_filtered[2];
joostbonekamp 24:710d7d99b915 72 int enc_value[2];
joostbonekamp 24:710d7d99b915 73 float current_error[2] = {0.0, 0.0};
joostbonekamp 26:3456b03d5bce 74 float errors[2];
joostbonekamp 24:710d7d99b915 75 float last_error[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 76 float action[2];
joostbonekamp 12:88cbc65f2563 77 bool state_changed = false; //used to see if the state is "starting"
joostbonekamp 24:710d7d99b915 78 volatile bool button1_pressed = false;
joostbonekamp 24:710d7d99b915 79 volatile bool button2_pressed = false;
joostbonekamp 16:696e9cbcc823 80 volatile bool failure_occurred = false;
joostbonekamp 29:dd2450e6eb57 81 bool EMG_low_has_been_calibrated;
joostbonekamp 29:dd2450e6eb57 82 bool EMG_high_has_been_calibrated;
joostbonekamp 24:710d7d99b915 83 float filter_value[2];
joostbonekamp 25:e1577c9e8c7e 84 int speed[2];
joostbonekamp 24:710d7d99b915 85 int past_speed[2] = {0, 0};
joostbonekamp 26:3456b03d5bce 86 float velocity_desired[2];
joostbonekamp 25:e1577c9e8c7e 87 float theta_desired[2];
joostbonekamp 29:dd2450e6eb57 88 const int EMG_cali_amount = 2000;
joostbonekamp 26:3456b03d5bce 89 float past_EMG_values[2][EMG_cali_amount];
joostbonekamp 26:3456b03d5bce 90 int past_EMG_count = 0;
joostbonekamp 12:88cbc65f2563 91
joostbonekamp 12:88cbc65f2563 92 void do_nothing()
PatrickZieverink 9:6537eead1241 93 /*
joostbonekamp 12:88cbc65f2563 94 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 95 */
joostbonekamp 16:696e9cbcc823 96 {
joostbonekamp 16:696e9cbcc823 97 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 98 state_changed = true;
joostbonekamp 29:dd2450e6eb57 99 state = s_cali_EMG_low;
joostbonekamp 16:696e9cbcc823 100 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 101 }
joostbonekamp 26:3456b03d5bce 102 errors[0] = 0.0;
joostbonekamp 26:3456b03d5bce 103 errors[1] = 0.0;
joostbonekamp 16:696e9cbcc823 104 }
joostbonekamp 12:88cbc65f2563 105
joostbonekamp 12:88cbc65f2563 106 void failure()
joostbonekamp 12:88cbc65f2563 107 /*
joostbonekamp 12:88cbc65f2563 108 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 109 */
joostbonekamp 12:88cbc65f2563 110 {
joostbonekamp 12:88cbc65f2563 111 if (state_changed) {
joostbonekamp 12:88cbc65f2563 112 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 113 state_changed = false;
joostbonekamp 12:88cbc65f2563 114 }
joostbonekamp 26:3456b03d5bce 115 errors[0] = 0.0;
joostbonekamp 26:3456b03d5bce 116 errors[1] = 0.0;
joostbonekamp 26:3456b03d5bce 117 PID.I_counter[0] = 0.0;
joostbonekamp 26:3456b03d5bce 118 PID.I_counter[1] = 0.0;
joostbonekamp 12:88cbc65f2563 119 }
PatrickZieverink 9:6537eead1241 120
joostbonekamp 29:dd2450e6eb57 121 void cali_EMG_low()
joostbonekamp 12:88cbc65f2563 122 /*
joostbonekamp 16:696e9cbcc823 123 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 124 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 125 */
joostbonekamp 12:88cbc65f2563 126 {
joostbonekamp 12:88cbc65f2563 127 if (state_changed) {
joostbonekamp 29:dd2450e6eb57 128 pc.printf("Started low value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 12:88cbc65f2563 129 state_changed = false;
PatrickZieverink 9:6537eead1241 130 }
joostbonekamp 19:a37cae6964ca 131 if (past_EMG_count < EMG_cali_amount) {
joostbonekamp 24:710d7d99b915 132 past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
joostbonekamp 24:710d7d99b915 133 past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
joostbonekamp 19:a37cae6964ca 134 past_EMG_count++;
joostbonekamp 19:a37cae6964ca 135 }
joostbonekamp 29:dd2450e6eb57 136 else if (!EMG_low_has_been_calibrated) { //calibration has concluded
joostbonekamp 29:dd2450e6eb57 137 pc.printf("Low value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 24:710d7d99b915 138 float sum[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 139 float mean[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 140 for(int c = 0; c<2; c++){
joostbonekamp 24:710d7d99b915 141 for(int i = 0; i<EMG_cali_amount; i++) {
joostbonekamp 24:710d7d99b915 142 sum[c] += past_EMG_values[c][i];
joostbonekamp 24:710d7d99b915 143 }
joostbonekamp 24:710d7d99b915 144 mean[c] = sum[c]/(float)EMG_cali_amount;
joostbonekamp 24:710d7d99b915 145 EMG.min[c] = mean[c];
joostbonekamp 19:a37cae6964ca 146 }
joostbonekamp 24:710d7d99b915 147
joostbonekamp 29:dd2450e6eb57 148 //calibration done
joostbonekamp 29:dd2450e6eb57 149 pc.printf("Low EMG values: %f %f\r\n", EMG.min[0], EMG.min[1]);
joostbonekamp 29:dd2450e6eb57 150 EMG_low_has_been_calibrated = true;
joostbonekamp 29:dd2450e6eb57 151 }
joostbonekamp 29:dd2450e6eb57 152 errors[0] = 0.0;
joostbonekamp 29:dd2450e6eb57 153 errors[1] = 0.0;
joostbonekamp 29:dd2450e6eb57 154 PID.I_counter[0] = 0.0;
joostbonekamp 29:dd2450e6eb57 155 PID.I_counter[1] = 0.0;
joostbonekamp 29:dd2450e6eb57 156 if (button1_pressed && EMG_low_has_been_calibrated) {// move to high value calibration
joostbonekamp 29:dd2450e6eb57 157 button1_pressed = false;
joostbonekamp 29:dd2450e6eb57 158 state = s_cali_EMG_high;
joostbonekamp 29:dd2450e6eb57 159 state_changed = true;
joostbonekamp 29:dd2450e6eb57 160 }
joostbonekamp 29:dd2450e6eb57 161 }
joostbonekamp 29:dd2450e6eb57 162
joostbonekamp 29:dd2450e6eb57 163 void cali_EMG_high()
joostbonekamp 29:dd2450e6eb57 164 /*
joostbonekamp 29:dd2450e6eb57 165 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 29:dd2450e6eb57 166 added to the EMG_params instance.
joostbonekamp 29:dd2450e6eb57 167 */
joostbonekamp 29:dd2450e6eb57 168 {
joostbonekamp 29:dd2450e6eb57 169 if (state_changed) {
joostbonekamp 29:dd2450e6eb57 170 pc.printf("Started high value EMG calibration\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 29:dd2450e6eb57 171 state_changed = false;
joostbonekamp 29:dd2450e6eb57 172 past_EMG_count = 0;
joostbonekamp 29:dd2450e6eb57 173 }
joostbonekamp 29:dd2450e6eb57 174 if (past_EMG_count < EMG_cali_amount) {
joostbonekamp 29:dd2450e6eb57 175 past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
joostbonekamp 29:dd2450e6eb57 176 past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
joostbonekamp 29:dd2450e6eb57 177 past_EMG_count++;
joostbonekamp 29:dd2450e6eb57 178 }
joostbonekamp 29:dd2450e6eb57 179 else { //calibration has concluded
joostbonekamp 29:dd2450e6eb57 180 pc.printf("High value calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 29:dd2450e6eb57 181 float max[2] = {0.0, 0.0};
joostbonekamp 29:dd2450e6eb57 182 for(int c = 0; c<2; c++){
joostbonekamp 29:dd2450e6eb57 183 for(int i = 0; i<EMG_cali_amount; i++) {
joostbonekamp 29:dd2450e6eb57 184 if (past_EMG_values[c][i] > max[c]) {
joostbonekamp 29:dd2450e6eb57 185 max[c] = past_EMG_values[c][i];
joostbonekamp 29:dd2450e6eb57 186 }
joostbonekamp 29:dd2450e6eb57 187 }
joostbonekamp 29:dd2450e6eb57 188 EMG.max[c] = max[c];
joostbonekamp 29:dd2450e6eb57 189 }
joostbonekamp 29:dd2450e6eb57 190
joostbonekamp 19:a37cae6964ca 191 //calibration done, moving to cali_enc
joostbonekamp 29:dd2450e6eb57 192 pc.printf("High EMG values: %f %f\r\n", EMG.max[0], EMG.max[1]);
joostbonekamp 29:dd2450e6eb57 193 EMG_high_has_been_calibrated = true;
joostbonekamp 19:a37cae6964ca 194 state_changed = true;
joostbonekamp 19:a37cae6964ca 195 state = s_cali_enc;
joostbonekamp 19:a37cae6964ca 196 }
joostbonekamp 26:3456b03d5bce 197 errors[0] = 0.0;
joostbonekamp 26:3456b03d5bce 198 errors[1] = 0.0;
joostbonekamp 26:3456b03d5bce 199 PID.I_counter[0] = 0.0;
joostbonekamp 26:3456b03d5bce 200 PID.I_counter[1] = 0.0;
joostbonekamp 12:88cbc65f2563 201 }
joostbonekamp 16:696e9cbcc823 202
joostbonekamp 12:88cbc65f2563 203 void cali_enc()
joostbonekamp 12:88cbc65f2563 204 /*
joostbonekamp 14:4cf17b10e504 205 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 19:a37cae6964ca 206 position for the linear stage and the horizontal postition for the
joostbonekamp 12:88cbc65f2563 207 rotating stage.
joostbonekamp 12:88cbc65f2563 208 */
joostbonekamp 12:88cbc65f2563 209 {
joostbonekamp 12:88cbc65f2563 210 if (state_changed) {
joostbonekamp 12:88cbc65f2563 211 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 212 state_changed = false;
PatrickZieverink 9:6537eead1241 213 }
joostbonekamp 16:696e9cbcc823 214 if (button1_pressed) {
joostbonekamp 25:e1577c9e8c7e 215 pc.printf("Encoder has been calibrated\r\n");
joostbonekamp 24:710d7d99b915 216 enc_zero[0] = enc_value[0];
joostbonekamp 28:f08665a5ef6c 217 enc_zero[1] = enc_value[1];//+130990; //the magic number!
joostbonekamp 16:696e9cbcc823 218 button1_pressed = false;
joostbonekamp 17:615c5d8b3710 219 state = s_moving_magnet_off;
joostbonekamp 16:696e9cbcc823 220 state_changed = true;
joostbonekamp 16:696e9cbcc823 221 }
joostbonekamp 28:f08665a5ef6c 222 errors[0] = 1.0e-5*speed[0];
joostbonekamp 28:f08665a5ef6c 223 errors[1] = 1.0e-5*speed[1];
joostbonekamp 28:f08665a5ef6c 224
joostbonekamp 12:88cbc65f2563 225 }
joostbonekamp 16:696e9cbcc823 226
joostbonekamp 12:88cbc65f2563 227 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 228 /*
joostbonekamp 14:4cf17b10e504 229 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 230 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 231 */
joostbonekamp 12:88cbc65f2563 232 {
joostbonekamp 12:88cbc65f2563 233 if (state_changed) {
joostbonekamp 12:88cbc65f2563 234 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 235 state_changed = false;
PatrickZieverink 9:6537eead1241 236 }
joostbonekamp 26:3456b03d5bce 237 theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
joostbonekamp 26:3456b03d5bce 238 theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
joostbonekamp 26:3456b03d5bce 239 errors[0] = theta_desired[0] - theta[0];
joostbonekamp 28:f08665a5ef6c 240 errors[1] = theta_desired[1] - theta[1];
joostbonekamp 26:3456b03d5bce 241 if (button2_pressed) {
joostbonekamp 28:f08665a5ef6c 242 pc.printf("positions: (rad, m): %f %f\r\n"
joostbonekamp 28:f08665a5ef6c 243 "Errors: %f %f\r\n"
joostbonekamp 28:f08665a5ef6c 244 "Previous actions: %f %f\r\n", theta[0], theta[1], errors[0], errors[1], actuator.duty_cycle[0], actuator.duty_cycle[1]);
joostbonekamp 26:3456b03d5bce 245 button2_pressed = false;
joostbonekamp 26:3456b03d5bce 246 }
PatrickZieverink 9:6537eead1241 247 }
joostbonekamp 16:696e9cbcc823 248
joostbonekamp 12:88cbc65f2563 249 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 250 /*
joostbonekamp 14:4cf17b10e504 251 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 252 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 253 */
joostbonekamp 12:88cbc65f2563 254 {
joostbonekamp 12:88cbc65f2563 255 if (state_changed) {
joostbonekamp 12:88cbc65f2563 256 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 257 state_changed = false;
joostbonekamp 12:88cbc65f2563 258 }
joostbonekamp 26:3456b03d5bce 259 theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
joostbonekamp 26:3456b03d5bce 260 theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
joostbonekamp 26:3456b03d5bce 261 errors[0] = theta_desired[0] - theta[0];
joostbonekamp 26:3456b03d5bce 262 errors[1] = theta_desired[1] - theta[0];
PatrickZieverink 9:6537eead1241 263 }
joostbonekamp 12:88cbc65f2563 264 void homing()
joostbonekamp 12:88cbc65f2563 265 /*
joostbonekamp 14:4cf17b10e504 266 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 267 */
joostbonekamp 12:88cbc65f2563 268 {
joostbonekamp 12:88cbc65f2563 269 if (state_changed) {
joostbonekamp 25:e1577c9e8c7e 270 pc.printf("Started homing\r\n");
joostbonekamp 12:88cbc65f2563 271 state_changed = false;
joostbonekamp 12:88cbc65f2563 272 }
joostbonekamp 26:3456b03d5bce 273 theta_desired[0] = theta[0] + dt*(velocity_desired[1]*cos(theta[0])/theta[1] - velocity_desired[0]*sin(theta[0])/theta[1]);
joostbonekamp 26:3456b03d5bce 274 theta_desired[1] = theta[1] + dt*(velocity_desired[0]*cos(theta[0]) - velocity_desired[1]*sin(theta[0]));
joostbonekamp 26:3456b03d5bce 275 errors[0] = theta_desired[0] - theta[0];
joostbonekamp 26:3456b03d5bce 276 errors[1] = theta_desired[1] - theta[0];
joostbonekamp 12:88cbc65f2563 277 }
PatrickZieverink 9:6537eead1241 278
joostbonekamp 12:88cbc65f2563 279 void measure_signals()
joostbonekamp 12:88cbc65f2563 280 {
joostbonekamp 19:a37cae6964ca 281 //only one emg input, reference and plus
joostbonekamp 28:f08665a5ef6c 282 EMG_raw[0][0] = EMG0_sig.read();
joostbonekamp 28:f08665a5ef6c 283 EMG_raw[0][1] = EMG0_ref.read();
joostbonekamp 28:f08665a5ef6c 284 EMG_raw[1][0] = EMG1_sig.read();
joostbonekamp 28:f08665a5ef6c 285 EMG_raw[1][1] = EMG1_ref.read();
joostbonekamp 24:710d7d99b915 286 filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
joostbonekamp 24:710d7d99b915 287 filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
joostbonekamp 24:710d7d99b915 288
joostbonekamp 24:710d7d99b915 289 enc_value[0] = enc1.getPulses();
joostbonekamp 24:710d7d99b915 290 enc_value[1] = enc2.getPulses();
joostbonekamp 19:a37cae6964ca 291
joostbonekamp 24:710d7d99b915 292 for(int c = 0; c < 2; c++) {
joostbonekamp 29:dd2450e6eb57 293 if (EMG_high_has_been_calibrated && EMG_low_has_been_calibrated) {
joostbonekamp 24:710d7d99b915 294 EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
joostbonekamp 24:710d7d99b915 295 }
joostbonekamp 24:710d7d99b915 296 else {
joostbonekamp 24:710d7d99b915 297 EMG_filtered[c] = filter_value[c];
joostbonekamp 24:710d7d99b915 298 }
joostbonekamp 24:710d7d99b915 299 enc_value[c] -= enc_zero[c];
joostbonekamp 24:710d7d99b915 300 theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
joostbonekamp 19:a37cae6964ca 301 }
joostbonekamp 26:3456b03d5bce 302 theta[1] = theta[1]*(5.05*0.008*2*PI)+0.63;
joostbonekamp 25:e1577c9e8c7e 303
joostbonekamp 25:e1577c9e8c7e 304 for(int c = 0; c<2; c++) {
joostbonekamp 25:e1577c9e8c7e 305 speed[c] = schmitt_trigger(EMG_filtered[c]);
joostbonekamp 25:e1577c9e8c7e 306 if (speed[c] == -1) {
joostbonekamp 25:e1577c9e8c7e 307 speed[c] = past_speed[c];
joostbonekamp 25:e1577c9e8c7e 308 }
joostbonekamp 25:e1577c9e8c7e 309 past_speed[c] = speed[c];
joostbonekamp 25:e1577c9e8c7e 310 if (speed[c] == 0){
joostbonekamp 28:f08665a5ef6c 311 velocity_desired[c] = 0.00f;
joostbonekamp 25:e1577c9e8c7e 312 }
joostbonekamp 25:e1577c9e8c7e 313 if (speed[c] == 1){
joostbonekamp 28:f08665a5ef6c 314 velocity_desired[c] = 0.01f;
joostbonekamp 25:e1577c9e8c7e 315 }
joostbonekamp 25:e1577c9e8c7e 316 if (speed[c] == 2){
joostbonekamp 28:f08665a5ef6c 317 velocity_desired[c] = 0.02f;
joostbonekamp 25:e1577c9e8c7e 318 }
joostbonekamp 25:e1577c9e8c7e 319 }
joostbonekamp 12:88cbc65f2563 320 }
joostbonekamp 10:b8c60fd468f1 321
joostbonekamp 29:dd2450e6eb57 322 void motor_controller() {
joostbonekamp 24:710d7d99b915 323 float P_action[2];
joostbonekamp 24:710d7d99b915 324 float I_action[2];
joostbonekamp 24:710d7d99b915 325 float D_action[2];
joostbonekamp 24:710d7d99b915 326 float velocity_estimate[2];
joostbonekamp 24:710d7d99b915 327 for (int c = 0; c<2; c++) {
joostbonekamp 24:710d7d99b915 328
joostbonekamp 24:710d7d99b915 329 //P action
joostbonekamp 24:710d7d99b915 330 P_action[c] = PID.P[c] * errors[c];
joostbonekamp 24:710d7d99b915 331
joostbonekamp 24:710d7d99b915 332 //I part
joostbonekamp 26:3456b03d5bce 333 PID.I_counter[c] += errors[c]*dt;
joostbonekamp 24:710d7d99b915 334 I_action[c] = PID.I_counter[c] * PID.I[c];
joostbonekamp 18:dddc8d9f7638 335
joostbonekamp 24:710d7d99b915 336 //D part
joostbonekamp 24:710d7d99b915 337 velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
joostbonekamp 24:710d7d99b915 338 D_action[c] = velocity_estimate[c] * PID.D[c];
joostbonekamp 18:dddc8d9f7638 339
joostbonekamp 26:3456b03d5bce 340 action[c] = (P_action[c] + I_action[c] + D_action[c])/dt;
joostbonekamp 24:710d7d99b915 341 last_error[c] = errors[c];
joostbonekamp 26:3456b03d5bce 342
joostbonekamp 26:3456b03d5bce 343 actuator.direction[c] = (action[c] > 0); //1 if action is positive, 0 otherwise
joostbonekamp 26:3456b03d5bce 344 actuator.duty_cycle[c] = fabs(action[c]);
joostbonekamp 28:f08665a5ef6c 345 if (actuator.duty_cycle[c] > 1.0f) {actuator.duty_cycle[c] = 1.0f;}
joostbonekamp 28:f08665a5ef6c 346 if (actuator.duty_cycle[c] < 0.0f) {actuator.duty_cycle[c] = 0.0f;}
joostbonekamp 24:710d7d99b915 347 }
joostbonekamp 17:615c5d8b3710 348 }
joostbonekamp 17:615c5d8b3710 349
joostbonekamp 15:9a1f34bc9958 350 void output()
joostbonekamp 14:4cf17b10e504 351 {
joostbonekamp 29:dd2450e6eb57 352 if (actuator.swap_direction[0]) {
joostbonekamp 29:dd2450e6eb57 353 motor0_direction = !actuator.direction[0];
joostbonekamp 29:dd2450e6eb57 354 }
joostbonekamp 29:dd2450e6eb57 355 else {
joostbonekamp 29:dd2450e6eb57 356 motor0_direction = actuator.direction[0];
joostbonekamp 29:dd2450e6eb57 357 }
joostbonekamp 29:dd2450e6eb57 358 if (actuator.swap_direction[1]) {
joostbonekamp 29:dd2450e6eb57 359 motor1_direction = !actuator.direction[1];
joostbonekamp 29:dd2450e6eb57 360 }
joostbonekamp 29:dd2450e6eb57 361 else {
joostbonekamp 29:dd2450e6eb57 362 motor0_direction = actuator.direction[1];
joostbonekamp 29:dd2450e6eb57 363 }
joostbonekamp 24:710d7d99b915 364 motor0_pwm.write(actuator.duty_cycle[0]);
joostbonekamp 24:710d7d99b915 365 motor1_pwm.write(actuator.duty_cycle[1]);
joostbonekamp 20:4424d447f3cd 366
joostbonekamp 28:f08665a5ef6c 367 scope.set(0, EMG_filtered[0]);
joostbonekamp 28:f08665a5ef6c 368 scope.set(1, speed[0]);
joostbonekamp 28:f08665a5ef6c 369 scope.set(2, actuator.duty_cycle[0]);
joostbonekamp 15:9a1f34bc9958 370 }
joostbonekamp 14:4cf17b10e504 371
joostbonekamp 15:9a1f34bc9958 372 void state_machine()
joostbonekamp 15:9a1f34bc9958 373 {
joostbonekamp 16:696e9cbcc823 374 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 375 //run current state
joostbonekamp 17:615c5d8b3710 376 switch (state) {
joostbonekamp 17:615c5d8b3710 377 case s_idle:
joostbonekamp 15:9a1f34bc9958 378 do_nothing();
joostbonekamp 15:9a1f34bc9958 379 break;
joostbonekamp 17:615c5d8b3710 380 case s_failure:
joostbonekamp 15:9a1f34bc9958 381 failure();
joostbonekamp 15:9a1f34bc9958 382 break;
joostbonekamp 29:dd2450e6eb57 383 case s_cali_EMG_low:
joostbonekamp 29:dd2450e6eb57 384 cali_EMG_low();
joostbonekamp 29:dd2450e6eb57 385 break;
joostbonekamp 29:dd2450e6eb57 386 case s_cali_EMG_high:
joostbonekamp 29:dd2450e6eb57 387 cali_EMG_high();
joostbonekamp 15:9a1f34bc9958 388 break;
joostbonekamp 17:615c5d8b3710 389 case s_cali_enc:
joostbonekamp 17:615c5d8b3710 390 cali_enc();
joostbonekamp 15:9a1f34bc9958 391 break;
joostbonekamp 17:615c5d8b3710 392 case s_moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 393 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 394 break;
joostbonekamp 17:615c5d8b3710 395 case s_moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 396 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 397 break;
joostbonekamp 17:615c5d8b3710 398 case s_homing:
joostbonekamp 15:9a1f34bc9958 399 homing();
joostbonekamp 15:9a1f34bc9958 400 break;
joostbonekamp 5:aa8b5d5e632f 401 }
joostbonekamp 5:aa8b5d5e632f 402 }
joostbonekamp 15:9a1f34bc9958 403
joostbonekamp 15:9a1f34bc9958 404 void main_loop()
joostbonekamp 12:88cbc65f2563 405 {
joostbonekamp 15:9a1f34bc9958 406 measure_signals();
joostbonekamp 15:9a1f34bc9958 407 state_machine();
joostbonekamp 15:9a1f34bc9958 408 motor_controller();
joostbonekamp 15:9a1f34bc9958 409 output();
joostbonekamp 15:9a1f34bc9958 410 }
joostbonekamp 14:4cf17b10e504 411
joostbonekamp 14:4cf17b10e504 412 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 413 //state machines
joostbonekamp 16:696e9cbcc823 414 void check_failure()
joostbonekamp 15:9a1f34bc9958 415 {
joostbonekamp 26:3456b03d5bce 416 if (failure_occurred) {
joostbonekamp 26:3456b03d5bce 417 state = s_failure;
joostbonekamp 26:3456b03d5bce 418 state_changed = true;
joostbonekamp 26:3456b03d5bce 419 }
joostbonekamp 16:696e9cbcc823 420 }
joostbonekamp 16:696e9cbcc823 421
joostbonekamp 16:696e9cbcc823 422 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 423 {
joostbonekamp 26:3456b03d5bce 424 if(!but2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 425 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 426 }
joostbonekamp 23:9eeac9d1ecbe 427 button1_pressed = true;
joostbonekamp 26:3456b03d5bce 428 pc.printf("Button 1 pressed!\n\r");
joostbonekamp 15:9a1f34bc9958 429 }
joostbonekamp 14:4cf17b10e504 430
joostbonekamp 16:696e9cbcc823 431 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 432 {
joostbonekamp 26:3456b03d5bce 433 if(!but1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 434 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 435 }
joostbonekamp 23:9eeac9d1ecbe 436 button2_pressed = true;
joostbonekamp 26:3456b03d5bce 437 pc.printf("Button 2 pressed!\n\r");
joostbonekamp 15:9a1f34bc9958 438 }
joostbonekamp 17:615c5d8b3710 439
joostbonekamp 16:696e9cbcc823 440 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 441 {
joostbonekamp 17:615c5d8b3710 442 int speed;
joostbonekamp 16:696e9cbcc823 443 speed = -1; //default value, this means the state should not change
joostbonekamp 25:e1577c9e8c7e 444 if (i > 0.000f && i < 0.125f) {speed = 0;}
joostbonekamp 25:e1577c9e8c7e 445 if (i > 0.250f && i < 0.375f) {speed = 1;}
joostbonekamp 25:e1577c9e8c7e 446 if (i > 0.500f && i < 1.000f) {speed = 2;}
joostbonekamp 16:696e9cbcc823 447 return speed;
joostbonekamp 16:696e9cbcc823 448 }
joostbonekamp 14:4cf17b10e504 449
joostbonekamp 15:9a1f34bc9958 450 int main()
joostbonekamp 15:9a1f34bc9958 451 {
joostbonekamp 15:9a1f34bc9958 452 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 453 pc.printf("Executing main()... \r\n");
joostbonekamp 17:615c5d8b3710 454 state = s_idle;
joostbonekamp 12:88cbc65f2563 455
joostbonekamp 28:f08665a5ef6c 456 motor0_pwm.period(1.0f/160000.0f); // 1/frequency van waarop hij draait
joostbonekamp 28:f08665a5ef6c 457 motor1_pwm.period(1.0f/160000.0f); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 458
joostbonekamp 24:710d7d99b915 459 actuator.direction[0] = 0;
joostbonekamp 24:710d7d99b915 460 actuator.direction[1] = 0;
joostbonekamp 26:3456b03d5bce 461
joostbonekamp 29:dd2450e6eb57 462 actuator.swap_direction[0] = false;
joostbonekamp 29:dd2450e6eb57 463 actuator.swap_direction[1] = false;
joostbonekamp 24:710d7d99b915 464
joostbonekamp 28:f08665a5ef6c 465 PID.P[0] = 1.0;
joostbonekamp 28:f08665a5ef6c 466 PID.P[1] = 1.0;
joostbonekamp 26:3456b03d5bce 467 PID.I[0] = 0.0;
joostbonekamp 26:3456b03d5bce 468 PID.I[1] = 0.0;
joostbonekamp 26:3456b03d5bce 469 PID.D[0] = 0.0;
joostbonekamp 26:3456b03d5bce 470 PID.D[1] = 0.0;
joostbonekamp 24:710d7d99b915 471 PID.I_counter[0] = 0.0;
joostbonekamp 24:710d7d99b915 472 PID.I_counter[1] = 0.0;
joostbonekamp 26:3456b03d5bce 473
joostbonekamp 15:9a1f34bc9958 474 actuator.magnet = false;
joostbonekamp 24:710d7d99b915 475 EMG.max[0] = 0.01;
joostbonekamp 24:710d7d99b915 476 EMG.max[1] = 0.01;
joostbonekamp 24:710d7d99b915 477
joostbonekamp 15:9a1f34bc9958 478 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 479 but2.fall(&but2_interrupt);
joostbonekamp 28:f08665a5ef6c 480 scope_ticker.attach(&scope, &HIDScope::send, 0.05);
joostbonekamp 18:dddc8d9f7638 481 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 482 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 483 while (true) {
joostbonekamp 16:696e9cbcc823 484 wait(0.1f);
joostbonekamp 16:696e9cbcc823 485 }
joostbonekamp 17:615c5d8b3710 486 }