Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@21:bea7c8a08e1d, 2019-10-18 (annotated)

Committer:
joostbonekamp
Date:
Fri Oct 18 09:05:58 2019 +0000
Revision:
21:bea7c8a08e1d
Parent:
20:4424d447f3cd
Child:
23:9eeac9d1ecbe
removed failure mode

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 */
joostbonekamp 18:dddc8d9f7638 13
RobertoO 0:67c50348f842 14 #include "mbed.h"
RobertoO 1:b862262a9d14 15 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 16 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 17 #include "QEI.h"
joostbonekamp 17:615c5d8b3710 18 #include "HIDScope.h"
joostbonekamp 17:615c5d8b3710 19 #include "BiQuad.h"
hidde1104 13:51ae2da8da55 20 #define PI 3.14159265
RobertoO 0:67c50348f842 21
joostbonekamp 12:88cbc65f2563 22 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 20:4424d447f3cd 23 HIDScope scope(1); //HIDScope instance
joostbonekamp 12:88cbc65f2563 24 DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 12:88cbc65f2563 25 FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 12:88cbc65f2563 26 DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 12:88cbc65f2563 27 FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 28 Ticker loop_ticker; //used in main()
joostbonekamp 20:4424d447f3cd 29 Ticker scope_ticker;
joostbonekamp 12:88cbc65f2563 30 AnalogIn Pot1(A1); //pot 1 on biorobotics shield
joostbonekamp 12:88cbc65f2563 31 AnalogIn Pot2(A0); //pot 2 on biorobotics shield
joostbonekamp 12:88cbc65f2563 32 InterruptIn but1(D10); //debounced button on biorobotics shield
joostbonekamp 12:88cbc65f2563 33 InterruptIn but2(D9); //debounced button on biorobotics shield
joostbonekamp 16:696e9cbcc823 34 AnalogIn EMG1(A2);
joostbonekamp 16:696e9cbcc823 35 AnalogIn EMG2(A3);
joostbonekamp 12:88cbc65f2563 36
joostbonekamp 17:615c5d8b3710 37 void check_failure();
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 17:615c5d8b3710 42 BiQuad bq1 (0.881889334678067, -1.76377866935613, 0.8818893346780671, -1.77069673005903, 0.797707797506027);
joostbonekamp 17:615c5d8b3710 43 BiQuad bq2 (0.000198358203463849, 0.000396716406927699, 0.000198358203463849, -1.96262073248799, 0.963423352820821);
joostbonekamp 3:e3d12393adb1 44
joostbonekamp 5:aa8b5d5e632f 45 //variables
joostbonekamp 17:615c5d8b3710 46 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 47 States state; //using the States enum
joostbonekamp 14:4cf17b10e504 48 struct actuator_state {
joostbonekamp 12:88cbc65f2563 49 float duty_cycle1; //pwm of 1st motor
joostbonekamp 12:88cbc65f2563 50 float duty_cycle2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 51 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 52 int dir2; //direction of 2nd 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 16:696e9cbcc823 57 float max; //params of the emg, tbd during calibration
joostbonekamp 17:615c5d8b3710 58 float min;
joostbonekamp 16:696e9cbcc823 59 } EMG_values;
joostbonekamp 5:aa8b5d5e632f 60
joostbonekamp 18:dddc8d9f7638 61 struct PID {
joostbonekamp 18:dddc8d9f7638 62 float P;
joostbonekamp 18:dddc8d9f7638 63 float I;
joostbonekamp 18:dddc8d9f7638 64 float D;
joostbonekamp 18:dddc8d9f7638 65 float I_counter;
joostbonekamp 19:a37cae6964ca 66 };
joostbonekamp 19:a37cae6964ca 67 PID PID1;
joostbonekamp 19:a37cae6964ca 68 PID PID2;
joostbonekamp 18:dddc8d9f7638 69
joostbonekamp 18:dddc8d9f7638 70 float dt = 0.001;
joostbonekamp 19:a37cae6964ca 71 float theta;
joostbonekamp 19:a37cae6964ca 72 float L;
joostbonekamp 20:4424d447f3cd 73 int enc1_zero = 0;//the zero position of the encoders, to be determined from the
joostbonekamp 20:4424d447f3cd 74 int enc2_zero = 0;//encoder calibration
joostbonekamp 16:696e9cbcc823 75 int EMG1_filtered;
joostbonekamp 16:696e9cbcc823 76 int EMG2_filtered;
joostbonekamp 16:696e9cbcc823 77 int enc1_value;
joostbonekamp 16:696e9cbcc823 78 int enc2_value;
joostbonekamp 18:dddc8d9f7638 79 float error1 = 0.0;
joostbonekamp 18:dddc8d9f7638 80 float error2 = 0.0;
joostbonekamp 19:a37cae6964ca 81 float last_error1 = 0.0;
joostbonekamp 19:a37cae6964ca 82 float last_error2 = 0.0;
joostbonekamp 19:a37cae6964ca 83 float action1;
joostbonekamp 19:a37cae6964ca 84 float action2;
joostbonekamp 12:88cbc65f2563 85 bool state_changed = false; //used to see if the state is "starting"
PatrickZieverink 8:6f6a4dc12036 86 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 87 volatile bool but2_pressed = false;
joostbonekamp 16:696e9cbcc823 88 volatile bool failure_occurred = false;
joostbonekamp 12:88cbc65f2563 89 float pot_1; //used to keep track of the potentiometer values
joostbonekamp 10:b8c60fd468f1 90 float pot_2;
joostbonekamp 16:696e9cbcc823 91 bool EMG_has_been_calibrated;
joostbonekamp 17:615c5d8b3710 92 bool button1_pressed;
joostbonekamp 17:615c5d8b3710 93 bool button2_pressed;
joostbonekamp 19:a37cae6964ca 94 const int EMG_cali_amount = 1000;
joostbonekamp 19:a37cae6964ca 95 float past_EMG_values[EMG_cali_amount];
joostbonekamp 19:a37cae6964ca 96 int past_EMG_count = 0;
joostbonekamp 12:88cbc65f2563 97
joostbonekamp 12:88cbc65f2563 98 void do_nothing()
joostbonekamp 5:aa8b5d5e632f 99
PatrickZieverink 9:6537eead1241 100 /*
joostbonekamp 12:88cbc65f2563 101 Idle state. Used in the beginning, before the calibration states.
joostbonekamp 12:88cbc65f2563 102 */
joostbonekamp 16:696e9cbcc823 103 {
joostbonekamp 16:696e9cbcc823 104 if (button1_pressed) {
joostbonekamp 16:696e9cbcc823 105 state_changed = true;
joostbonekamp 17:615c5d8b3710 106 state = s_cali_enc;
joostbonekamp 16:696e9cbcc823 107 button1_pressed = false;
joostbonekamp 16:696e9cbcc823 108 }
joostbonekamp 16:696e9cbcc823 109 }
joostbonekamp 12:88cbc65f2563 110
joostbonekamp 12:88cbc65f2563 111 void failure()
joostbonekamp 12:88cbc65f2563 112 /*
joostbonekamp 12:88cbc65f2563 113 Failure mode. This should execute when button 2 is pressed during operation.
joostbonekamp 12:88cbc65f2563 114 */
joostbonekamp 12:88cbc65f2563 115 {
joostbonekamp 12:88cbc65f2563 116 if (state_changed) {
joostbonekamp 12:88cbc65f2563 117 pc.printf("Something went wrong!\r\n");
joostbonekamp 12:88cbc65f2563 118 state_changed = false;
joostbonekamp 12:88cbc65f2563 119 }
joostbonekamp 12:88cbc65f2563 120 }
PatrickZieverink 9:6537eead1241 121
joostbonekamp 12:88cbc65f2563 122 void cali_EMG()
joostbonekamp 12:88cbc65f2563 123 /*
joostbonekamp 16:696e9cbcc823 124 Calibration of the EMG. Values determined during calibration should be
joostbonekamp 12:88cbc65f2563 125 added to the EMG_params instance.
joostbonekamp 12:88cbc65f2563 126 */
joostbonekamp 12:88cbc65f2563 127 {
joostbonekamp 12:88cbc65f2563 128 if (state_changed) {
joostbonekamp 12:88cbc65f2563 129 pc.printf("Started EMG calibration\r\n");
joostbonekamp 12:88cbc65f2563 130 state_changed = false;
PatrickZieverink 9:6537eead1241 131 }
joostbonekamp 19:a37cae6964ca 132 if (past_EMG_count < EMG_cali_amount) {
joostbonekamp 19:a37cae6964ca 133 past_EMG_values[past_EMG_count] = EMG1_filtered;
joostbonekamp 19:a37cae6964ca 134 past_EMG_count++;
joostbonekamp 19:a37cae6964ca 135 }
joostbonekamp 19:a37cae6964ca 136 else { //calibration is has concluded
joostbonekamp 19:a37cae6964ca 137 float sum = 0.0;
joostbonekamp 19:a37cae6964ca 138 for(int i = 0; i<EMG_cali_amount; i++) {
joostbonekamp 19:a37cae6964ca 139 sum += past_EMG_values[i];
joostbonekamp 19:a37cae6964ca 140 }
joostbonekamp 19:a37cae6964ca 141 float mean = sum/(float)EMG_cali_amount;
joostbonekamp 19:a37cae6964ca 142 EMG_values.min = mean;
joostbonekamp 19:a37cae6964ca 143 //calibration done, moving to cali_enc
joostbonekamp 19:a37cae6964ca 144 pc.printf("Calibration of the EMG is done, lower bound = %f", mean);
joostbonekamp 19:a37cae6964ca 145 EMG_has_been_calibrated = true;
joostbonekamp 19:a37cae6964ca 146 state_changed = true;
joostbonekamp 19:a37cae6964ca 147 state = s_cali_enc;
joostbonekamp 19:a37cae6964ca 148 }
joostbonekamp 12:88cbc65f2563 149 }
joostbonekamp 16:696e9cbcc823 150
joostbonekamp 12:88cbc65f2563 151 void cali_enc()
joostbonekamp 12:88cbc65f2563 152 /*
joostbonekamp 14:4cf17b10e504 153 Calibration of the encoder. The encoder should be moved to the lowest
joostbonekamp 19:a37cae6964ca 154 position for the linear stage and the horizontal postition for the
joostbonekamp 12:88cbc65f2563 155 rotating stage.
joostbonekamp 12:88cbc65f2563 156 */
joostbonekamp 12:88cbc65f2563 157 {
joostbonekamp 12:88cbc65f2563 158 if (state_changed) {
joostbonekamp 12:88cbc65f2563 159 pc.printf("Started encoder calibration\r\n");
joostbonekamp 12:88cbc65f2563 160 state_changed = false;
PatrickZieverink 9:6537eead1241 161 }
joostbonekamp 16:696e9cbcc823 162 if (button1_pressed) {
joostbonekamp 19:a37cae6964ca 163 pc.printf("Encoder has been calibrated");
joostbonekamp 16:696e9cbcc823 164 enc1_zero = enc1_value;
joostbonekamp 16:696e9cbcc823 165 enc2_zero = enc2_value;
joostbonekamp 16:696e9cbcc823 166 button1_pressed = false;
joostbonekamp 17:615c5d8b3710 167 state = s_moving_magnet_off;
joostbonekamp 16:696e9cbcc823 168 state_changed = true;
joostbonekamp 16:696e9cbcc823 169
joostbonekamp 16:696e9cbcc823 170 }
joostbonekamp 12:88cbc65f2563 171 }
joostbonekamp 16:696e9cbcc823 172
joostbonekamp 12:88cbc65f2563 173 void moving_magnet_off()
joostbonekamp 12:88cbc65f2563 174 /*
joostbonekamp 14:4cf17b10e504 175 Moving with the magnet disabled. This is the part from the home position
joostbonekamp 12:88cbc65f2563 176 towards the storage of chips.
joostbonekamp 12:88cbc65f2563 177 */
joostbonekamp 12:88cbc65f2563 178 {
joostbonekamp 12:88cbc65f2563 179 if (state_changed) {
joostbonekamp 12:88cbc65f2563 180 pc.printf("Moving without magnet\r\n");
joostbonekamp 12:88cbc65f2563 181 state_changed = false;
PatrickZieverink 9:6537eead1241 182 }
PatrickZieverink 9:6537eead1241 183 }
joostbonekamp 16:696e9cbcc823 184
joostbonekamp 12:88cbc65f2563 185 void moving_magnet_on()
joostbonekamp 12:88cbc65f2563 186 /*
joostbonekamp 14:4cf17b10e504 187 Moving with the magnet enabled. This is the part of the movement from the
joostbonekamp 12:88cbc65f2563 188 chip holder to the top of the playing board.
joostbonekamp 12:88cbc65f2563 189 */
joostbonekamp 12:88cbc65f2563 190 {
joostbonekamp 12:88cbc65f2563 191 if (state_changed) {
joostbonekamp 12:88cbc65f2563 192 pc.printf("Moving with magnet\r\n");
joostbonekamp 12:88cbc65f2563 193 state_changed = false;
joostbonekamp 12:88cbc65f2563 194 }
joostbonekamp 12:88cbc65f2563 195 return;
PatrickZieverink 9:6537eead1241 196 }
joostbonekamp 12:88cbc65f2563 197 void homing()
joostbonekamp 12:88cbc65f2563 198 /*
joostbonekamp 14:4cf17b10e504 199 Dropping the chip and moving towards the rest position.
PatrickZieverink 9:6537eead1241 200 */
joostbonekamp 12:88cbc65f2563 201 {
joostbonekamp 12:88cbc65f2563 202 if (state_changed) {
joostbonekamp 12:88cbc65f2563 203 pc.printf("Started homing");
joostbonekamp 12:88cbc65f2563 204 state_changed = false;
joostbonekamp 12:88cbc65f2563 205 }
joostbonekamp 12:88cbc65f2563 206 return;
joostbonekamp 12:88cbc65f2563 207 }
PatrickZieverink 9:6537eead1241 208
joostbonekamp 12:88cbc65f2563 209 void measure_signals()
joostbonekamp 12:88cbc65f2563 210 {
joostbonekamp 19:a37cae6964ca 211 //only one emg input, reference and plus
joostbonekamp 19:a37cae6964ca 212 float EMG1_raw = EMG1.read();
joostbonekamp 19:a37cae6964ca 213 float EMG2_raw = EMG2.read();
joostbonekamp 19:a37cae6964ca 214 float filter_value1 = fabs(bq2.step(fabs(bq1.step(EMG1_raw - EMG2_raw))));
joostbonekamp 19:a37cae6964ca 215
joostbonekamp 19:a37cae6964ca 216 if (filter_value1 > EMG_values.max) {
joostbonekamp 19:a37cae6964ca 217 EMG_values.max = filter_value1;
joostbonekamp 19:a37cae6964ca 218 }
joostbonekamp 19:a37cae6964ca 219 if (EMG_has_been_calibrated) {
joostbonekamp 19:a37cae6964ca 220 EMG1_filtered = (filter_value1-EMG_values.min)/(EMG_values.max-EMG_values.min);
joostbonekamp 19:a37cae6964ca 221 }
joostbonekamp 19:a37cae6964ca 222 else {
joostbonekamp 19:a37cae6964ca 223 EMG1_filtered = filter_value1;
joostbonekamp 19:a37cae6964ca 224 }
joostbonekamp 19:a37cae6964ca 225
joostbonekamp 16:696e9cbcc823 226 enc1_value = enc1.getPulses();
joostbonekamp 16:696e9cbcc823 227 enc2_value = enc2.getPulses();
joostbonekamp 19:a37cae6964ca 228 enc1_value -= enc1_zero;
joostbonekamp 19:a37cae6964ca 229 enc2_value -= enc2_zero;
joostbonekamp 19:a37cae6964ca 230 theta = (float)(enc1_value)/(float)(8400*2*PI);
joostbonekamp 19:a37cae6964ca 231 L = (float)(enc2_value)/(float)(8400*2*PI);
joostbonekamp 19:a37cae6964ca 232
joostbonekamp 19:a37cae6964ca 233
joostbonekamp 12:88cbc65f2563 234 }
joostbonekamp 10:b8c60fd468f1 235
joostbonekamp 17:615c5d8b3710 236 void sample()
joostbonekamp 17:615c5d8b3710 237 {
joostbonekamp 17:615c5d8b3710 238 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
joostbonekamp 17:615c5d8b3710 239 float emg0_value = EMG1.read();
joostbonekamp 17:615c5d8b3710 240 float emg1_value = EMG2.read();
joostbonekamp 17:615c5d8b3710 241
joostbonekamp 17:615c5d8b3710 242 //double filter_value = bqc.step(emg1_value);
joostbonekamp 17:615c5d8b3710 243 float filter_value = fabs(bq2.step(fabs(bq1.step(emg0_value - emg1_value))));
joostbonekamp 17:615c5d8b3710 244 if (filter_value > EMG_values.max) {
joostbonekamp 17:615c5d8b3710 245 EMG_values.max = filter_value;
joostbonekamp 17:615c5d8b3710 246 }
joostbonekamp 17:615c5d8b3710 247 if (EMG_values.min > filter_value) {
joostbonekamp 17:615c5d8b3710 248 EMG_values.min = filter_value;
joostbonekamp 17:615c5d8b3710 249 }
joostbonekamp 17:615c5d8b3710 250
joostbonekamp 17:615c5d8b3710 251 filter_value = filter_value-EMG_values.min;
joostbonekamp 17:615c5d8b3710 252 filter_value = filter_value/(EMG_values.max-EMG_values.min);
joostbonekamp 17:615c5d8b3710 253
joostbonekamp 17:615c5d8b3710 254 scope.set(0, EMG1.read() );
joostbonekamp 17:615c5d8b3710 255 scope.set(1, EMG2.read() );
joostbonekamp 17:615c5d8b3710 256 scope.set(2, filter_value);
joostbonekamp 17:615c5d8b3710 257 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
joostbonekamp 17:615c5d8b3710 258 * Ensure that enough channels are available (HIDScope scope( 2 ))
joostbonekamp 17:615c5d8b3710 259 * Finally, send all channels to the PC at once */
joostbonekamp 17:615c5d8b3710 260 scope.send();
joostbonekamp 17:615c5d8b3710 261 /* To indicate that the function is working, the LED is toggled */
joostbonekamp 17:615c5d8b3710 262 }
joostbonekamp 17:615c5d8b3710 263
joostbonekamp 17:615c5d8b3710 264 void motor_controller() {
joostbonekamp 18:dddc8d9f7638 265 float error1, error2;
joostbonekamp 18:dddc8d9f7638 266 //P part of the controller
joostbonekamp 18:dddc8d9f7638 267 float P_action1 = PID1.P * error1;
joostbonekamp 18:dddc8d9f7638 268 float P_action2 = PID2.P * error2;
joostbonekamp 17:615c5d8b3710 269
joostbonekamp 18:dddc8d9f7638 270 //I part
joostbonekamp 18:dddc8d9f7638 271 PID1.I_counter += error1;
joostbonekamp 18:dddc8d9f7638 272 PID2.I_counter += error2;
joostbonekamp 18:dddc8d9f7638 273 float I_action1 = PID1.I_counter * PID1.I;
joostbonekamp 18:dddc8d9f7638 274 float I_action2 = PID2.I_counter * PID2.I;
joostbonekamp 18:dddc8d9f7638 275
joostbonekamp 18:dddc8d9f7638 276 //D part
joostbonekamp 18:dddc8d9f7638 277 float velocity_estimate_1 = (error1-last_error1)/dt; //estimate of the time derivative of the error
joostbonekamp 18:dddc8d9f7638 278 float velocity_estimate_2 = (error2-last_error2)/dt;
joostbonekamp 18:dddc8d9f7638 279 float D_action1 = velocity_estimate_1 * PID1.D;
joostbonekamp 18:dddc8d9f7638 280 float D_action2 = velocity_estimate_2 * PID2.D;
joostbonekamp 18:dddc8d9f7638 281
joostbonekamp 18:dddc8d9f7638 282 action1 = P_action1 + I_action1 + D_action1;
joostbonekamp 18:dddc8d9f7638 283 action2 = P_action2 + I_action2 + D_action2;
joostbonekamp 18:dddc8d9f7638 284
joostbonekamp 18:dddc8d9f7638 285 last_error1 = error1;
joostbonekamp 18:dddc8d9f7638 286 last_error2 = error2;
joostbonekamp 17:615c5d8b3710 287 }
joostbonekamp 17:615c5d8b3710 288
joostbonekamp 15:9a1f34bc9958 289 void output()
joostbonekamp 14:4cf17b10e504 290 {
joostbonekamp 16:696e9cbcc823 291 motor1_direction = actuator.dir1;
joostbonekamp 17:615c5d8b3710 292 motor2_direction = actuator.dir2;
joostbonekamp 17:615c5d8b3710 293 motor1_pwm.write(actuator.duty_cycle1);
joostbonekamp 17:615c5d8b3710 294 motor2_pwm.write(actuator.duty_cycle2);
joostbonekamp 20:4424d447f3cd 295
joostbonekamp 20:4424d447f3cd 296 scope.set(0, EMG1_filtered);
joostbonekamp 15:9a1f34bc9958 297 }
joostbonekamp 14:4cf17b10e504 298
joostbonekamp 15:9a1f34bc9958 299 void state_machine()
joostbonekamp 15:9a1f34bc9958 300 {
joostbonekamp 16:696e9cbcc823 301 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 302 //run current state
joostbonekamp 17:615c5d8b3710 303 switch (state) {
joostbonekamp 17:615c5d8b3710 304 case s_idle:
joostbonekamp 15:9a1f34bc9958 305 do_nothing();
joostbonekamp 15:9a1f34bc9958 306 break;
joostbonekamp 17:615c5d8b3710 307 case s_failure:
joostbonekamp 15:9a1f34bc9958 308 failure();
joostbonekamp 15:9a1f34bc9958 309 break;
joostbonekamp 17:615c5d8b3710 310 case s_cali_EMG:
joostbonekamp 15:9a1f34bc9958 311 cali_EMG();
joostbonekamp 15:9a1f34bc9958 312 break;
joostbonekamp 17:615c5d8b3710 313 case s_cali_enc:
joostbonekamp 17:615c5d8b3710 314 cali_enc();
joostbonekamp 15:9a1f34bc9958 315 break;
joostbonekamp 17:615c5d8b3710 316 case s_moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 317 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 318 break;
joostbonekamp 17:615c5d8b3710 319 case s_moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 320 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 321 break;
joostbonekamp 17:615c5d8b3710 322 case s_homing:
joostbonekamp 15:9a1f34bc9958 323 homing();
joostbonekamp 15:9a1f34bc9958 324 break;
joostbonekamp 5:aa8b5d5e632f 325 }
joostbonekamp 5:aa8b5d5e632f 326 }
joostbonekamp 15:9a1f34bc9958 327
joostbonekamp 15:9a1f34bc9958 328 void main_loop()
joostbonekamp 12:88cbc65f2563 329 {
joostbonekamp 15:9a1f34bc9958 330 measure_signals();
joostbonekamp 15:9a1f34bc9958 331 state_machine();
joostbonekamp 15:9a1f34bc9958 332 motor_controller();
joostbonekamp 15:9a1f34bc9958 333 output();
joostbonekamp 15:9a1f34bc9958 334 }
joostbonekamp 14:4cf17b10e504 335
joostbonekamp 14:4cf17b10e504 336 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 337 //state machines
joostbonekamp 16:696e9cbcc823 338 void check_failure()
joostbonekamp 15:9a1f34bc9958 339 {
joostbonekamp 21:bea7c8a08e1d 340 //state = s_failure;
joostbonekamp 21:bea7c8a08e1d 341 //state_changed = true;
joostbonekamp 16:696e9cbcc823 342 }
joostbonekamp 16:696e9cbcc823 343
joostbonekamp 16:696e9cbcc823 344 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 345 {
joostbonekamp 17:615c5d8b3710 346 if(but2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 347 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 348 }
joostbonekamp 15:9a1f34bc9958 349 but1_pressed = true;
joostbonekamp 15:9a1f34bc9958 350 pc.printf("Button 1 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 351 }
joostbonekamp 14:4cf17b10e504 352
joostbonekamp 16:696e9cbcc823 353 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 354 {
joostbonekamp 17:615c5d8b3710 355 if(but1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 356 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 357 }
joostbonekamp 15:9a1f34bc9958 358 but2_pressed = true;
joostbonekamp 15:9a1f34bc9958 359 pc.printf("Button 2 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 360 }
joostbonekamp 17:615c5d8b3710 361
joostbonekamp 16:696e9cbcc823 362 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 363 {
joostbonekamp 17:615c5d8b3710 364 int speed;
joostbonekamp 16:696e9cbcc823 365 speed = -1; //default value, this means the state should not change
joostbonekamp 16:696e9cbcc823 366 if (i > 0/14 && i < 2/14) {speed = 0;}
joostbonekamp 16:696e9cbcc823 367 if (i > 3/14 && i < 5/14) {speed = 1;}
joostbonekamp 17:615c5d8b3710 368 if (i > 6/14 && i < 8/14) {speed = 2;}
joostbonekamp 16:696e9cbcc823 369 if (i > 9/14 && i < 11/14) {speed = 3;}
joostbonekamp 16:696e9cbcc823 370 if (i > 12/14 && i < 14/14) {speed = 4;}
joostbonekamp 16:696e9cbcc823 371 return speed;
joostbonekamp 16:696e9cbcc823 372 }
joostbonekamp 14:4cf17b10e504 373
joostbonekamp 15:9a1f34bc9958 374 int main()
joostbonekamp 15:9a1f34bc9958 375 {
joostbonekamp 15:9a1f34bc9958 376 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 377 pc.printf("Executing main()... \r\n");
joostbonekamp 17:615c5d8b3710 378 state = s_idle;
joostbonekamp 12:88cbc65f2563 379
joostbonekamp 17:615c5d8b3710 380 motor2_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 17:615c5d8b3710 381 motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 382
joostbonekamp 15:9a1f34bc9958 383 actuator.dir1 = 0;
joostbonekamp 15:9a1f34bc9958 384 actuator.dir2 = 0;
joostbonekamp 15:9a1f34bc9958 385
joostbonekamp 15:9a1f34bc9958 386 actuator.magnet = false;
joostbonekamp 14:4cf17b10e504 387
joostbonekamp 15:9a1f34bc9958 388 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 389 but2.fall(&but2_interrupt);
joostbonekamp 20:4424d447f3cd 390 scope_ticker.attach(&scope, &HIDScope::send, 0.02);
joostbonekamp 18:dddc8d9f7638 391 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 392 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 393 while (true) {
joostbonekamp 16:696e9cbcc823 394 wait(0.1f);
joostbonekamp 16:696e9cbcc823 395 }
joostbonekamp 17:615c5d8b3710 396 }