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@38:8b597ab8344f, 2019-10-30 (annotated)

Committer:
Jellehierck
Date:
Wed Oct 30 15:49:09 2019 +0000
Revision:
38:8b597ab8344f
Parent:
37:806c7c8381a7
Child:
39:f9042483b921
Implemented EMG substate, not working correctly yet

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 38:8b597ab8344f 317 case emg_cal_MVC:
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 38:8b597ab8344f 324 case emg_cal_rest:
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 38:8b597ab8344f 346 double margin_percentage = 5; // Set up % margin for rest
Jellehierck 38:8b597ab8344f 347
Jellehierck 38:8b597ab8344f 348 emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 349 emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 350 emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 351 emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 352 emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
Jellehierck 38:8b597ab8344f 353 emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
Jellehierck 38:8b597ab8344f 354
Jellehierck 38:8b597ab8344f 355
Jellehierck 38:8b597ab8344f 356 // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------
Jellehierck 38:8b597ab8344f 357 //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
Jellehierck 38:8b597ab8344f 358 //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
Jellehierck 38:8b597ab8344f 359
Jellehierck 38:8b597ab8344f 360 emg_sampleNow = true; // Enable signal sampling in sampleSignals()
Jellehierck 38:8b597ab8344f 361 emg_calibrateNow = false; // Disable calibration vector functionality in sampleSignals()
Jellehierck 38:8b597ab8344f 362 }
Jellehierck 38:8b597ab8344f 363
Jellehierck 38:8b597ab8344f 364 // Do stuff until end condition is met
Jellehierck 38:8b597ab8344f 365 emg1_norm = emg1_env * emg1_factor; // Normalize EMG signal with calibrated factor
Jellehierck 38:8b597ab8344f 366 emg2_norm = emg2_env * emg2_factor; // Idem
Jellehierck 38:8b597ab8344f 367 emg3_norm = emg3_env * emg3_factor; // Idem
Jellehierck 38:8b597ab8344f 368
Jellehierck 38:8b597ab8344f 369 emg1_out_prev = emg1_out; // Set previous emg_out signal
Jellehierck 38:8b597ab8344f 370 emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal
Jellehierck 38:8b597ab8344f 371
Jellehierck 38:8b597ab8344f 372 // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
Jellehierck 38:8b597ab8344f 373 if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
Jellehierck 38:8b597ab8344f 374 emg1_out = 0.0;
Jellehierck 38:8b597ab8344f 375 } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
Jellehierck 38:8b597ab8344f 376 emg1_out = 1.0;
Jellehierck 38:8b597ab8344f 377 } else { // If in between threshold and MVC, scale EMG signal accordingly
Jellehierck 38:8b597ab8344f 378 // Inputs may be in range [emg_th, 1]
Jellehierck 38:8b597ab8344f 379 // Outputs are scaled to range [0, 1]
Jellehierck 38:8b597ab8344f 380 emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
Jellehierck 38:8b597ab8344f 381 }
Jellehierck 38:8b597ab8344f 382 emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
Jellehierck 38:8b597ab8344f 383 emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
Jellehierck 38:8b597ab8344f 384 emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
Jellehierck 38:8b597ab8344f 385
Jellehierck 38:8b597ab8344f 386 // Idem for emg2
Jellehierck 38:8b597ab8344f 387 if ( emg2_norm < emg2_th ) {
Jellehierck 38:8b597ab8344f 388 emg2_out = 0.0;
Jellehierck 38:8b597ab8344f 389 } else if ( emg2_norm > 1.0f ) {
Jellehierck 38:8b597ab8344f 390 emg2_out = 1.0;
Jellehierck 38:8b597ab8344f 391 } else {
Jellehierck 38:8b597ab8344f 392 emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
Jellehierck 38:8b597ab8344f 393 }
Jellehierck 38:8b597ab8344f 394 emg2_out = emg2_out * emg2_dir; // Set direction of EMG output
Jellehierck 38:8b597ab8344f 395
Jellehierck 38:8b597ab8344f 396 // Idem for emg3
Jellehierck 38:8b597ab8344f 397 if ( emg3_norm < emg3_th ) {
Jellehierck 38:8b597ab8344f 398 emg3_out = 0.0;
Jellehierck 38:8b597ab8344f 399 } else if ( emg3_norm > 1.0f ) {
Jellehierck 38:8b597ab8344f 400 emg3_out = 1.0;
Jellehierck 38:8b597ab8344f 401 } else {
Jellehierck 38:8b597ab8344f 402 emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
Jellehierck 38:8b597ab8344f 403 }
Jellehierck 38:8b597ab8344f 404
Jellehierck 38:8b597ab8344f 405 // Set HIDScope outputs
Jellehierck 38:8b597ab8344f 406 scope.set(0, emg1 );
Jellehierck 38:8b597ab8344f 407 scope.set(1, emg1_out );
Jellehierck 38:8b597ab8344f 408 scope.set(2, emg1_dt );
Jellehierck 38:8b597ab8344f 409 scope.set(3, emg1_dtdt );
Jellehierck 38:8b597ab8344f 410 //scope.set(2, emg2_out );
Jellehierck 38:8b597ab8344f 411 //scope.set(3, emg3_out );
Jellehierck 38:8b597ab8344f 412 scope.send();
Jellehierck 38:8b597ab8344f 413
Jellehierck 38:8b597ab8344f 414 led_g = !led_g;
Jellehierck 38:8b597ab8344f 415
Jellehierck 38:8b597ab8344f 416 // State transition guard
Jellehierck 38:8b597ab8344f 417 if ( false ) {
Jellehierck 38:8b597ab8344f 418 emg_curr_state = emg_wait; // Set next state
Jellehierck 38:8b597ab8344f 419 emg_state_changed = true; // Enable entry function
Jellehierck 38:8b597ab8344f 420 }
Jellehierck 38:8b597ab8344f 421 }
Jellehierck 38:8b597ab8344f 422
Jellehierck 38:8b597ab8344f 423 /*
Jellehierck 38:8b597ab8344f 424 ------------------------------ EMG SUBSTATE MACHINE ------------------------------
Jellehierck 38:8b597ab8344f 425 */
Jellehierck 38:8b597ab8344f 426
Jellehierck 38:8b597ab8344f 427 void emg_state_machine()
Jellehierck 38:8b597ab8344f 428 {
Jellehierck 38:8b597ab8344f 429 switch(emg_curr_state) {
Jellehierck 38:8b597ab8344f 430 case emg_wait:
Jellehierck 38:8b597ab8344f 431 do_emg_wait();
Jellehierck 38:8b597ab8344f 432 break;
Jellehierck 38:8b597ab8344f 433 case emg_cal_MVC:
Jellehierck 38:8b597ab8344f 434 do_emg_cal();
Jellehierck 38:8b597ab8344f 435 break;
Jellehierck 38:8b597ab8344f 436 case emg_cal_rest:
Jellehierck 38:8b597ab8344f 437 do_emg_cal();
Jellehierck 38:8b597ab8344f 438 break;
Jellehierck 38:8b597ab8344f 439 case emg_operation:
Jellehierck 38:8b597ab8344f 440 do_emg_operation();
Jellehierck 38:8b597ab8344f 441 break;
Jellehierck 38:8b597ab8344f 442 }
Jellehierck 38:8b597ab8344f 443 }
Jellehierck 7:7a088536f1c9 444
Jellehierck 15:421d3d9c563b 445 /*
Jellehierck 37:806c7c8381a7 446 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
Jellehierck 15:421d3d9c563b 447 */
Jellehierck 25:a1be4cf2ab0b 448 /* ALL STATES HAVE THE FOLLOWING FORM:
Jellehierck 25:a1be4cf2ab0b 449 void do_state_function() {
Jellehierck 25:a1be4cf2ab0b 450 // Entry function
Jellehierck 37:806c7c8381a7 451 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 452 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 453 // More functions
Jellehierck 25:a1be4cf2ab0b 454 }
Jellehierck 25:a1be4cf2ab0b 455
Jellehierck 25:a1be4cf2ab0b 456 // Do stuff until end condition is met
Jellehierck 25:a1be4cf2ab0b 457 doStuff();
Jellehierck 25:a1be4cf2ab0b 458
Jellehierck 25:a1be4cf2ab0b 459 // State transition guard
Jellehierck 25:a1be4cf2ab0b 460 if ( endCondition == true ) {
Jellehierck 37:806c7c8381a7 461 global_curr_state = next_state;
Jellehierck 37:806c7c8381a7 462 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 463 // More functions
Jellehierck 25:a1be4cf2ab0b 464 }
Jellehierck 25:a1be4cf2ab0b 465 }
Jellehierck 25:a1be4cf2ab0b 466 */
Jellehierck 25:a1be4cf2ab0b 467
Jellehierck 37:806c7c8381a7 468 // FAILURE MODE
Jellehierck 37:806c7c8381a7 469 void do_global_failure()
Jellehierck 7:7a088536f1c9 470 {
Jellehierck 37:806c7c8381a7 471 // Entry function
Jellehierck 37:806c7c8381a7 472 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 473 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 474
Jellehierck 37:806c7c8381a7 475 failure_mode = true; // Set failure mode
Jellehierck 22:9079c6c0d898 476 }
Jellehierck 37:806c7c8381a7 477
Jellehierck 37:806c7c8381a7 478 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 479
Jellehierck 37:806c7c8381a7 480 // State transition guard
Jellehierck 37:806c7c8381a7 481 if ( false ) { // Never move to other state
Jellehierck 37:806c7c8381a7 482 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 483 global_state_changed = true;
Jellehierck 37:806c7c8381a7 484 }
Jellehierck 25:a1be4cf2ab0b 485 }
Jellehierck 25:a1be4cf2ab0b 486
Jellehierck 37:806c7c8381a7 487 // DEMO MODE
Jellehierck 37:806c7c8381a7 488 void do_global_demo()
Jellehierck 25:a1be4cf2ab0b 489 {
Jellehierck 25:a1be4cf2ab0b 490 // Entry function
Jellehierck 37:806c7c8381a7 491 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 492 global_state_changed = false;
Jellehierck 37:806c7c8381a7 493 // More functions
Jellehierck 37:806c7c8381a7 494 }
Jellehierck 37:806c7c8381a7 495
Jellehierck 37:806c7c8381a7 496 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 497 doStuff();
Jellehierck 35:e82834e62e44 498
Jellehierck 37:806c7c8381a7 499 // State transition guard
Jellehierck 37:806c7c8381a7 500 if ( switch2_pressed == true ) {
Jellehierck 37:806c7c8381a7 501 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 502 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 503 global_state_changed = true;
Jellehierck 37:806c7c8381a7 504 }
Jellehierck 37:806c7c8381a7 505 }
Jellehierck 37:806c7c8381a7 506
Jellehierck 37:806c7c8381a7 507 // WAIT MODE
Jellehierck 37:806c7c8381a7 508 void do_global_wait()
Jellehierck 37:806c7c8381a7 509 {
Jellehierck 37:806c7c8381a7 510 // Entry function
Jellehierck 37:806c7c8381a7 511 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 512 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 513 }
Jellehierck 25:a1be4cf2ab0b 514
Jellehierck 27:f18da01093c9 515 // Do nothing until end condition is met
Jellehierck 25:a1be4cf2ab0b 516
Jellehierck 37:806c7c8381a7 517 // State transition guard
Jellehierck 37:806c7c8381a7 518 if ( switch2_pressed == true ) { // DEMO MODE
Jellehierck 37:806c7c8381a7 519 switch2_pressed = false;
Jellehierck 37:806c7c8381a7 520 global_curr_state = global_demo;
Jellehierck 37:806c7c8381a7 521 global_state_changed = true;
Jellehierck 31:b5188b6d45db 522
Jellehierck 37:806c7c8381a7 523 } else if ( button1_pressed == true ) { // EMG CALIBRATION
Jellehierck 37:806c7c8381a7 524 button1_pressed = false;
Jellehierck 38:8b597ab8344f 525 global_curr_state = global_emg_cal;
Jellehierck 37:806c7c8381a7 526 global_state_changed = true;
Jellehierck 31:b5188b6d45db 527
Jellehierck 37:806c7c8381a7 528 } else if ( button2_pressed == true ) { // MOTOR CALIBRATION
Jellehierck 37:806c7c8381a7 529 button2_pressed = false;
Jellehierck 38:8b597ab8344f 530 global_curr_state = global_motor_cal;
Jellehierck 37:806c7c8381a7 531 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 532 }
Jellehierck 7:7a088536f1c9 533 }
Jellehierck 7:7a088536f1c9 534
Jellehierck 37:806c7c8381a7 535 // EMG CALIBRATION MODE
Jellehierck 38:8b597ab8344f 536 void do_global_emg_cal()
Jellehierck 21:e4569b47945e 537 {
Jellehierck 37:806c7c8381a7 538 // Entry function
Jellehierck 37:806c7c8381a7 539 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 540 global_state_changed = false;
Jellehierck 22:9079c6c0d898 541 }
Jellehierck 7:7a088536f1c9 542
Jellehierck 31:b5188b6d45db 543 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 544 doStuff();
Jellehierck 31:b5188b6d45db 545
Jellehierck 29:f51683a6cbbf 546 // State transition guard
Jellehierck 38:8b597ab8344f 547 if ( motor_cal_done == true ) { // OPERATION MODE
Jellehierck 38:8b597ab8344f 548 emg_cal_done = true;
Jellehierck 37:806c7c8381a7 549 global_curr_state = global_operation;
Jellehierck 37:806c7c8381a7 550 global_state_changed = true;
Jellehierck 37:806c7c8381a7 551 } else if ( button1_pressed == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 552 button1_pressed = false;
Jellehierck 38:8b597ab8344f 553 emg_cal_done = true;
Jellehierck 37:806c7c8381a7 554 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 555 global_state_changed = true;
Jellehierck 25:a1be4cf2ab0b 556 }
Jellehierck 25:a1be4cf2ab0b 557 }
Jellehierck 23:8a0a0b959af1 558
Jellehierck 37:806c7c8381a7 559 // MOTOR CALIBRATION MODE
Jellehierck 38:8b597ab8344f 560 void do_global_motor_cal()
Jellehierck 26:7e81c7db6e7a 561 {
Jellehierck 25:a1be4cf2ab0b 562 // Entry function
Jellehierck 37:806c7c8381a7 563 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 564 global_state_changed = false;
Jellehierck 25:a1be4cf2ab0b 565 }
Jellehierck 25:a1be4cf2ab0b 566
Jellehierck 25:a1be4cf2ab0b 567 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 568 doStuff();
Jellehierck 28:59e8266f4633 569
Jellehierck 25:a1be4cf2ab0b 570 // State transition guard
Jellehierck 38:8b597ab8344f 571 if ( emg_cal_done == true ) { // OPERATION MODE
Jellehierck 38:8b597ab8344f 572 motor_cal_done = true;
Jellehierck 37:806c7c8381a7 573 global_curr_state = global_operation;
Jellehierck 37:806c7c8381a7 574 global_state_changed = true;
Jellehierck 37:806c7c8381a7 575 } else if ( button2_pressed == true ) { // WAIT MODE
Jellehierck 37:806c7c8381a7 576 button2_pressed = false;
Jellehierck 38:8b597ab8344f 577 motor_cal_done = true;
Jellehierck 37:806c7c8381a7 578 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 579 global_state_changed = true;
Jellehierck 23:8a0a0b959af1 580 }
Jellehierck 23:8a0a0b959af1 581 }
Jellehierck 23:8a0a0b959af1 582
Jellehierck 37:806c7c8381a7 583 // OPERATION MODE
Jellehierck 37:806c7c8381a7 584 void do_global_operation()
Jellehierck 37:806c7c8381a7 585 {
Jellehierck 37:806c7c8381a7 586 // Entry function
Jellehierck 37:806c7c8381a7 587 if ( global_state_changed == true ) {
Jellehierck 37:806c7c8381a7 588 global_state_changed = false;
Jellehierck 37:806c7c8381a7 589 }
Jellehierck 37:806c7c8381a7 590
Jellehierck 37:806c7c8381a7 591 // Do stuff until end condition is met
Jellehierck 37:806c7c8381a7 592 doStuff();
Jellehierck 37:806c7c8381a7 593
Jellehierck 37:806c7c8381a7 594 // State transition guard
Jellehierck 37:806c7c8381a7 595 if ( false ) { // Always stay in operation mode (can be changed)
Jellehierck 37:806c7c8381a7 596 global_curr_state = global_wait;
Jellehierck 37:806c7c8381a7 597 global_state_changed = true;
Jellehierck 37:806c7c8381a7 598 }
Jellehierck 37:806c7c8381a7 599 }
Jellehierck 23:8a0a0b959af1 600 /*
Jellehierck 37:806c7c8381a7 601 ------------------------------ GLOBAL STATE MACHINE ------------------------------
Jellehierck 23:8a0a0b959af1 602 */
Jellehierck 37:806c7c8381a7 603 void global_state_machine()
Jellehierck 23:8a0a0b959af1 604 {
Jellehierck 37:806c7c8381a7 605 switch(global_curr_state) {
Jellehierck 37:806c7c8381a7 606 case global_failure:
Jellehierck 37:806c7c8381a7 607 do_global_failure();
Jellehierck 23:8a0a0b959af1 608 break;
Jellehierck 37:806c7c8381a7 609 case global_wait:
Jellehierck 37:806c7c8381a7 610 do_global_wait();
Jellehierck 37:806c7c8381a7 611 break;
Jellehierck 38:8b597ab8344f 612 case global_emg_cal:
Jellehierck 38:8b597ab8344f 613 do_global_emg_cal();
Jellehierck 38:8b597ab8344f 614 emg_state_machine();
Jellehierck 23:8a0a0b959af1 615 break;
Jellehierck 38:8b597ab8344f 616 case global_motor_cal:
Jellehierck 38:8b597ab8344f 617 do_global_motor_cal();
Jellehierck 23:8a0a0b959af1 618 break;
Jellehierck 37:806c7c8381a7 619 case global_operation:
Jellehierck 37:806c7c8381a7 620 do_global_operation();
Jellehierck 37:806c7c8381a7 621 break;
Jellehierck 37:806c7c8381a7 622 case global_demo:
Jellehierck 37:806c7c8381a7 623 do_global_demo();
Jellehierck 23:8a0a0b959af1 624 break;
Jellehierck 23:8a0a0b959af1 625 }
Jellehierck 23:8a0a0b959af1 626 }
Jellehierck 23:8a0a0b959af1 627
Jellehierck 38:8b597ab8344f 628 /*
Jellehierck 38:8b597ab8344f 629 ------------------------------ READ SAMPLES ------------------------------
Jellehierck 38:8b597ab8344f 630 */
Jellehierck 38:8b597ab8344f 631 void sampleSignals()
Jellehierck 38:8b597ab8344f 632 {
Jellehierck 38:8b597ab8344f 633 if (emg_sampleNow == true) { // This ticker only samples if the sample flag is true, to prevent unnecessary computations
Jellehierck 38:8b597ab8344f 634 // Read EMG inputs
Jellehierck 38:8b597ab8344f 635 emg1 = emg1_in.read();
Jellehierck 38:8b597ab8344f 636 emg2 = emg2_in.read();
Jellehierck 38:8b597ab8344f 637 emg3 = emg3_in.read();
Jellehierck 38:8b597ab8344f 638
Jellehierck 38:8b597ab8344f 639
Jellehierck 38:8b597ab8344f 640 double emg1_n = bqc1_notch.step( emg1 ); // Filter notch
Jellehierck 38:8b597ab8344f 641 double emg1_hp = bqc1_high.step( emg1_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 642 double emg1_rectify = fabs( emg1_hp ); // Rectify
Jellehierck 38:8b597ab8344f 643 emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 644
Jellehierck 38:8b597ab8344f 645 double emg2_n = bqc2_notch.step( emg2 ); // Filter notch
Jellehierck 38:8b597ab8344f 646 double emg2_hp = bqc2_high.step( emg2_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 647 double emg2_rectify = fabs( emg2_hp ); // Rectify
Jellehierck 38:8b597ab8344f 648 emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 649
Jellehierck 38:8b597ab8344f 650 double emg3_n = bqc3_notch.step( emg3 ); // Filter notch
Jellehierck 38:8b597ab8344f 651 double emg3_hp = bqc3_high.step( emg3_n ); // Filter highpass
Jellehierck 38:8b597ab8344f 652 double emg3_rectify = fabs( emg3_hp ); // Rectify
Jellehierck 38:8b597ab8344f 653 emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
Jellehierck 38:8b597ab8344f 654
Jellehierck 38:8b597ab8344f 655 if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true
Jellehierck 38:8b597ab8344f 656 emg1_cal.push_back(emg1_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 657 // emg1_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 658 emg2_cal.push_back(emg2_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 659 // emg2_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 660 emg3_cal.push_back(emg3_env); // Add values to calibration vector
Jellehierck 38:8b597ab8344f 661 // emg3_cal_size = emg1_cal.size(); // Used for debugging
Jellehierck 38:8b597ab8344f 662 }
Jellehierck 38:8b597ab8344f 663 }
Jellehierck 38:8b597ab8344f 664 }
Jellehierck 37:806c7c8381a7 665
Jellehierck 37:806c7c8381a7 666 /*
Jellehierck 37:806c7c8381a7 667 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
Jellehierck 37:806c7c8381a7 668 */
Jellehierck 25:a1be4cf2ab0b 669 void tickGlobalFunc()
Jellehierck 25:a1be4cf2ab0b 670 {
Jellehierck 38:8b597ab8344f 671 sampleSignals();
Jellehierck 37:806c7c8381a7 672 global_state_machine();
Jellehierck 25:a1be4cf2ab0b 673 // controller();
Jellehierck 25:a1be4cf2ab0b 674 // outputToMotors();
Jellehierck 25:a1be4cf2ab0b 675 }
Jellehierck 25:a1be4cf2ab0b 676
Jellehierck 37:806c7c8381a7 677 /*
Jellehierck 37:806c7c8381a7 678 ------------------------------ MAIN FUNCTION ------------------------------
Jellehierck 37:806c7c8381a7 679 */
Jellehierck 23:8a0a0b959af1 680 void main()
Jellehierck 23:8a0a0b959af1 681 {
Jellehierck 23:8a0a0b959af1 682 pc.baud(115200); // MODSERIAL rate
Jellehierck 23:8a0a0b959af1 683 pc.printf("Starting\r\n");
Jellehierck 23:8a0a0b959af1 684
Jellehierck 37:806c7c8381a7 685 global_curr_state = global_wait; // Start off in EMG Wait state
Jellehierck 34:13fac02ef324 686 tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker
Jellehierck 8:ea3de43c9e8b 687
Jellehierck 38:8b597ab8344f 688 // ---------- Attach filters ----------
Jellehierck 38:8b597ab8344f 689 bqc1_notch.add( &bq1_notch );
Jellehierck 38:8b597ab8344f 690 bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
Jellehierck 38:8b597ab8344f 691 bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
Jellehierck 38:8b597ab8344f 692
Jellehierck 38:8b597ab8344f 693 bqc2_notch.add( &bq2_notch );
Jellehierck 38:8b597ab8344f 694 bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
Jellehierck 38:8b597ab8344f 695 bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
Jellehierck 38:8b597ab8344f 696
Jellehierck 38:8b597ab8344f 697 bqc3_notch.add( &bq3_notch );
Jellehierck 38:8b597ab8344f 698 bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
Jellehierck 38:8b597ab8344f 699 bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
Jellehierck 38:8b597ab8344f 700
Jellehierck 38:8b597ab8344f 701 // ---------- Attach buttons ----------
Jellehierck 37:806c7c8381a7 702 button1.fall( &button1Press );
Jellehierck 37:806c7c8381a7 703 button2.fall( &button2Press );
Jellehierck 37:806c7c8381a7 704 switch2.fall( &switch2Press );
Jellehierck 37:806c7c8381a7 705 switch3.fall( &switch3Press );
Jellehierck 38:8b597ab8344f 706
Jellehierck 38:8b597ab8344f 707 // ---------- Turn OFF LEDs ----------
Jellehierck 38:8b597ab8344f 708 led_b = 1;
Jellehierck 38:8b597ab8344f 709 led_g = 1;
Jellehierck 38:8b597ab8344f 710 led_r = 1;
Jellehierck 37:806c7c8381a7 711
Jellehierck 23:8a0a0b959af1 712 while(true) {
Jellehierck 38:8b597ab8344f 713 pc.printf("Global state: %i EMG substate: %i\r\n", global_curr_state, emg_curr_state);
Jellehierck 30:bac3b60d6283 714 wait(0.5f);
Jellehierck 23:8a0a0b959af1 715 }
Jellehierck 23:8a0a0b959af1 716 }