Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@19:a37cae6964ca, 2019-10-17 (annotated)

Committer:
joostbonekamp
Date:
Thu Oct 17 11:01:37 2019 +0000
Revision:
19:a37cae6964ca
Parent:
18:dddc8d9f7638
Child:
20:4424d447f3cd
added emg filter and normalizing;

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 EMG normalizing
joostbonekamp 18:dddc8d9f7638 6 Homing
joostbonekamp 18:dddc8d9f7638 7 Turning the magnet on/off
joostbonekamp 18:dddc8d9f7638 8 Inverse kinematics
joostbonekamp 18:dddc8d9f7638 9 Gravity compensation
joostbonekamp 18:dddc8d9f7638 10 PID values
joostbonekamp 18:dddc8d9f7638 11 General program layout
joostbonekamp 19:a37cae6964ca 12 better names for EMG input
joostbonekamp 18:dddc8d9f7638 13 */
joostbonekamp 18:dddc8d9f7638 14
RobertoO 0:67c50348f842 15 #include "mbed.h"
RobertoO 1:b862262a9d14 16 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 17 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 18 #include "QEI.h"
joostbonekamp 17:615c5d8b3710 19 #include "HIDScope.h"
joostbonekamp 17:615c5d8b3710 20 #include "BiQuad.h"
hidde1104 13:51ae2da8da55 21 #define PI 3.14159265
RobertoO 0:67c50348f842 22
joostbonekamp 12:88cbc65f2563 23 Serial pc(USBTX, USBRX); //connect to pc
joostbonekamp 17:615c5d8b3710 24 HIDScope scope(3); //HIDScope instance
joostbonekamp 12:88cbc65f2563 25 DigitalOut motor1_direction(D4); //rotation motor 1 on shield (always D6)
joostbonekamp 12:88cbc65f2563 26 FastPWM motor1_pwm(D5); //pwm 1 on shield (always D7)
joostbonekamp 12:88cbc65f2563 27 DigitalOut motor2_direction(D7); //rotation motor 2 on shield (always D4)
joostbonekamp 12:88cbc65f2563 28 FastPWM motor2_pwm(D6); //pwm 2 on shield (always D5)
joostbonekamp 5:aa8b5d5e632f 29 Ticker loop_ticker; //used in main()
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 19:a37cae6964ca 73 int enc1_zero = 0; //the zero position of the encoders, to be determined from the
joostbonekamp 19:a37cae6964ca 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 15:9a1f34bc9958 295 }
joostbonekamp 14:4cf17b10e504 296
joostbonekamp 15:9a1f34bc9958 297 void state_machine()
joostbonekamp 15:9a1f34bc9958 298 {
joostbonekamp 16:696e9cbcc823 299 check_failure(); //check for an error in the last loop before state machine
joostbonekamp 15:9a1f34bc9958 300 //run current state
joostbonekamp 17:615c5d8b3710 301 switch (state) {
joostbonekamp 17:615c5d8b3710 302 case s_idle:
joostbonekamp 15:9a1f34bc9958 303 do_nothing();
joostbonekamp 15:9a1f34bc9958 304 break;
joostbonekamp 17:615c5d8b3710 305 case s_failure:
joostbonekamp 15:9a1f34bc9958 306 failure();
joostbonekamp 15:9a1f34bc9958 307 break;
joostbonekamp 17:615c5d8b3710 308 case s_cali_EMG:
joostbonekamp 15:9a1f34bc9958 309 cali_EMG();
joostbonekamp 15:9a1f34bc9958 310 break;
joostbonekamp 17:615c5d8b3710 311 case s_cali_enc:
joostbonekamp 17:615c5d8b3710 312 cali_enc();
joostbonekamp 15:9a1f34bc9958 313 break;
joostbonekamp 17:615c5d8b3710 314 case s_moving_magnet_on:
joostbonekamp 15:9a1f34bc9958 315 moving_magnet_on();
joostbonekamp 15:9a1f34bc9958 316 break;
joostbonekamp 17:615c5d8b3710 317 case s_moving_magnet_off:
joostbonekamp 15:9a1f34bc9958 318 moving_magnet_off();
joostbonekamp 15:9a1f34bc9958 319 break;
joostbonekamp 17:615c5d8b3710 320 case s_homing:
joostbonekamp 15:9a1f34bc9958 321 homing();
joostbonekamp 15:9a1f34bc9958 322 break;
joostbonekamp 5:aa8b5d5e632f 323 }
joostbonekamp 5:aa8b5d5e632f 324 }
joostbonekamp 15:9a1f34bc9958 325
joostbonekamp 15:9a1f34bc9958 326 void main_loop()
joostbonekamp 12:88cbc65f2563 327 {
joostbonekamp 15:9a1f34bc9958 328 measure_signals();
joostbonekamp 15:9a1f34bc9958 329 state_machine();
joostbonekamp 15:9a1f34bc9958 330 motor_controller();
joostbonekamp 15:9a1f34bc9958 331 output();
joostbonekamp 15:9a1f34bc9958 332 }
joostbonekamp 14:4cf17b10e504 333
joostbonekamp 14:4cf17b10e504 334 //Helper functions, not directly called by the main_loop functions or
joostbonekamp 14:4cf17b10e504 335 //state machines
joostbonekamp 16:696e9cbcc823 336 void check_failure()
joostbonekamp 15:9a1f34bc9958 337 {
joostbonekamp 17:615c5d8b3710 338 state = s_failure;
joostbonekamp 16:696e9cbcc823 339 state_changed = true;
joostbonekamp 16:696e9cbcc823 340 }
joostbonekamp 16:696e9cbcc823 341
joostbonekamp 16:696e9cbcc823 342 void but1_interrupt()
joostbonekamp 16:696e9cbcc823 343 {
joostbonekamp 17:615c5d8b3710 344 if(but2.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 345 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 346 }
joostbonekamp 15:9a1f34bc9958 347 but1_pressed = true;
joostbonekamp 15:9a1f34bc9958 348 pc.printf("Button 1 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 349 }
joostbonekamp 14:4cf17b10e504 350
joostbonekamp 16:696e9cbcc823 351 void but2_interrupt()
joostbonekamp 15:9a1f34bc9958 352 {
joostbonekamp 17:615c5d8b3710 353 if(but1.read()) {//both buttons are pressed
joostbonekamp 16:696e9cbcc823 354 failure_occurred = true;
joostbonekamp 16:696e9cbcc823 355 }
joostbonekamp 15:9a1f34bc9958 356 but2_pressed = true;
joostbonekamp 15:9a1f34bc9958 357 pc.printf("Button 2 pressed \n\r");
joostbonekamp 15:9a1f34bc9958 358 }
joostbonekamp 17:615c5d8b3710 359
joostbonekamp 16:696e9cbcc823 360 int schmitt_trigger(float i)
joostbonekamp 16:696e9cbcc823 361 {
joostbonekamp 17:615c5d8b3710 362 int speed;
joostbonekamp 16:696e9cbcc823 363 speed = -1; //default value, this means the state should not change
joostbonekamp 16:696e9cbcc823 364 if (i > 0/14 && i < 2/14) {speed = 0;}
joostbonekamp 16:696e9cbcc823 365 if (i > 3/14 && i < 5/14) {speed = 1;}
joostbonekamp 17:615c5d8b3710 366 if (i > 6/14 && i < 8/14) {speed = 2;}
joostbonekamp 16:696e9cbcc823 367 if (i > 9/14 && i < 11/14) {speed = 3;}
joostbonekamp 16:696e9cbcc823 368 if (i > 12/14 && i < 14/14) {speed = 4;}
joostbonekamp 16:696e9cbcc823 369 return speed;
joostbonekamp 16:696e9cbcc823 370 }
joostbonekamp 14:4cf17b10e504 371
joostbonekamp 15:9a1f34bc9958 372 int main()
joostbonekamp 15:9a1f34bc9958 373 {
joostbonekamp 15:9a1f34bc9958 374 pc.baud(115200);
joostbonekamp 15:9a1f34bc9958 375 pc.printf("Executing main()... \r\n");
joostbonekamp 17:615c5d8b3710 376 state = s_idle;
joostbonekamp 12:88cbc65f2563 377
joostbonekamp 17:615c5d8b3710 378 motor2_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 17:615c5d8b3710 379 motor1_pwm.period(1/160000); // 1/frequency van waarop hij draait
joostbonekamp 14:4cf17b10e504 380
joostbonekamp 15:9a1f34bc9958 381 actuator.dir1 = 0;
joostbonekamp 15:9a1f34bc9958 382 actuator.dir2 = 0;
joostbonekamp 15:9a1f34bc9958 383
joostbonekamp 15:9a1f34bc9958 384 actuator.magnet = false;
joostbonekamp 14:4cf17b10e504 385
joostbonekamp 15:9a1f34bc9958 386 but1.fall(&but1_interrupt);
joostbonekamp 15:9a1f34bc9958 387 but2.fall(&but2_interrupt);
joostbonekamp 18:dddc8d9f7638 388 loop_ticker.attach(&main_loop, dt); //main loop at 1kHz
joostbonekamp 15:9a1f34bc9958 389 pc.printf("Main_loop is running\n\r");
joostbonekamp 16:696e9cbcc823 390 while (true) {
joostbonekamp 16:696e9cbcc823 391 wait(0.1f);
joostbonekamp 16:696e9cbcc823 392 }
joostbonekamp 17:615c5d8b3710 393 }