Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@22:afd521069446, 2019-10-21 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 21 08:20:32 2019 +0000
Revision:
22:afd521069446
Parent:
1:b862262a9d14 
fixed versions for libraries

Who changed what in which revision?

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