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@18:dddc8d9f7638, 2019-10-15 (annotated)

Committer:
joostbonekamp
Date:
Tue Oct 15 09:14:37 2019 +0000
Revision:
18:dddc8d9f7638
Parent:
17:615c5d8b3710
Child:
19:a37cae6964ca
added PID controller and todo list

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