Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@24:710d7d99b915, 2019-10-24 (annotated)

Committer:
joostbonekamp
Date:
Thu Oct 24 11:30:32 2019 +0000
Revision:
24:710d7d99b915
Parent:
23:9eeac9d1ecbe
Child:
25:e1577c9e8c7e
Everything is working for 2 channels now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joostbonekamp 18:dddc8d9f7638 1 /*
joostbonekamp 18:dddc8d9f7638 2 To-do:
joostbonekamp 18:dddc8d9f7638 3 Add reference generator
joostbonekamp 18:dddc8d9f7638 4 fully implement schmitt trigger
joostbonekamp 18:dddc8d9f7638 5 Homing
joostbonekamp 18:dddc8d9f7638 6 Turning the magnet on/off
joostbonekamp 18:dddc8d9f7638 7 Inverse kinematics
joostbonekamp 18:dddc8d9f7638 8 Gravity compensation
joostbonekamp 18:dddc8d9f7638 9 PID values
joostbonekamp 18:dddc8d9f7638 10 General program layout
joostbonekamp 19:a37cae6964ca 11 better names for EMG input
joostbonekamp 18:dddc8d9f7638 12 */
RobertoO 0:67c50348f842 13 #include "mbed.h"
RobertoO 1:b862262a9d14 14 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 15 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 16 #include "QEI.h"
joostbonekamp 17:615c5d8b3710 17 #include "HIDScope.h"
joostbonekamp 17:615c5d8b3710 18 #include "BiQuad.h"
hidde1104 13:51ae2da8da55 19 #define PI 3.14159265
RobertoO 0:67c50348f842 20
joostbonekamp 12:88cbc65f2563 21 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 24:710d7d99b915 22 HIDScope scope(4); //HIDScope instance
joostbonekamp 24:710d7d99b915 23 DigitalOut motor0_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 24:710d7d99b915 24 FastPWM motor0_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 24:710d7d99b915 25 DigitalOut motor1_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 24:710d7d99b915 26 FastPWM motor1_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 27 Ticker loop_ticker; //used in main()
joostbonekamp 20:4424d447f3cd 28 Ticker scope_ticker;
joostbonekamp 23:9eeac9d1ecbe 29 InterruptIn but1(SW2); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 30 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 24:710d7d99b915 31 AnalogIn EMG1_sig(A0);
joostbonekamp 24:710d7d99b915 32 AnalogIn EMG1_ref(A1);
joostbonekamp 24:710d7d99b915 33 AnalogIn EMG2_sig(A2);
joostbonekamp 24:710d7d99b915 34 AnalogIn EMG2_ref(A3);
joostbonekamp 12:88cbc65f2563 35
joostbonekamp 17:615c5d8b3710 36 void check_failure();
joostbonekamp 23:9eeac9d1ecbe 37 int schmitt_trigger(float i);
joostbonekamp 10:b8c60fd468f1 38
joostbonekamp 17:615c5d8b3710 39 QEI enc1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 17:615c5d8b3710 40 QEI enc2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 17:615c5d8b3710 41
joostbonekamp 24:710d7d99b915 42 BiQuad bq1_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 24:710d7d99b915 43 BiQuad bq1_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
joostbonekamp 24:710d7d99b915 44 BiQuad bq2_1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 24:710d7d99b915 45 BiQuad bq2_2 (8.05339325492925e-06, 1.61067865098585e-05, 8.05339325492925e-06, -1.99254216118629, 0.992574747774901);
joostbonekamp 3:e3d12393adb1 46
joostbonekamp 5:aa8b5d5e632f 47 //variables
joostbonekamp 17:615c5d8b3710 48 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 49 States state; //using the States enum
joostbonekamp 14:4cf17b10e504 50 struct actuator_state {
joostbonekamp 24:710d7d99b915 51 float duty_cycle[2]; //pwm of 1st motor
joostbonekamp 24:710d7d99b915 52 int direction[2]; //direction of 1st motor
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 24:710d7d99b915 69 float theta[2];
joostbonekamp 24:710d7d99b915 70 int enc_zero[2] = {0, 0};//the zero position of the encoders, to be determined from the encoder calibration
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 24:710d7d99b915 74 float last_error[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 75 float action[2];
joostbonekamp 12:88cbc65f2563 76 bool state_changed = false; //used to see if the state is "starting"
joostbonekamp 24:710d7d99b915 77 volatile bool button1_pressed = false;
joostbonekamp 24:710d7d99b915 78 volatile bool button2_pressed = false;
joostbonekamp 16:696e9cbcc823 79 volatile bool failure_occurred = false;
joostbonekamp 16:696e9cbcc823 80 bool EMG_has_been_calibrated;
joostbonekamp 24:710d7d99b915 81 float filter_value[2];
joostbonekamp 24:710d7d99b915 82 int past_speed[2] = {0, 0};
joostbonekamp 12:88cbc65f2563 83
joostbonekamp 12:88cbc65f2563 84 void do_nothing()
joostbonekamp 5:aa8b5d5e632f 85
PatrickZieverink 9:6537eead1241 86 /*
joostbonekamp 12:88cbc65f2563 87 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 88 */
joostbonekamp 16:696e9cbcc823 89 {
joostbonekamp 16:696e9cbcc823 90 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 91 state_changed = true;
joostbonekamp 23:9eeac9d1ecbe 92 state = s_cali_EMG;
joostbonekamp 16:696e9cbcc823 93 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 94 }
joostbonekamp 16:696e9cbcc823 95 }
joostbonekamp 12:88cbc65f2563 96
joostbonekamp 12:88cbc65f2563 97 void failure()
joostbonekamp 12:88cbc65f2563 98 /*
joostbonekamp 12:88cbc65f2563 99 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 100 */
joostbonekamp 12:88cbc65f2563 101 {
joostbonekamp 12:88cbc65f2563 102 if (state_changed) {
joostbonekamp 12:88cbc65f2563 103 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 104 state_changed = false;
joostbonekamp 12:88cbc65f2563 105 }
joostbonekamp 12:88cbc65f2563 106 }
PatrickZieverink 9:6537eead1241 107
joostbonekamp 23:9eeac9d1ecbe 108 const int EMG_cali_amount = 1000;
joostbonekamp 24:710d7d99b915 109 float past_EMG_values[2][EMG_cali_amount];
joostbonekamp 23:9eeac9d1ecbe 110 int past_EMG_count = 0;
joostbonekamp 23:9eeac9d1ecbe 111
joostbonekamp 12:88cbc65f2563 112 void cali_EMG()
joostbonekamp 12:88cbc65f2563 113 /*
joostbonekamp 16:696e9cbcc823 114 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 115 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 116 */
joostbonekamp 12:88cbc65f2563 117 {
joostbonekamp 12:88cbc65f2563 118 if (state_changed) {
joostbonekamp 23:9eeac9d1ecbe 119 pc.printf("Started EMG calibration\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 12:88cbc65f2563 120 state_changed = false;
PatrickZieverink 9:6537eead1241 121 }
joostbonekamp 19:a37cae6964ca 122 if (past_EMG_count < EMG_cali_amount) {
joostbonekamp 24:710d7d99b915 123 past_EMG_values[0][past_EMG_count] = EMG_filtered[0];
joostbonekamp 24:710d7d99b915 124 past_EMG_values[1][past_EMG_count] = EMG_filtered[1];
joostbonekamp 19:a37cae6964ca 125 past_EMG_count++;
joostbonekamp 19:a37cae6964ca 126 }
joostbonekamp 23:9eeac9d1ecbe 127 else { //calibration has concluded
joostbonekamp 23:9eeac9d1ecbe 128 pc.printf("Calibration concluded.\r\nTime is %i\r\n", us_ticker_read());
joostbonekamp 24:710d7d99b915 129 float sum[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 130 float mean[2] = {0.0, 0.0};
joostbonekamp 24:710d7d99b915 131 for(int c = 0; c<2; c++){
joostbonekamp 24:710d7d99b915 132 for(int i = 0; i<EMG_cali_amount; i++) {
joostbonekamp 24:710d7d99b915 133 sum[c] += past_EMG_values[c][i];
joostbonekamp 24:710d7d99b915 134 }
joostbonekamp 24:710d7d99b915 135 mean[c] = sum[c]/(float)EMG_cali_amount;
joostbonekamp 24:710d7d99b915 136 EMG.min[c] = mean[c];
joostbonekamp 19:a37cae6964ca 137 }
joostbonekamp 24:710d7d99b915 138
joostbonekamp 19:a37cae6964ca 139 //calibration done, moving to cali_enc
joostbonekamp 24:710d7d99b915 140 pc.printf("Min values: %f %f\r\n", EMG.min[0], EMG.min[1]);
joostbonekamp 23:9eeac9d1ecbe 141 pc.printf("Length: %f\r\n", (float)EMG_cali_amount);
joostbonekamp 19:a37cae6964ca 142 EMG_has_been_calibrated = true;
joostbonekamp 19:a37cae6964ca 143 state_changed = true;
joostbonekamp 19:a37cae6964ca 144 state = s_cali_enc;
joostbonekamp 19:a37cae6964ca 145 }
joostbonekamp 12:88cbc65f2563 146 }
joostbonekamp 16:696e9cbcc823 147
joostbonekamp 12:88cbc65f2563 148 void cali_enc()
joostbonekamp 12:88cbc65f2563 149 /*
joostbonekamp 14:4cf17b10e504 150 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 19:a37cae6964ca 151 position for the linear stage and the horizontal postition for the
joostbonekamp 12:88cbc65f2563 152 rotating stage.
joostbonekamp 12:88cbc65f2563 153 */
joostbonekamp 12:88cbc65f2563 154 {
joostbonekamp 12:88cbc65f2563 155 if (state_changed) {
joostbonekamp 12:88cbc65f2563 156 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 157 state_changed = false;
PatrickZieverink 9:6537eead1241 158 }
joostbonekamp 16:696e9cbcc823 159 if (button1_pressed) {
joostbonekamp 19:a37cae6964ca 160 pc.printf("Encoder has been calibrated");
joostbonekamp 24:710d7d99b915 161 enc_zero[0] = enc_value[0];
joostbonekamp 24:710d7d99b915 162 enc_zero[1] = enc_value[1];
joostbonekamp 16:696e9cbcc823 163 button1_pressed = false;
joostbonekamp 17:615c5d8b3710 164 state = s_moving_magnet_off;
joostbonekamp 16:696e9cbcc823 165 state_changed = true;
joostbonekamp 16:696e9cbcc823 166
joostbonekamp 16:696e9cbcc823 167 }
joostbonekamp 12:88cbc65f2563 168 }
joostbonekamp 16:696e9cbcc823 169
joostbonekamp 12:88cbc65f2563 170 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 171 /*
joostbonekamp 14:4cf17b10e504 172 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 173 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 174 */
joostbonekamp 12:88cbc65f2563 175 {
joostbonekamp 12:88cbc65f2563 176 if (state_changed) {
joostbonekamp 12:88cbc65f2563 177 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 178 state_changed = false;
PatrickZieverink 9:6537eead1241 179 }
PatrickZieverink 9:6537eead1241 180 }
joostbonekamp 16:696e9cbcc823 181
joostbonekamp 12:88cbc65f2563 182 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 183 /*
joostbonekamp 14:4cf17b10e504 184 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 185 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 186 */
joostbonekamp 12:88cbc65f2563 187 {
joostbonekamp 12:88cbc65f2563 188 if (state_changed) {
joostbonekamp 12:88cbc65f2563 189 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 190 state_changed = false;
joostbonekamp 12:88cbc65f2563 191 }
joostbonekamp 12:88cbc65f2563 192 return;
PatrickZieverink 9:6537eead1241 193 }
joostbonekamp 12:88cbc65f2563 194 void homing()
joostbonekamp 12:88cbc65f2563 195 /*
joostbonekamp 14:4cf17b10e504 196 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 197 */
joostbonekamp 12:88cbc65f2563 198 {
joostbonekamp 12:88cbc65f2563 199 if (state_changed) {
joostbonekamp 12:88cbc65f2563 200 pc.printf("Started homing");
joostbonekamp 12:88cbc65f2563 201 state_changed = false;
joostbonekamp 12:88cbc65f2563 202 }
joostbonekamp 12:88cbc65f2563 203 return;
joostbonekamp 12:88cbc65f2563 204 }
PatrickZieverink 9:6537eead1241 205
joostbonekamp 24:710d7d99b915 206 float EMG_raw[2][2];
joostbonekamp 12:88cbc65f2563 207 void measure_signals()
joostbonekamp 12:88cbc65f2563 208 {
joostbonekamp 19:a37cae6964ca 209 //only one emg input, reference and plus
joostbonekamp 24:710d7d99b915 210 EMG_raw[0][0] = EMG1_sig.read();
joostbonekamp 24:710d7d99b915 211 EMG_raw[0][1] = EMG1_ref.read();
joostbonekamp 24:710d7d99b915 212 EMG_raw[1][0] = EMG2_sig.read();
joostbonekamp 24:710d7d99b915 213 EMG_raw[1][1] = EMG2_ref.read();
joostbonekamp 24:710d7d99b915 214 filter_value[0] = fabs(bq1_2.step(fabs(bq1_1.step(EMG_raw[0][0] - EMG_raw[0][1]))));
joostbonekamp 24:710d7d99b915 215 filter_value[1] = fabs(bq2_2.step(fabs(bq2_1.step(EMG_raw[1][0] - EMG_raw[1][1]))));
joostbonekamp 24:710d7d99b915 216
joostbonekamp 24:710d7d99b915 217 enc_value[0] = enc1.getPulses();
joostbonekamp 24:710d7d99b915 218 enc_value[1] = enc2.getPulses();
joostbonekamp 19:a37cae6964ca 219
joostbonekamp 24:710d7d99b915 220 for(int c = 0; c < 2; c++) {
joostbonekamp 24:710d7d99b915 221 if (filter_value[c] > EMG.max[c]) {
joostbonekamp 24:710d7d99b915 222 EMG.max[c] = filter_value[c];
joostbonekamp 24:710d7d99b915 223 }
joostbonekamp 24:710d7d99b915 224 if (EMG_has_been_calibrated) {
joostbonekamp 24:710d7d99b915 225 EMG_filtered[c] = (filter_value[c]-EMG.min[c])/(EMG.max[c]-EMG.min[c]);
joostbonekamp 24:710d7d99b915 226 }
joostbonekamp 24:710d7d99b915 227 else {
joostbonekamp 24:710d7d99b915 228 EMG_filtered[c] = filter_value[c];
joostbonekamp 24:710d7d99b915 229 }
joostbonekamp 24:710d7d99b915 230 enc_value[c] -= enc_zero[c];
joostbonekamp 24:710d7d99b915 231 theta[c] = (float)(enc_value[c])/(float)(8400*2*PI);
joostbonekamp 24:710d7d99b915 232
joostbonekamp 19:a37cae6964ca 233 }
joostbonekamp 12:88cbc65f2563 234 }
joostbonekamp 10:b8c60fd468f1 235
joostbonekamp 23:9eeac9d1ecbe 236 void motor_controller() {
joostbonekamp 24:710d7d99b915 237 int speed[2];
joostbonekamp 24:710d7d99b915 238 for(int c = 0; c<2; c++) {
joostbonekamp 24:710d7d99b915 239 speed[c] = schmitt_trigger(EMG_filtered[c]);
joostbonekamp 24:710d7d99b915 240 if (speed[c] == -1) {speed[c] = past_speed[c];}
joostbonekamp 24:710d7d99b915 241 past_speed[c] = speed[c];
joostbonekamp 24:710d7d99b915 242 }
joostbonekamp 17:615c5d8b3710 243
joostbonekamp 24:710d7d99b915 244 float errors[2];
joostbonekamp 24:710d7d99b915 245 float P_action[2];
joostbonekamp 24:710d7d99b915 246 float I_action[2];
joostbonekamp 24:710d7d99b915 247 float D_action[2];
joostbonekamp 24:710d7d99b915 248 float velocity_estimate[2];
joostbonekamp 24:710d7d99b915 249 for (int c = 0; c<2; c++) {
joostbonekamp 24:710d7d99b915 250
joostbonekamp 24:710d7d99b915 251 //P action
joostbonekamp 24:710d7d99b915 252 P_action[c] = PID.P[c] * errors[c];
joostbonekamp 24:710d7d99b915 253
joostbonekamp 24:710d7d99b915 254 //I part
joostbonekamp 24:710d7d99b915 255 PID.I_counter[c] += errors[c];
joostbonekamp 24:710d7d99b915 256 I_action[c] = PID.I_counter[c] * PID.I[c];
joostbonekamp 18:dddc8d9f7638 257
joostbonekamp 24:710d7d99b915 258 //D part
joostbonekamp 24:710d7d99b915 259 velocity_estimate[c] = (errors[c]-last_error[c])/dt; //estimate of the time derivative of the error
joostbonekamp 24:710d7d99b915 260 D_action[c] = velocity_estimate[c] * PID.D[c];
joostbonekamp 18:dddc8d9f7638 261
joostbonekamp 24:710d7d99b915 262 action[c] = P_action[c] + I_action[c] + D_action[c];
joostbonekamp 24:710d7d99b915 263 last_error[c] = errors[c];
joostbonekamp 24:710d7d99b915 264 }
joostbonekamp 17:615c5d8b3710 265 }
joostbonekamp 17:615c5d8b3710 266
joostbonekamp 15:9a1f34bc9958 267 void output()
joostbonekamp 14:4cf17b10e504 268 {
joostbonekamp 24:710d7d99b915 269 motor0_direction = actuator.direction[0];
joostbonekamp 24:710d7d99b915 270 motor1_direction = actuator.direction[1];
joostbonekamp 24:710d7d99b915 271 motor0_pwm.write(actuator.duty_cycle[0]);
joostbonekamp 24:710d7d99b915 272 motor1_pwm.write(actuator.duty_cycle[1]);
joostbonekamp 20:4424d447f3cd 273
joostbonekamp 24:710d7d99b915 274 scope.set(0, EMG_filtered[0]);
joostbonekamp 24:710d7d99b915 275 scope.set(1, past_speed[0]);
joostbonekamp 24:710d7d99b915 276 scope.set(2, EMG_filtered[1]);
joostbonekamp 24:710d7d99b915 277 scope.set(3, past_speed[1]);
joostbonekamp 15:9a1f34bc9958 278 }
joostbonekamp 14:4cf17b10e504 279
joostbonekamp 15:9a1f34bc9958 280 void state_machine()
joostbonekamp 15:9a1f34bc9958 281 {
joostbonekamp 16:696e9cbcc823 282 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 283 //run current state
joostbonekamp 17:615c5d8b3710 284 switch (state) {
joostbonekamp 17:615c5d8b3710 285 case s_idle:
joostbonekamp 15:9a1f34bc9958 286 do_nothing();
joostbonekamp 15:9a1f34bc9958 287 break;
joostbonekamp 17:615c5d8b3710 288 case s_failure:
joostbonekamp 15:9a1f34bc9958 289 failure();
joostbonekamp 15:9a1f34bc9958 290 break;
joostbonekamp 17:615c5d8b3710 291 case s_cali_EMG:
joostbonekamp 15:9a1f34bc9958 292 cali_EMG();
joostbonekamp 15:9a1f34bc9958 293 break;
joostbonekamp 17:615c5d8b3710 294 case s_cali_enc:
joostbonekamp 17:615c5d8b3710 295 cali_enc();
joostbonekamp 15:9a1f34bc9958 296 break;
joostbonekamp 17:615c5d8b3710 297 case s_moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 298 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 299 break;
joostbonekamp 17:615c5d8b3710 300 case s_moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 301 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 302 break;
joostbonekamp 17:615c5d8b3710 303 case s_homing:
joostbonekamp 15:9a1f34bc9958 304 homing();
joostbonekamp 15:9a1f34bc9958 305 break;
joostbonekamp 5:aa8b5d5e632f 306 }
joostbonekamp 5:aa8b5d5e632f 307 }
joostbonekamp 15:9a1f34bc9958 308
joostbonekamp 15:9a1f34bc9958 309 void main_loop()
joostbonekamp 12:88cbc65f2563 310 {
joostbonekamp 15:9a1f34bc9958 311 measure_signals();
joostbonekamp 15:9a1f34bc9958 312 state_machine();
joostbonekamp 15:9a1f34bc9958 313 motor_controller();
joostbonekamp 15:9a1f34bc9958 314 output();
joostbonekamp 15:9a1f34bc9958 315 }
joostbonekamp 14:4cf17b10e504 316
joostbonekamp 14:4cf17b10e504 317 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 318 //state machines
joostbonekamp 16:696e9cbcc823 319 void check_failure()
joostbonekamp 15:9a1f34bc9958 320 {
joostbonekamp 24:710d7d99b915 321 //if (failure_occurred) {
joostbonekamp 24:710d7d99b915 322 // state = s_failure;
joostbonekamp 24:710d7d99b915 323 // state_changed = true;
joostbonekamp 24:710d7d99b915 324 //}
joostbonekamp 16:696e9cbcc823 325 }
joostbonekamp 16:696e9cbcc823 326
joostbonekamp 16:696e9cbcc823 327 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 328 {
joostbonekamp 17:615c5d8b3710 329 if(but2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 330 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 331 }
joostbonekamp 23:9eeac9d1ecbe 332 button1_pressed = true;
joostbonekamp 15:9a1f34bc9958 333 pc.printf("Button 1 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 334 }
joostbonekamp 14:4cf17b10e504 335
joostbonekamp 16:696e9cbcc823 336 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 337 {
joostbonekamp 17:615c5d8b3710 338 if(but1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 339 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 340 }
joostbonekamp 23:9eeac9d1ecbe 341 button2_pressed = true;
joostbonekamp 15:9a1f34bc9958 342 pc.printf("Button 2 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 343 }
joostbonekamp 17:615c5d8b3710 344
joostbonekamp 16:696e9cbcc823 345 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 346 {
joostbonekamp 17:615c5d8b3710 347 int speed;
joostbonekamp 16:696e9cbcc823 348 speed = -1; //default value, this means the state should not change
joostbonekamp 23:9eeac9d1ecbe 349 if (i > 0.000 && i < 0.125) {speed = 0;}
joostbonekamp 23:9eeac9d1ecbe 350 if (i > 0.250 && i < 0.375) {speed = 1;}
joostbonekamp 23:9eeac9d1ecbe 351 if (i > 0.500 && i < 1.000) {speed = 2;}
joostbonekamp 16:696e9cbcc823 352 return speed;
joostbonekamp 16:696e9cbcc823 353 }
joostbonekamp 14:4cf17b10e504 354
joostbonekamp 15:9a1f34bc9958 355 int main()
joostbonekamp 15:9a1f34bc9958 356 {
joostbonekamp 15:9a1f34bc9958 357 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 358 pc.printf("Executing main()... \r\n");
joostbonekamp 17:615c5d8b3710 359 state = s_idle;
joostbonekamp 12:88cbc65f2563 360
joostbonekamp 24:710d7d99b915 361 motor0_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 17:615c5d8b3710 362 motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 363
joostbonekamp 24:710d7d99b915 364 actuator.direction[0] = 0;
joostbonekamp 24:710d7d99b915 365 actuator.direction[1] = 0;
joostbonekamp 24:710d7d99b915 366
joostbonekamp 24:710d7d99b915 367
joostbonekamp 24:710d7d99b915 368 PID.I_counter[0] = 0.0;
joostbonekamp 24:710d7d99b915 369 PID.I_counter[1] = 0.0;
joostbonekamp 24:710d7d99b915 370
joostbonekamp 15:9a1f34bc9958 371 actuator.magnet = false;
joostbonekamp 24:710d7d99b915 372 EMG.max[0] = 0.01;
joostbonekamp 24:710d7d99b915 373 EMG.max[1] = 0.01;
joostbonekamp 24:710d7d99b915 374
joostbonekamp 15:9a1f34bc9958 375 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 376 but2.fall(&but2_interrupt);
joostbonekamp 20:4424d447f3cd 377 scope_ticker.attach(&scope, &HIDScope::send, 0.02);
joostbonekamp 18:dddc8d9f7638 378 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 379 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 380 while (true) {
joostbonekamp 16:696e9cbcc823 381 wait(0.1f);
joostbonekamp 16:696e9cbcc823 382 }
joostbonekamp 17:615c5d8b3710 383 }