Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@28:f08665a5ef6c, 2019-10-28 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 28 16:01:30 2019 +0000
Revision:
28:f08665a5ef6c
Parent:
27:4fa916e1f6a9
Child:
29:dd2450e6eb57
Upload working again, fixed encoder issues, swapped motors, tried PID values

Who changed what in which revision?

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