M9 Biorobotics Group 8 / Mbed 2 deprecated CARD_program

De hele robot in 1 keer bam

Dependencies:   mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@39:f9042483b921, 2019-10-30 (annotated)

Committer:
Jellehierck
Date:
Wed Oct 30 16:33:01 2019 +0000
Revision:
39:f9042483b921
Parent:
38:8b597ab8344f
Child:
40:c6dffb676350
EMG substate is fully implemented, needs testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jellehierck 37:806c7c8381a7 1 /*
Jellehierck 37:806c7c8381a7 2 ------------------------------ ADD LIBRARIES ------------------------------
Jellehierck 37:806c7c8381a7 3 */
Jellehierck 38:8b597ab8344f 4 #include "mbed.h" // Base library
Jellehierck 38:8b597ab8344f 5 #include "HIDScope.h" // Scope connection to PC
Jellehierck 38:8b597ab8344f 6 #include "MODSERIAL.h" // Serial connection to PC
Jellehierck 38:8b597ab8344f 7 #include "BiQuad.h" // Biquad filter management
Jellehierck 38:8b597ab8344f 8 #include <vector> // Array management
IsaRobin 0:6972d0e91af1 9
Jellehierck 15:421d3d9c563b 10 /*
Jellehierck 37:806c7c8381a7 11 ------------------------------ DEFINE MBED CONNECTIONS ------------------------------
Jellehierck 15:421d3d9c563b 12 */
IsaRobin 0:6972d0e91af1 13
Jellehierck 38:8b597ab8344f 14 // PC connections
Jellehierck 38:8b597ab8344f 15 HIDScope scope( 4 );
Jellehierck 15:421d3d9c563b 16 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 17
Jellehierck 8:ea3de43c9e8b 18 // Buttons
Jellehierck 8:ea3de43c9e8b 19 InterruptIn button1(D11);
Jellehierck 8:ea3de43c9e8b 20 InterruptIn button2(D10);
Jellehierck 37:806c7c8381a7 21 InterruptIn switch2(SW2);
Jellehierck 37:806c7c8381a7 22 InterruptIn switch3(SW3);
Jellehierck 4:09a01d2db8f7 23
Jellehierck 38:8b597ab8344f 24 // LEDs
Jellehierck 38:8b597ab8344f 25 DigitalOut led_g(LED_GREEN);
Jellehierck 38:8b597ab8344f 26 DigitalOut led_r(LED_RED);
Jellehierck 38:8b597ab8344f 27 DigitalOut led_b(LED_BLUE);
Jellehierck 38:8b597ab8344f 28
Jellehierck 38:8b597ab8344f 29 // Analog EMG inputs
Jellehierck 38:8b597ab8344f 30 AnalogIn emg1_in (A1); // Right biceps -> x axis
Jellehierck 38:8b597ab8344f 31 AnalogIn emg2_in (A2); // Left biceps -> y axis
Jellehierck 38:8b597ab8344f 32 AnalogIn emg3_in (A3); // Third muscle -> TBD
Jellehierck 38:8b597ab8344f 33
Jellehierck 15:421d3d9c563b 34 /*
Jellehierck 38:8b597ab8344f 35 ------------------------------ INITIALIZE TICKERS, TIMERS & TIMEOUTS ------------------------------
Jellehierck 38:8b597ab8344f 36 */
Jellehierck 38:8b597ab8344f 37 Ticker tickGlobal; // Set global ticker
Jellehierck 38:8b597ab8344f 38 Timer timerCalibration; // Set EMG Calibration timer
Jellehierck 38:8b597ab8344f 39
Jellehierck 38:8b597ab8344f 40 /*
Jellehierck 38:8b597ab8344f 41 ------------------------------ INITIALIZE GLOBAL VARIABLES ------------------------------
Jellehierck 15:421d3d9c563b 42 */
Jellehierck 15:421d3d9c563b 43
Jellehierck 37:806c7c8381a7 44 // State machine variables
Jellehierck 38:8b597ab8344f 45 enum GLOBAL_States { global_failure, global_wait, global_emg_cal, global_motor_cal, global_operation, global_demo }; // Define global states
Jellehierck 37:806c7c8381a7 46 GLOBAL_States global_curr_state = global_wait; // Initialize global state to waiting state
Jellehierck 37:806c7c8381a7 47 bool global_state_changed = true; // Enable entry functions
Jellehierck 37:806c7c8381a7 48 bool failure_mode = false;
Jellehierck 35:e82834e62e44 49
Jellehierck 38:8b597ab8344f 50 bool emg_cal_done = false;
Jellehierck 38:8b597ab8344f 51 bool motor_cal_done = false;
Jellehierck 38:8b597ab8344f 52
Jellehierck 38:8b597ab8344f 53 // EMG Substate variables
Jellehierck 38:8b597ab8344f 54 enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_operation }; // Define EMG substates
Jellehierck 38:8b597ab8344f 55 EMG_States emg_curr_state = emg_wait; // Initialize EMG substate variable
Jellehierck 38:8b597ab8344f 56 bool emg_state_changed = true;
Jellehierck 38:8b597ab8344f 57
Jellehierck 38:8b597ab8344f 58 bool emg_sampleNow = false;
Jellehierck 38:8b597ab8344f 59 bool emg_calibrateNow = false;
Jellehierck 38:8b597ab8344f 60 bool emg_MVC_cal_done = false;
Jellehierck 38:8b597ab8344f 61 bool emg_rest_cal_done = false;
Jellehierck 8:ea3de43c9e8b 62
Jellehierck 37:806c7c8381a7 63 // Button press interrupts (to prevent bounce)
Jellehierck 37:806c7c8381a7 64 bool button1_pressed = false;
Jellehierck 37:806c7c8381a7 65 bool button2_pressed = false;
Jellehierck 37:806c7c8381a7 66 bool switch2_pressed = false;
Jellehierck 7:7a088536f1c9 67
Jellehierck 38:8b597ab8344f 68 // Global constants
Jellehierck 38:8b597ab8344f 69 const double Fs = 500.0;
Jellehierck 38:8b597ab8344f 70 const double Ts = 1/Fs;
Jellehierck 35:e82834e62e44 71
Jellehierck 35:e82834e62e44 72 /*
Jellehierck 37:806c7c8381a7 73 ------------------------------ HELPER FUNCTIONS ------------------------------
Jellehierck 37:806c7c8381a7 74 */
Jellehierck 38:8b597ab8344f 75 // Empty placeholder function, needs to be deleted at end of project
Jellehierck 38:8b597ab8344f 76 void doStuff() {}
Jellehierck 38:8b597ab8344f 77
Jellehierck 38:8b597ab8344f 78 // Return max value of vector
Jellehierck 38:8b597ab8344f 79 double getMax(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 80 {
Jellehierck 38:8b597ab8344f 81 double curr_max = 0.0;
Jellehierck 38:8b597ab8344f 82 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 83 for (int i = 0; i < vect_n; i++) {
Jellehierck 38:8b597ab8344f 84 if (vect[i] > curr_max) {
Jellehierck 38:8b597ab8344f 85 curr_max = vect[i];
Jellehierck 38:8b597ab8344f 86 };
Jellehierck 38:8b597ab8344f 87 }
Jellehierck 38:8b597ab8344f 88 return curr_max;
Jellehierck 38:8b597ab8344f 89 }
Jellehierck 37:806c7c8381a7 90
Jellehierck 38:8b597ab8344f 91 // Return mean of vector
Jellehierck 38:8b597ab8344f 92 double getMean(const vector<double> &vect)
Jellehierck 38:8b597ab8344f 93 {
Jellehierck 38:8b597ab8344f 94 double sum = 0.0;
Jellehierck 38:8b597ab8344f 95 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 96 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 97 sum += vect[i];
Jellehierck 38:8b597ab8344f 98 }
Jellehierck 38:8b597ab8344f 99 return sum/vect_n;
Jellehierck 38:8b597ab8344f 100 }
Jellehierck 37:806c7c8381a7 101
Jellehierck 38:8b597ab8344f 102 // Return standard deviation of vector
Jellehierck 38:8b597ab8344f 103 double getStdev(const vector<double> &vect, const double vect_mean)
Jellehierck 38:8b597ab8344f 104 {
Jellehierck 38:8b597ab8344f 105 double sum2 = 0.0;
Jellehierck 38:8b597ab8344f 106 int vect_n = vect.size();
Jellehierck 38:8b597ab8344f 107 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 38:8b597ab8344f 108 sum2 += pow( vect[i] - vect_mean, 2 );
Jellehierck 38:8b597ab8344f 109 }
Jellehierck 38:8b597ab8344f 110 double output = sqrt( sum2 / vect_n );
Jellehierck 38:8b597ab8344f 111 return output;
Jellehierck 38:8b597ab8344f 112 }
Jellehierck 38:8b597ab8344f 113
Jellehierck 38:8b597ab8344f 114 // Rescale double values to certain range
Jellehierck 38:8b597ab8344f 115 double rescale(double input, double out_min, double out_max, double in_min, double in_max)
Jellehierck 38:8b597ab8344f 116 {
Jellehierck 38:8b597ab8344f 117 double output = out_min + ((input-in_min)/(in_max-in_min))*(out_max-out_min); // Based on MATLAB rescale function
Jellehierck 38:8b597ab8344f 118 return output;
Jellehierck 38:8b597ab8344f 119 }
Jellehierck 37:806c7c8381a7 120
Jellehierck 37:806c7c8381a7 121 /*
Jellehierck 37:806c7c8381a7 122 ------------------------------ BUTTON FUNCTIONS ------------------------------
Jellehierck 35:e82834e62e44 123 */
Jellehierck 35:e82834e62e44 124
Jellehierck 25:a1be4cf2ab0b 125 // Handle button press
Jellehierck 25:a1be4cf2ab0b 126 void button1Press()
Jellehierck 25:a1be4cf2ab0b 127 {
Jellehierck 25:a1be4cf2ab0b 128 button1_pressed = true;
Jellehierck 25:a1be4cf2ab0b 129 }
Jellehierck 25:a1be4cf2ab0b 130
Jellehierck 25:a1be4cf2ab0b 131 // Handle button press
Jellehierck 25:a1be4cf2ab0b 132 void button2Press()
Jellehierck 25:a1be4cf2ab0b 133 {
Jellehierck 25:a1be4cf2ab0b 134 button2_pressed = true;
Jellehierck 25:a1be4cf2ab0b 135 }
Jellehierck 25:a1be4cf2ab0b 136
Jellehierck 37:806c7c8381a7 137 void switch2Press()
Jellehierck 6:5437cc97e1e6 138 {
Jellehierck 37:806c7c8381a7 139 switch2_pressed = true;
Jellehierck 35:e82834e62e44 140 }
Jellehierck 6:5437cc97e1e6 141
Jellehierck 37:806c7c8381a7 142 void switch3Press()
Jellehierck 35:e82834e62e44 143 {
Jellehierck 37:806c7c8381a7 144 global_curr_state = global_failure;
Jellehierck 37:806c7c8381a7 145 global_state_changed = true;
Jellehierck 6:5437cc97e1e6 146 }
Jellehierck 6:5437cc97e1e6 147
Jellehierck 15:421d3d9c563b 148 /*
Jellehierck 38:8b597ab8344f 149 ------------------------------ EMG GLOBAL VARIABLES & CONSTANTS ------------------------------
Jellehierck 38:8b597ab8344f 150 */
Jellehierck 38:8b597ab8344f 151
Jellehierck 38:8b597ab8344f 152 // Set global constant values for EMG reading & analysis
Jellehierck 38:8b597ab8344f 153 const double Tcal = 10.0f; // Calibration duration (s)
Jellehierck 38:8b597ab8344f 154
Jellehierck 38:8b597ab8344f 155 // Initialize variables for EMG reading & analysis
Jellehierck 38:8b597ab8344f 156 double emg1;
Jellehierck 38:8b597ab8344f 157 double emg1_env;
Jellehierck 38:8b597ab8344f 158 double emg1_MVC;
Jellehierck 38:8b597ab8344f 159 double emg1_rest;
Jellehierck 38:8b597ab8344f 160 double emg1_factor;//delete
Jellehierck 38:8b597ab8344f 161 double emg1_th;
Jellehierck 38:8b597ab8344f 162 double emg1_out;
Jellehierck 38:8b597ab8344f 163 double emg1_norm; //delete
Jellehierck 38:8b597ab8344f 164 vector<double> emg1_cal;
Jellehierck 38:8b597ab8344f 165 int emg1_cal_size; //delete
Jellehierck 38:8b597ab8344f 166 int emg1_dir = 1;
Jellehierck 38:8b597ab8344f 167 double emg1_out_prev;
Jellehierck 38:8b597ab8344f 168 double emg1_dt; //delete
Jellehierck 38:8b597ab8344f 169 double emg1_dt_prev;
Jellehierck 38:8b597ab8344f 170 double emg1_dtdt; //delete
Jellehierck 38:8b597ab8344f 171
Jellehierck 38:8b597ab8344f 172 double emg2;
Jellehierck 38:8b597ab8344f 173 double emg2_env;
Jellehierck 38:8b597ab8344f 174 double emg2_MVC;
Jellehierck 38:8b597ab8344f 175 double emg2_rest;
Jellehierck 38:8b597ab8344f 176 double emg2_factor;//delete
Jellehierck 38:8b597ab8344f 177 double emg2_th;
Jellehierck 38:8b597ab8344f 178 double emg2_out;
Jellehierck 38:8b597ab8344f 179 double emg2_norm;//delete
Jellehierck 38:8b597ab8344f 180 vector<double> emg2_cal;
Jellehierck 38:8b597ab8344f 181 int emg2_cal_size;//delete
Jellehierck 38:8b597ab8344f 182 int emg2_dir = 1;
Jellehierck 38:8b597ab8344f 183
Jellehierck 38:8b597ab8344f 184 double emg3;
Jellehierck 38:8b597ab8344f 185 double emg3_env;
Jellehierck 38:8b597ab8344f 186 double emg3_MVC;
Jellehierck 38:8b597ab8344f 187 double emg3_rest;
Jellehierck 38:8b597ab8344f 188 double emg3_factor;//delete
Jellehierck 38:8b597ab8344f 189 double emg3_th;
Jellehierck 38:8b597ab8344f 190 double emg3_out;
Jellehierck 38:8b597ab8344f 191 double emg3_norm;//delete
Jellehierck 38:8b597ab8344f 192 vector<double> emg3_cal;
Jellehierck 38:8b597ab8344f 193 int emg3_cal_size;//delete
Jellehierck 38:8b597ab8344f 194 int emg3_dir = 1;
Jellehierck 38:8b597ab8344f 195
Jellehierck 38:8b597ab8344f 196 /*
Jellehierck 38:8b597ab8344f 197 ------------------------------ EMG FILTERS ------------------------------
Jellehierck 38:8b597ab8344f 198 */
Jellehierck 38:8b597ab8344f 199
Jellehierck 38:8b597ab8344f 200 // Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB:
Jellehierck 38:8b597ab8344f 201 BiQuad bq1_notch( 0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 202 BiQuad bq2_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 203 BiQuad bq3_notch = bq1_notch;
Jellehierck 38:8b597ab8344f 204 BiQuadChain bqc1_notch;
Jellehierck 38:8b597ab8344f 205 BiQuadChain bqc2_notch;
Jellehierck 38:8b597ab8344f 206 BiQuadChain bqc3_notch;
Jellehierck 38:8b597ab8344f 207
Jellehierck 38:8b597ab8344f 208 // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
Jellehierck 38:8b597ab8344f 209 BiQuad bq1_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 210 BiQuad bq1_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 211 BiQuad bq2_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 212 BiQuad bq2_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 213 BiQuad bq3_H1 = bq1_H1;
Jellehierck 38:8b597ab8344f 214 BiQuad bq3_H2 = bq1_H2;
Jellehierck 38:8b597ab8344f 215 BiQuadChain bqc1_high;
Jellehierck 38:8b597ab8344f 216 BiQuadChain bqc2_high;
Jellehierck 38:8b597ab8344f 217 BiQuadChain bqc3_high;
Jellehierck 38:8b597ab8344f 218
Jellehierck 38:8b597ab8344f 219 // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
Jellehierck 38:8b597ab8344f 220 BiQuad bq1_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138); // b01 b11 b21 a01 a11 a21
Jellehierck 38:8b597ab8344f 221 BiQuad bq1_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162); // b02 b12 b22 a02 a12 a22
Jellehierck 38:8b597ab8344f 222 BiQuad bq2_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 223 BiQuad bq2_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 224 BiQuad bq3_L1 = bq1_L1;
Jellehierck 38:8b597ab8344f 225 BiQuad bq3_L2 = bq1_L2;
Jellehierck 38:8b597ab8344f 226 BiQuadChain bqc1_low;
Jellehierck 38:8b597ab8344f 227 BiQuadChain bqc2_low;
Jellehierck 38:8b597ab8344f 228 BiQuadChain bqc3_low;
Jellehierck 38:8b597ab8344f 229
Jellehierck 38:8b597ab8344f 230 // Function to check filter stability
Jellehierck 38:8b597ab8344f 231 bool checkBQChainStable()
Jellehierck 38:8b597ab8344f 232 {
Jellehierck 38:8b597ab8344f 233 bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains
Jellehierck 38:8b597ab8344f 234 bool hp_stable = bqc1_high.stable();
Jellehierck 38:8b597ab8344f 235 bool l_stable = bqc1_low.stable();
Jellehierck 38:8b597ab8344f 236
Jellehierck 38:8b597ab8344f 237 if (n_stable && hp_stable && l_stable) {
Jellehierck 38:8b597ab8344f 238 return true;
Jellehierck 38:8b597ab8344f 239 } else {
Jellehierck 38:8b597ab8344f 240 return false;
Jellehierck 38:8b597ab8344f 241 }
Jellehierck 38:8b597ab8344f 242 }
Jellehierck 38:8b597ab8344f 243 /*
Jellehierck 38:8b597ab8344f 244 ------------------------------ EMG SUBSTATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 245 */
Jellehierck 38:8b597ab8344f 246
Jellehierck 38:8b597ab8344f 247 // EMG Waiting state
Jellehierck 38:8b597ab8344f 248 void do_emg_wait()
Jellehierck 38:8b597ab8344f 249 {
Jellehierck 38:8b597ab8344f 250 // Entry function
Jellehierck 38:8b597ab8344f 251 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 252 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 253
Jellehierck 38:8b597ab8344f 254 button1.fall( &button1Press ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 255 button2.fall( &button2Press ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 256 }
Jellehierck 38:8b597ab8344f 257
Jellehierck 38:8b597ab8344f 258 // Do nothing until end condition is met
Jellehierck 38:8b597ab8344f 259
Jellehierck 38:8b597ab8344f 260 // State transition guard
Jellehierck 38:8b597ab8344f 261 if ( button1_pressed ) { // MVC calibration
Jellehierck 38:8b597ab8344f 262 button1_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 263 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 264 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 265 emg_curr_state = emg_cal_MVC; // Set next state
Jellehierck 38:8b597ab8344f 266 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 267
Jellehierck 38:8b597ab8344f 268 } else if ( button2_pressed ) { // Rest calibration
Jellehierck 38:8b597ab8344f 269 button2_pressed = false; // Disable button pressed function until next button press
Jellehierck 38:8b597ab8344f 270 button1.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 271 button2.fall( NULL ); // Disable interrupt during calibration
Jellehierck 38:8b597ab8344f 272 emg_curr_state = emg_cal_rest; // Set next state
Jellehierck 38:8b597ab8344f 273 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 274
Jellehierck 38:8b597ab8344f 275 } else if ( emg_MVC_cal_done && emg_rest_cal_done ) { // Operation mode
Jellehierck 38:8b597ab8344f 276 button1.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 277 button2.fall( NULL ); // Disable interrupt during operation
Jellehierck 38:8b597ab8344f 278 emg_curr_state = emg_operation; // Set next state
Jellehierck 38:8b597ab8344f 279 emg_state_changed = true; // Enable entry functions
Jellehierck 38:8b597ab8344f 280 }
Jellehierck 38:8b597ab8344f 281 }
Jellehierck 38:8b597ab8344f 282
Jellehierck 38:8b597ab8344f 283 // EMG Calibration state
Jellehierck 38:8b597ab8344f 284 void do_emg_cal()
Jellehierck 38:8b597ab8344f 285 {
Jellehierck 38:8b597ab8344f 286 // Entry functions
Jellehierck 38:8b597ab8344f 287 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 288 emg_state_changed = false; // Disable entry functions
Jellehierck 38:8b597ab8344f 289 led_b = 0; // Turn on calibration led
Jellehierck 38:8b597ab8344f 290
Jellehierck 38:8b597ab8344f 291 timerCalibration.reset();
Jellehierck 38:8b597ab8344f 292 timerCalibration.start(); // Sets up timer to stop calibration after Tcal seconds
Jellehierck 38:8b597ab8344f 293 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 294 emg_calibrateNow = true; // Enable calibration vector functionality in sampleSignals()
Jellehierck 38:8b597ab8344f 295
Jellehierck 38:8b597ab8344f 296 emg1_cal.reserve(Fs * Tcal); // Initialize vector lengths to prevent memory overflow
Jellehierck 38:8b597ab8344f 297 emg2_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 298 emg3_cal.reserve(Fs * Tcal); // Idem
Jellehierck 38:8b597ab8344f 299 }
Jellehierck 38:8b597ab8344f 300
Jellehierck 38:8b597ab8344f 301 // Do stuff until end condition is met
Jellehierck 38:8b597ab8344f 302 // Set HIDScope outputs
Jellehierck 38:8b597ab8344f 303 scope.set(0, emg1 );
Jellehierck 38:8b597ab8344f 304 scope.set(1, emg1_env );
Jellehierck 38:8b597ab8344f 305 //scope.set(2, emg2_env );
Jellehierck 38:8b597ab8344f 306 //scope.set(3, emg3_env );
Jellehierck 38:8b597ab8344f 307 scope.send();
Jellehierck 38:8b597ab8344f 308
Jellehierck 38:8b597ab8344f 309 // State transition guard
Jellehierck 38:8b597ab8344f 310 if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
Jellehierck 38:8b597ab8344f 311 emg_sampleNow = false; // Disable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 312 emg_calibrateNow = false; // Disable calibration sampling
Jellehierck 38:8b597ab8344f 313 led_b = 1; // Turn off calibration led
Jellehierck 38:8b597ab8344f 314
Jellehierck 38:8b597ab8344f 315 // Extract EMG scale data from calibration
Jellehierck 38:8b597ab8344f 316 switch( emg_curr_state ) {
Jellehierck 39:f9042483b921 317 case emg_cal_MVC: // In case of MVC calibration
Jellehierck 38:8b597ab8344f 318 emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally
Jellehierck 38:8b597ab8344f 319 emg2_MVC = getMax(emg2_cal);
Jellehierck 38:8b597ab8344f 320 emg3_MVC = getMax(emg3_cal);
Jellehierck 38:8b597ab8344f 321
Jellehierck 38:8b597ab8344f 322 emg_MVC_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 323 break;
Jellehierck 39:f9042483b921 324 case emg_cal_rest: // In case of rest calibration
Jellehierck 38:8b597ab8344f 325 emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally
Jellehierck 38:8b597ab8344f 326 emg2_rest = getMean(emg2_cal);
Jellehierck 38:8b597ab8344f 327 emg3_rest = getMean(emg3_cal);
Jellehierck 38:8b597ab8344f 328 emg_rest_cal_done = true; // Set up transition to EMG operation mode
Jellehierck 38:8b597ab8344f 329 break;
Jellehierck 38:8b597ab8344f 330 }
Jellehierck 38:8b597ab8344f 331 vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
Jellehierck 38:8b597ab8344f 332 vector<double>().swap(emg2_cal);
Jellehierck 38:8b597ab8344f 333 vector<double>().swap(emg3_cal);
Jellehierck 38:8b597ab8344f 334
Jellehierck 38:8b597ab8344f 335 emg_curr_state = emg_wait; // Set next substate
Jellehierck 38:8b597ab8344f 336 emg_state_changed = true; // Enable substate entry function
Jellehierck 38:8b597ab8344f 337 }
Jellehierck 38:8b597ab8344f 338 }
Jellehierck 38:8b597ab8344f 339
Jellehierck 38:8b597ab8344f 340 // EMG Operation state
Jellehierck 38:8b597ab8344f 341 void do_emg_operation()
Jellehierck 38:8b597ab8344f 342 {
Jellehierck 38:8b597ab8344f 343 // Entry function
Jellehierck 38:8b597ab8344f 344 if ( emg_state_changed == true ) {
Jellehierck 38:8b597ab8344f 345 emg_state_changed = false; // Disable entry functions
Jellehierck 39:f9042483b921 346
Jellehierck 39:f9042483b921 347 // Compute scale factors for all EMG signals
Jellehierck 38:8b597ab8344f 348 double margin_percentage = 5; // Set up % margin for rest
Jellehierck 38:8b597ab8344f 349 emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 350 emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 351 emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 352 emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 353 emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 354 emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 355
Jellehierck 38:8b597ab8344f 356
Jellehierck 38:8b597ab8344f 357 // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------
Jellehierck 38:8b597ab8344f 358 //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 359 //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 360
Jellehierck 39:f9042483b921 361 emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
Jellehierck 38:8b597ab8344f 362 }
Jellehierck 39:f9042483b921 363
Jellehierck 39:f9042483b921 364 // This state only runs its entry functions ONCE and then exits the EMG substate machine
Jellehierck 38:8b597ab8344f 365
Jellehierck 38:8b597ab8344f 366 // State transition guard
Jellehierck 39:f9042483b921 367 if ( false ) { // EMG substate machine is terminated after running this state once, so there is no transition to next EMG substate
Jellehierck 38:8b597ab8344f 368 emg_curr_state = emg_wait; // Set next state
Jellehierck 38:8b597ab8344f 369 emg_state_changed = true; // Enable entry function
Jellehierck 38:8b597ab8344f 370 }
Jellehierck 38:8b597ab8344f 371 }
Jellehierck 38:8b597ab8344f 372
Jellehierck 38:8b597ab8344f 373 /*
Jellehierck 38:8b597ab8344f 374 ------------------------------ EMG SUBSTATE MACHINE ------------------------------
Jellehierck 38:8b597ab8344f 375 */
Jellehierck 38:8b597ab8344f 376
Jellehierck 38:8b597ab8344f 377 void emg_state_machine()
Jellehierck 38:8b597ab8344f 378 {
Jellehierck 38:8b597ab8344f 379 switch(emg_curr_state) {
Jellehierck 38:8b597ab8344f 380 case emg_wait:
Jellehierck 38:8b597ab8344f 381 do_emg_wait();
Jellehierck 38:8b597ab8344f 382 break;
Jellehierck 38:8b597ab8344f 383 case emg_cal_MVC:
Jellehierck 38:8b597ab8344f 384 do_emg_cal();
Jellehierck 38:8b597ab8344f 385 break;
Jellehierck 38:8b597ab8344f 386 case emg_cal_rest:
Jellehierck 38:8b597ab8344f 387 do_emg_cal();
Jellehierck 38:8b597ab8344f 388 break;
Jellehierck 38:8b597ab8344f 389 case emg_operation:
Jellehierck 38:8b597ab8344f 390 do_emg_operation();
Jellehierck 38:8b597ab8344f 391 break;
Jellehierck 38:8b597ab8344f 392 }
Jellehierck 38:8b597ab8344f 393 }
Jellehierck 7:7a088536f1c9 394
Jellehierck 15:421d3d9c563b 395 /*
Jellehierck 37:806c7c8381a7 396 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 397 */
Jellehierck 25:a1be4cf2ab0b 398 /* ALL STATES HAVE THE FOLLOWING FORM:
Jellehierck 25:a1be4cf2ab0b 399 void do_state_function() {
Jellehierck 25:a1be4cf2ab0b 400 // Entry function
Jellehierck 37:806c7c8381a7 401 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 402 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 403 // More functions
Jellehierck 25:a1be4cf2ab0b 404 }
Jellehierck 25:a1be4cf2ab0b 405
Jellehierck 25:a1be4cf2ab0b 406 // Do stuff until end condition is met
Jellehierck 25:a1be4cf2ab0b 407 doStuff();
Jellehierck 25:a1be4cf2ab0b 408
Jellehierck 25:a1be4cf2ab0b 409 // State transition guard
Jellehierck 25:a1be4cf2ab0b 410 if ( endCondition == true ) {
Jellehierck 37:806c7c8381a7 411 global_curr_state = next_state;
Jellehierck 37:806c7c8381a7 412 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 413 // More functions
Jellehierck 25:a1be4cf2ab0b 414 }
Jellehierck 25:a1be4cf2ab0b 415 }
Jellehierck 25:a1be4cf2ab0b 416 */
Jellehierck 25:a1be4cf2ab0b 417
Jellehierck 37:806c7c8381a7 418 // FAILURE MODE
Jellehierck 37:806c7c8381a7 419 void do_global_failure()
Jellehierck 7:7a088536f1c9 420 {
Jellehierck 37:806c7c8381a7 421 // Entry function
Jellehierck 37:806c7c8381a7 422 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 423 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 424
Jellehierck 37:806c7c8381a7 425 failure_mode = true; // Set failure mode
Jellehierck 22:9079c6c0d898 426 }
Jellehierck 37:806c7c8381a7 427
Jellehierck 37:806c7c8381a7 428 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 429
Jellehierck 37:806c7c8381a7 430 // State transition guard
Jellehierck 37:806c7c8381a7 431 if ( false ) { // Never move to other state
Jellehierck 37:806c7c8381a7 432 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 433 global_state_changed = true;
Jellehierck 37:806c7c8381a7 434 }
Jellehierck 25:a1be4cf2ab0b 435 }
Jellehierck 25:a1be4cf2ab0b 436
Jellehierck 37:806c7c8381a7 437 // DEMO MODE
Jellehierck 37:806c7c8381a7 438 void do_global_demo()
Jellehierck 25:a1be4cf2ab0b 439 {
Jellehierck 25:a1be4cf2ab0b 440 // Entry function
Jellehierck 37:806c7c8381a7 441 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 442 global_state_changed = false;
Jellehierck 37:806c7c8381a7 443 // More functions
Jellehierck 37:806c7c8381a7 444 }
Jellehierck 37:806c7c8381a7 445
Jellehierck 37:806c7c8381a7 446 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 447 doStuff();
Jellehierck 35:e82834e62e44 448
Jellehierck 37:806c7c8381a7 449 // State transition guard
Jellehierck 37:806c7c8381a7 450 if ( switch2_pressed == true ) {
Jellehierck 37:806c7c8381a7 451 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 452 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 453 global_state_changed = true;
Jellehierck 37:806c7c8381a7 454 }
Jellehierck 37:806c7c8381a7 455 }
Jellehierck 37:806c7c8381a7 456
Jellehierck 37:806c7c8381a7 457 // WAIT MODE
Jellehierck 37:806c7c8381a7 458 void do_global_wait()
Jellehierck 37:806c7c8381a7 459 {
Jellehierck 37:806c7c8381a7 460 // Entry function
Jellehierck 37:806c7c8381a7 461 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 462 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 463 }
Jellehierck 25:a1be4cf2ab0b 464
Jellehierck 27:f18da01093c9 465 // Do nothing until end condition is met
Jellehierck 25:a1be4cf2ab0b 466
Jellehierck 37:806c7c8381a7 467 // State transition guard
Jellehierck 37:806c7c8381a7 468 if ( switch2_pressed == true ) { // DEMO MODE
Jellehierck 37:806c7c8381a7 469 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 470 global_curr_state = global_demo;
Jellehierck 37:806c7c8381a7 471 global_state_changed = true;
Jellehierck 31:b5188b6d45db 472
Jellehierck 37:806c7c8381a7 473 } else if ( button1_pressed == true ) { // EMG CALIBRATION
Jellehierck 37:806c7c8381a7 474 button1_pressed = false;
Jellehierck 38:8b597ab8344f 475 global_curr_state = global_emg_cal;
Jellehierck 37:806c7c8381a7 476 global_state_changed = true;
Jellehierck 31:b5188b6d45db 477
Jellehierck 37:806c7c8381a7 478 } else if ( button2_pressed == true ) { // MOTOR CALIBRATION
Jellehierck 37:806c7c8381a7 479 button2_pressed = false;
Jellehierck 38:8b597ab8344f 480 global_curr_state = global_motor_cal;
Jellehierck 37:806c7c8381a7 481 global_state_changed = true;
Jellehierck 39:f9042483b921 482 } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE
Jellehierck 39:f9042483b921 483 global_curr_state = global_operation;
Jellehierck 39:f9042483b921 484 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 485 }
Jellehierck 7:7a088536f1c9 486 }
Jellehierck 7:7a088536f1c9 487
Jellehierck 37:806c7c8381a7 488 // EMG CALIBRATION MODE
Jellehierck 38:8b597ab8344f 489 void do_global_emg_cal()
Jellehierck 21:e4569b47945e 490 {
Jellehierck 37:806c7c8381a7 491 // Entry function
Jellehierck 37:806c7c8381a7 492 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 493 global_state_changed = false;
Jellehierck 22:9079c6c0d898 494 }
Jellehierck 7:7a088536f1c9 495
Jellehierck 39:f9042483b921 496 // Run EMG state machine until emg_cal_done flag is true
Jellehierck 39:f9042483b921 497 emg_state_machine();
Jellehierck 31:b5188b6d45db 498
Jellehierck 29:f51683a6cbbf 499 // State transition guard
Jellehierck 39:f9042483b921 500 if ( emg_cal_done == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 501 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 502 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 503 }
Jellehierck 25:a1be4cf2ab0b 504 }
Jellehierck 23:8a0a0b959af1 505
Jellehierck 37:806c7c8381a7 506 // MOTOR CALIBRATION MODE
Jellehierck 38:8b597ab8344f 507 void do_global_motor_cal()
Jellehierck 26:7e81c7db6e7a 508 {
Jellehierck 25:a1be4cf2ab0b 509 // Entry function
Jellehierck 37:806c7c8381a7 510 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 511 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 512 }
Jellehierck 25:a1be4cf2ab0b 513
Jellehierck 25:a1be4cf2ab0b 514 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 515 doStuff();
Jellehierck 28:59e8266f4633 516
Jellehierck 25:a1be4cf2ab0b 517 // State transition guard
Jellehierck 38:8b597ab8344f 518 if ( emg_cal_done == true ) { // OPERATION MODE
Jellehierck 38:8b597ab8344f 519 motor_cal_done = true;
Jellehierck 37:806c7c8381a7 520 global_curr_state = global_operation;
Jellehierck 37:806c7c8381a7 521 global_state_changed = true;
Jellehierck 37:806c7c8381a7 522 } else if ( button2_pressed == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 523 button2_pressed = false;
Jellehierck 38:8b597ab8344f 524 motor_cal_done = true;
Jellehierck 37:806c7c8381a7 525 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 526 global_state_changed = true;
Jellehierck 23:8a0a0b959af1 527 }
Jellehierck 23:8a0a0b959af1 528 }
Jellehierck 23:8a0a0b959af1 529
Jellehierck 37:806c7c8381a7 530 // OPERATION MODE
Jellehierck 37:806c7c8381a7 531 void do_global_operation()
Jellehierck 37:806c7c8381a7 532 {
Jellehierck 37:806c7c8381a7 533 // Entry function
Jellehierck 37:806c7c8381a7 534 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 535 global_state_changed = false;
Jellehierck 39:f9042483b921 536
Jellehierck 39:f9042483b921 537 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 39:f9042483b921 538 emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
Jellehierck 37:806c7c8381a7 539 }
Jellehierck 37:806c7c8381a7 540
Jellehierck 37:806c7c8381a7 541 // Do stuff until end condition is met
Jellehierck 39:f9042483b921 542 emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor
Jellehierck 39:f9042483b921 543 emg2_norm = emg2_env * emg2_factor; // Idem
Jellehierck 39:f9042483b921 544 emg3_norm = emg3_env * emg3_factor; // Idem
Jellehierck 39:f9042483b921 545
Jellehierck 39:f9042483b921 546 emg1_out_prev = emg1_out; // Set previous emg_out signal
Jellehierck 39:f9042483b921 547 emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal
Jellehierck 39:f9042483b921 548
Jellehierck 39:f9042483b921 549 // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
Jellehierck 39:f9042483b921 550 if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
Jellehierck 39:f9042483b921 551 emg1_out = 0.0;
Jellehierck 39:f9042483b921 552 } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
Jellehierck 39:f9042483b921 553 emg1_out = 1.0;
Jellehierck 39:f9042483b921 554 } else { // If in between threshold and MVC, scale EMG signal accordingly
Jellehierck 39:f9042483b921 555 // Inputs may be in range [emg_th, 1]
Jellehierck 39:f9042483b921 556 // Outputs are scaled to range [0, 1]
Jellehierck 39:f9042483b921 557 emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
Jellehierck 39:f9042483b921 558 }
Jellehierck 39:f9042483b921 559 emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
Jellehierck 39:f9042483b921 560 emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
Jellehierck 39:f9042483b921 561 emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
Jellehierck 39:f9042483b921 562
Jellehierck 39:f9042483b921 563 // Idem for emg2
Jellehierck 39:f9042483b921 564 if ( emg2_norm < emg2_th ) {
Jellehierck 39:f9042483b921 565 emg2_out = 0.0;
Jellehierck 39:f9042483b921 566 } else if ( emg2_norm > 1.0f ) {
Jellehierck 39:f9042483b921 567 emg2_out = 1.0;
Jellehierck 39:f9042483b921 568 } else {
Jellehierck 39:f9042483b921 569 emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
Jellehierck 39:f9042483b921 570 }
Jellehierck 39:f9042483b921 571 emg2_out = emg2_out * emg2_dir; // Set direction of EMG output
Jellehierck 39:f9042483b921 572
Jellehierck 39:f9042483b921 573 // Idem for emg3
Jellehierck 39:f9042483b921 574 if ( emg3_norm < emg3_th ) {
Jellehierck 39:f9042483b921 575 emg3_out = 0.0;
Jellehierck 39:f9042483b921 576 } else if ( emg3_norm > 1.0f ) {
Jellehierck 39:f9042483b921 577 emg3_out = 1.0;
Jellehierck 39:f9042483b921 578 } else {
Jellehierck 39:f9042483b921 579 emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
Jellehierck 39:f9042483b921 580 }
Jellehierck 39:f9042483b921 581
Jellehierck 39:f9042483b921 582 // Set HIDScope outputs
Jellehierck 39:f9042483b921 583 scope.set(0, emg1 );
Jellehierck 39:f9042483b921 584 scope.set(1, emg1_out );
Jellehierck 39:f9042483b921 585 scope.set(2, emg1_dt );
Jellehierck 39:f9042483b921 586 scope.set(3, emg1_dtdt );
Jellehierck 39:f9042483b921 587 //scope.set(2, emg2_out );
Jellehierck 39:f9042483b921 588 //scope.set(3, emg3_out );
Jellehierck 39:f9042483b921 589 scope.send();
Jellehierck 39:f9042483b921 590
Jellehierck 39:f9042483b921 591 led_g = !led_g;
Jellehierck 37:806c7c8381a7 592
Jellehierck 37:806c7c8381a7 593 // State transition guard
Jellehierck 37:806c7c8381a7 594 if ( false ) { // Always stay in operation mode (can be changed)
Jellehierck 37:806c7c8381a7 595 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 596 global_state_changed = true;
Jellehierck 37:806c7c8381a7 597 }
Jellehierck 37:806c7c8381a7 598 }
Jellehierck 23:8a0a0b959af1 599 /*
Jellehierck 37:806c7c8381a7 600 ------------------------------ GLOBAL STATE MACHINE ------------------------------
Jellehierck 23:8a0a0b959af1 601 */
Jellehierck 37:806c7c8381a7 602 void global_state_machine()
Jellehierck 23:8a0a0b959af1 603 {
Jellehierck 37:806c7c8381a7 604 switch(global_curr_state) {
Jellehierck 37:806c7c8381a7 605 case global_failure:
Jellehierck 37:806c7c8381a7 606 do_global_failure();
Jellehierck 23:8a0a0b959af1 607 break;
Jellehierck 37:806c7c8381a7 608 case global_wait:
Jellehierck 37:806c7c8381a7 609 do_global_wait();
Jellehierck 37:806c7c8381a7 610 break;
Jellehierck 38:8b597ab8344f 611 case global_emg_cal:
Jellehierck 38:8b597ab8344f 612 do_global_emg_cal();
Jellehierck 23:8a0a0b959af1 613 break;
Jellehierck 38:8b597ab8344f 614 case global_motor_cal:
Jellehierck 38:8b597ab8344f 615 do_global_motor_cal();
Jellehierck 23:8a0a0b959af1 616 break;
Jellehierck 37:806c7c8381a7 617 case global_operation:
Jellehierck 37:806c7c8381a7 618 do_global_operation();
Jellehierck 37:806c7c8381a7 619 break;
Jellehierck 37:806c7c8381a7 620 case global_demo:
Jellehierck 37:806c7c8381a7 621 do_global_demo();
Jellehierck 23:8a0a0b959af1 622 break;
Jellehierck 23:8a0a0b959af1 623 }
Jellehierck 23:8a0a0b959af1 624 }
Jellehierck 23:8a0a0b959af1 625
Jellehierck 38:8b597ab8344f 626 /*
Jellehierck 38:8b597ab8344f 627 ------------------------------ READ SAMPLES ------------------------------
Jellehierck 38:8b597ab8344f 628 */
Jellehierck 38:8b597ab8344f 629 void sampleSignals()
Jellehierck 38:8b597ab8344f 630 {
Jellehierck 38:8b597ab8344f 631 if (emg_sampleNow == true) { // This ticker only samples if the sample flag is true, to prevent unnecessary computations
Jellehierck 38:8b597ab8344f 632 // Read EMG inputs
Jellehierck 38:8b597ab8344f 633 emg1 = emg1_in.read();
Jellehierck 38:8b597ab8344f 634 emg2 = emg2_in.read();
Jellehierck 38:8b597ab8344f 635 emg3 = emg3_in.read();
Jellehierck 38:8b597ab8344f 636
Jellehierck 38:8b597ab8344f 637 double emg1_n = bqc1_notch.step( emg1 ); // Filter notch
Jellehierck 38:8b597ab8344f 638 double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 639 double emg1_rectify = fabs( emg1_hp ); // Rectify
Jellehierck 38:8b597ab8344f 640 emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 641
Jellehierck 38:8b597ab8344f 642 double emg2_n = bqc2_notch.step( emg2 ); // Filter notch
Jellehierck 38:8b597ab8344f 643 double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 644 double emg2_rectify = fabs( emg2_hp ); // Rectify
Jellehierck 38:8b597ab8344f 645 emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 646
Jellehierck 38:8b597ab8344f 647 double emg3_n = bqc3_notch.step( emg3 ); // Filter notch
Jellehierck 38:8b597ab8344f 648 double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 649 double emg3_rectify = fabs( emg3_hp ); // Rectify
Jellehierck 38:8b597ab8344f 650 emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 651
Jellehierck 38:8b597ab8344f 652 if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true
Jellehierck 38:8b597ab8344f 653 emg1_cal.push_back(emg1_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 654 // emg1_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 655 emg2_cal.push_back(emg2_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 656 // emg2_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 657 emg3_cal.push_back(emg3_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 658 // emg3_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 659 }
Jellehierck 38:8b597ab8344f 660 }
Jellehierck 38:8b597ab8344f 661 }
Jellehierck 37:806c7c8381a7 662
Jellehierck 37:806c7c8381a7 663 /*
Jellehierck 37:806c7c8381a7 664 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
Jellehierck 37:806c7c8381a7 665 */
Jellehierck 25:a1be4cf2ab0b 666 void tickGlobalFunc()
Jellehierck 25:a1be4cf2ab0b 667 {
Jellehierck 38:8b597ab8344f 668 sampleSignals();
Jellehierck 37:806c7c8381a7 669 global_state_machine();
Jellehierck 25:a1be4cf2ab0b 670 // controller();
Jellehierck 25:a1be4cf2ab0b 671 // outputToMotors();
Jellehierck 25:a1be4cf2ab0b 672 }
Jellehierck 25:a1be4cf2ab0b 673
Jellehierck 37:806c7c8381a7 674 /*
Jellehierck 37:806c7c8381a7 675 ------------------------------ MAIN FUNCTION ------------------------------
Jellehierck 37:806c7c8381a7 676 */
Jellehierck 39:f9042483b921 677 int main()
Jellehierck 23:8a0a0b959af1 678 {
Jellehierck 23:8a0a0b959af1 679 pc.baud(115200); // MODSERIAL rate
Jellehierck 23:8a0a0b959af1 680 pc.printf("Starting\r\n");
Jellehierck 23:8a0a0b959af1 681
Jellehierck 37:806c7c8381a7 682 global_curr_state = global_wait; // Start off in EMG Wait state
Jellehierck 34:13fac02ef324 683 tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
Jellehierck 8:ea3de43c9e8b 684
Jellehierck 38:8b597ab8344f 685 // ---------- Attach filters ----------
Jellehierck 38:8b597ab8344f 686 bqc1_notch.add( &bq1_notch );
Jellehierck 38:8b597ab8344f 687 bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
Jellehierck 38:8b597ab8344f 688 bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
Jellehierck 38:8b597ab8344f 689
Jellehierck 38:8b597ab8344f 690 bqc2_notch.add( &bq2_notch );
Jellehierck 38:8b597ab8344f 691 bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
Jellehierck 38:8b597ab8344f 692 bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
Jellehierck 38:8b597ab8344f 693
Jellehierck 38:8b597ab8344f 694 bqc3_notch.add( &bq3_notch );
Jellehierck 38:8b597ab8344f 695 bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
Jellehierck 38:8b597ab8344f 696 bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
Jellehierck 38:8b597ab8344f 697
Jellehierck 38:8b597ab8344f 698 // ---------- Attach buttons ----------
Jellehierck 37:806c7c8381a7 699 button1.fall( &button1Press );
Jellehierck 37:806c7c8381a7 700 button2.fall( &button2Press );
Jellehierck 37:806c7c8381a7 701 switch2.fall( &switch2Press );
Jellehierck 37:806c7c8381a7 702 switch3.fall( &switch3Press );
Jellehierck 38:8b597ab8344f 703
Jellehierck 38:8b597ab8344f 704 // ---------- Turn OFF LEDs ----------
Jellehierck 38:8b597ab8344f 705 led_b = 1;
Jellehierck 38:8b597ab8344f 706 led_g = 1;
Jellehierck 38:8b597ab8344f 707 led_r = 1;
Jellehierck 37:806c7c8381a7 708
Jellehierck 23:8a0a0b959af1 709 while(true) {
Jellehierck 38:8b597ab8344f 710 pc.printf("Global state: %i EMG substate: %i\r\n", global_curr_state, emg_curr_state);
Jellehierck 30:bac3b60d6283 711 wait(0.5f);
Jellehierck 23:8a0a0b959af1 712 }
Jellehierck 23:8a0a0b959af1 713 }