Hidde van der Bijl / Mbed 2 deprecated biorobotics_four_scorers2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@23:9eeac9d1ecbe, 2019-10-22 (annotated)

Committer:
joostbonekamp
Date:
Tue Oct 22 12:06:33 2019 +0000
Revision:
23:9eeac9d1ecbe
Parent:
21:bea7c8a08e1d
Child:
24:7cad312a1e38
Child:
25:710d7d99b915
Child:
26:6af46ad1e4ea
3 stage schmitt trigger

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