Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@17:615c5d8b3710, 2019-10-11 (annotated)

Committer:
joostbonekamp
Date:
Fri Oct 11 09:51:35 2019 +0000
Revision:
17:615c5d8b3710
Parent:
16:696e9cbcc823
Child:
18:dddc8d9f7638
Added EMG interface and filtering, using HIDScope to display the result. Removed all errors, code actually compiles now.

Who changed what in which revision?

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