M9 Biorobotics Group 8 / Mbed 2 deprecated EMGfiltering_EMG

Dit is alleen het EMG gedeelte

Dependencies:   mbed HIDScope biquadFilter MODSERIAL FXOS8700Q

main.cpp@9:f6a935be28e1, 2019-10-21 (annotated)

Committer:
Jellehierck
Date:
Mon Oct 21 11:57:25 2019 +0000
Revision:
9:f6a935be28e1
Parent:
8:ea3de43c9e8b 
Vector removed, replaced with array methods

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IsaRobin 0:6972d0e91af1 1 //c++ script for filtering of measured EMG signals
IsaRobin 0:6972d0e91af1 2 #include "mbed.h" //Base library
IsaRobin 0:6972d0e91af1 3 #include "HIDScope.h" // to see if program is working and EMG is filtered properly
Jellehierck 2:d3e9788ab1b3 4 // #include "QEI.h"// is needed for the encoder
Jellehierck 8:ea3de43c9e8b 5 #include "MODSERIAL.h"// in order for connection with the pc
Jellehierck 2:d3e9788ab1b3 6 #include "BiQuad.h"
Jellehierck 2:d3e9788ab1b3 7 // #include "FastPWM.h"
Jellehierck 2:d3e9788ab1b3 8 // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?)
Jellehierck 2:d3e9788ab1b3 9 // #include "EMGFilters.h"
IsaRobin 0:6972d0e91af1 10 #include <vector> // For easy array management
Jellehierck 7:7a088536f1c9 11 #include <numeric> // For manipulating array data
IsaRobin 0:6972d0e91af1 12
Jellehierck 2:d3e9788ab1b3 13 // PC serial connection
Jellehierck 4:09a01d2db8f7 14 HIDScope scope( 2 );
Jellehierck 8:ea3de43c9e8b 15 MODSERIAL pc(USBTX, USBRX);
IsaRobin 0:6972d0e91af1 16
IsaRobin 0:6972d0e91af1 17 //EMG inputs definieren
IsaRobin 0:6972d0e91af1 18 AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
IsaRobin 0:6972d0e91af1 19 AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
IsaRobin 0:6972d0e91af1 20 AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
IsaRobin 0:6972d0e91af1 21
Jellehierck 4:09a01d2db8f7 22 // LED
Jellehierck 6:5437cc97e1e6 23 DigitalOut led_g(LED_GREEN);
Jellehierck 6:5437cc97e1e6 24 DigitalOut led_r(LED_RED);
Jellehierck 8:ea3de43c9e8b 25 DigitalOut led_b(LED_BLUE);
Jellehierck 8:ea3de43c9e8b 26
Jellehierck 8:ea3de43c9e8b 27 // Buttons
Jellehierck 8:ea3de43c9e8b 28 InterruptIn button1(D11);
Jellehierck 8:ea3de43c9e8b 29 InterruptIn button2(D10);
Jellehierck 4:09a01d2db8f7 30
IsaRobin 0:6972d0e91af1 31 //variablen voor EMG
Jellehierck 9:f6a935be28e1 32 volatile double emg1;
Jellehierck 9:f6a935be28e1 33 volatile double emg2;
Jellehierck 9:f6a935be28e1 34 volatile double emg3;
IsaRobin 0:6972d0e91af1 35
Jellehierck 4:09a01d2db8f7 36 // Initialize tickers
Jellehierck 4:09a01d2db8f7 37 Ticker tickSample;
Jellehierck 7:7a088536f1c9 38 Timeout timeoutCalibrationMVC;
Jellehierck 7:7a088536f1c9 39 Ticker tickSampleCalibration;
Jellehierck 4:09a01d2db8f7 40
Jellehierck 9:f6a935be28e1 41 // Initialize values
Jellehierck 9:f6a935be28e1 42 const double Fs = 100; // sampling frequency (Hz)
Jellehierck 9:f6a935be28e1 43 const double Ts = 1/Fs; // sampling time (s)
Jellehierck 9:f6a935be28e1 44 const double T_cal = 5.0f; // Calibration duration (s)
Jellehierck 9:f6a935be28e1 45
Jellehierck 9:f6a935be28e1 46
Jellehierck 9:f6a935be28e1 47 int i_cal = 0; // Global iterator of calibration
Jellehierck 9:f6a935be28e1 48 const int n_cal = Fs * T_cal; // Number of elements of calibration
Jellehierck 9:f6a935be28e1 49 double emg1_cal[n_cal]; // Initialize calibration array
Jellehierck 4:09a01d2db8f7 50
Jellehierck 3:c0ece64850db 51 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 52 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 53 BiQuad bq_notch(0.995636295063941, -1.89829218816065, 0.995636295063941, 1, -1.89829218816065, 0.991272590127882);
Jellehierck 1:059cca298369 54
Jellehierck 3:c0ece64850db 55 // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 56 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 57 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 58 BiQuad bq_H1(0.922946103200875, -1.84589220640175, 0.922946103200875, 1, -1.88920703055163, 0.892769008131025);
Jellehierck 3:c0ece64850db 59 BiQuad bq_H2(1, -2, 1, 1, -1.95046575793011, 0.954143234875078);
Jellehierck 4:09a01d2db8f7 60 BiQuadChain bqc_notch_high; // Used to chain two 2nd other filters into a 4th order filter
IsaRobin 0:6972d0e91af1 61
Jellehierck 3:c0ece64850db 62 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
Jellehierck 3:c0ece64850db 63 // b01 b11 b21 a01 a11 a21
Jellehierck 3:c0ece64850db 64 // b02 b12 b22 a02 a12 a22
Jellehierck 3:c0ece64850db 65 BiQuad bq_L1(5.32116245737504e-08, 1.06423249147501e-07, 5.32116245737504e-08, 1, -1.94396715039462, 0.944882378004138);
Jellehierck 3:c0ece64850db 66 BiQuad bq_L2(1, 2, 1, 1, -1.97586467534468, 0.976794920438162);
Jellehierck 3:c0ece64850db 67 BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
Jellehierck 2:d3e9788ab1b3 68
Jellehierck 9:f6a935be28e1 69 double getMean(double vect[n_cal])
Jellehierck 7:7a088536f1c9 70 {
Jellehierck 8:ea3de43c9e8b 71 double sum = 0.0;
Jellehierck 9:f6a935be28e1 72 int vect_n = n_cal;
Jellehierck 8:ea3de43c9e8b 73
Jellehierck 8:ea3de43c9e8b 74 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 8:ea3de43c9e8b 75 sum += vect[i];
Jellehierck 8:ea3de43c9e8b 76 }
Jellehierck 8:ea3de43c9e8b 77 return sum/vect_n;
Jellehierck 8:ea3de43c9e8b 78 }
Jellehierck 8:ea3de43c9e8b 79
Jellehierck 9:f6a935be28e1 80 double getStdev(double vect[n_cal], double vect_mean)
Jellehierck 8:ea3de43c9e8b 81 {
Jellehierck 8:ea3de43c9e8b 82 double sum2 = 0.0;
Jellehierck 9:f6a935be28e1 83 int vect_n = n_cal;
Jellehierck 8:ea3de43c9e8b 84
Jellehierck 8:ea3de43c9e8b 85 for ( int i = 0; i < vect_n; i++ ) {
Jellehierck 8:ea3de43c9e8b 86 sum2 += pow( vect[i] - vect_mean, 2 );
Jellehierck 8:ea3de43c9e8b 87 }
Jellehierck 8:ea3de43c9e8b 88 double output = sqrt( sum2 / vect_n );
Jellehierck 8:ea3de43c9e8b 89 return output;
Jellehierck 7:7a088536f1c9 90 }
Jellehierck 7:7a088536f1c9 91
Jellehierck 6:5437cc97e1e6 92 // Check if filters are stable
Jellehierck 6:5437cc97e1e6 93 bool checkBQChainStable()
Jellehierck 6:5437cc97e1e6 94 {
Jellehierck 6:5437cc97e1e6 95 bool n_hp_stable = bqc_notch_high.stable();
Jellehierck 6:5437cc97e1e6 96 bool l_stable = bqc_low.stable();
Jellehierck 6:5437cc97e1e6 97
Jellehierck 6:5437cc97e1e6 98 if (n_hp_stable && l_stable) {
Jellehierck 6:5437cc97e1e6 99 return true;
Jellehierck 6:5437cc97e1e6 100 } else {
Jellehierck 6:5437cc97e1e6 101 return false;
Jellehierck 6:5437cc97e1e6 102 }
Jellehierck 6:5437cc97e1e6 103 }
Jellehierck 6:5437cc97e1e6 104
Jellehierck 9:f6a935be28e1 105 /*
Jellehierck 6:5437cc97e1e6 106 // Read samples, filter samples and output to HIDScope
Jellehierck 2:d3e9788ab1b3 107 void sample()
Jellehierck 2:d3e9788ab1b3 108 {
Jellehierck 4:09a01d2db8f7 109 // Read EMG inputs
Jellehierck 2:d3e9788ab1b3 110 emg1 = emg1_in.read();
Jellehierck 2:d3e9788ab1b3 111 emg2 = emg2_in.read();
Jellehierck 2:d3e9788ab1b3 112 emg3 = emg3_in.read();
Jellehierck 4:09a01d2db8f7 113
Jellehierck 4:09a01d2db8f7 114 // Output raw EMG input
Jellehierck 4:09a01d2db8f7 115 scope.set(0, emg1 );
Jellehierck 6:5437cc97e1e6 116
Jellehierck 5:3d65f89e3755 117 // Filter notch and highpass
Jellehierck 5:3d65f89e3755 118 double emg1_n_hp = bqc_notch_high.step( emg1 );
Jellehierck 6:5437cc97e1e6 119
Jellehierck 5:3d65f89e3755 120 // Rectify
Jellehierck 5:3d65f89e3755 121 double emg1_rectify = fabs( emg1_n_hp );
Jellehierck 6:5437cc97e1e6 122
Jellehierck 5:3d65f89e3755 123 // Filter lowpass (completes envelope)
Jellehierck 5:3d65f89e3755 124 double emg1_env = bqc_low.step( emg1_rectify );
Jellehierck 4:09a01d2db8f7 125
Jellehierck 4:09a01d2db8f7 126 // Output EMG after filters
Jellehierck 5:3d65f89e3755 127 scope.set(1, emg1_env );
Jellehierck 4:09a01d2db8f7 128 scope.send();
Jellehierck 9:f6a935be28e1 129 }
Jellehierck 9:f6a935be28e1 130 */
Jellehierck 7:7a088536f1c9 131
Jellehierck 9:f6a935be28e1 132 void calibrationMVCFinished()
Jellehierck 9:f6a935be28e1 133 {
Jellehierck 9:f6a935be28e1 134 // pc.printf("Stop calibration");
Jellehierck 9:f6a935be28e1 135 tickSampleCalibration.detach();
Jellehierck 9:f6a935be28e1 136 double emg1_mean = getMean(emg1_cal);
Jellehierck 9:f6a935be28e1 137 double emg1_stdev = getStdev(emg1_cal, emg1_mean);
Jellehierck 7:7a088536f1c9 138
Jellehierck 9:f6a935be28e1 139 led_b = 1;
Jellehierck 9:f6a935be28e1 140
Jellehierck 9:f6a935be28e1 141 // pc.printf("EMG Mean: %f stdev: %f\r\n", emg1_mean, emg1_stdev);
Jellehierck 2:d3e9788ab1b3 142 }
IsaRobin 0:6972d0e91af1 143
Jellehierck 7:7a088536f1c9 144 void sampleCalibration()
Jellehierck 7:7a088536f1c9 145 {
Jellehierck 9:f6a935be28e1 146 if (i_cal >= n_cal-1) {
Jellehierck 9:f6a935be28e1 147 calibrationMVCFinished();
Jellehierck 9:f6a935be28e1 148 }
Jellehierck 7:7a088536f1c9 149 // Read EMG inputs
Jellehierck 7:7a088536f1c9 150 emg1 = emg1_in.read();
Jellehierck 7:7a088536f1c9 151 emg2 = emg2_in.read();
Jellehierck 7:7a088536f1c9 152 emg3 = emg3_in.read();
Jellehierck 7:7a088536f1c9 153
Jellehierck 7:7a088536f1c9 154 // Output raw EMG input
Jellehierck 7:7a088536f1c9 155 scope.set(0, emg1 );
Jellehierck 7:7a088536f1c9 156
Jellehierck 7:7a088536f1c9 157 double emg1_n_hp = bqc_notch_high.step( emg1 ); // Filter notch and highpass
Jellehierck 7:7a088536f1c9 158 double emg1_rectify = fabs( emg1_n_hp ); // Filter lowpass (completes envelope)
Jellehierck 7:7a088536f1c9 159 double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
Jellehierck 7:7a088536f1c9 160
Jellehierck 7:7a088536f1c9 161 // Output EMG after filters
Jellehierck 7:7a088536f1c9 162 scope.set(1, emg1_env );
Jellehierck 7:7a088536f1c9 163 scope.send();
Jellehierck 7:7a088536f1c9 164
Jellehierck 9:f6a935be28e1 165 emg1_cal[i_cal] = emg1_env;
Jellehierck 9:f6a935be28e1 166 i_cal++;
Jellehierck 7:7a088536f1c9 167
Jellehierck 7:7a088536f1c9 168 }
Jellehierck 7:7a088536f1c9 169
Jellehierck 7:7a088536f1c9 170 void calibrationMVC()
Jellehierck 7:7a088536f1c9 171 {
Jellehierck 9:f6a935be28e1 172
Jellehierck 7:7a088536f1c9 173 tickSampleCalibration.attach( &sampleCalibration, Ts );
Jellehierck 8:ea3de43c9e8b 174 led_b = 0;
Jellehierck 7:7a088536f1c9 175 }
Jellehierck 7:7a088536f1c9 176
Jellehierck 7:7a088536f1c9 177
Jellehierck 7:7a088536f1c9 178
Jellehierck 5:3d65f89e3755 179 void main()
Jellehierck 4:09a01d2db8f7 180 {
Jellehierck 9:f6a935be28e1 181 pc.baud(115200);
Jellehierck 8:ea3de43c9e8b 182 pc.printf("Starting\r\n");
Jellehierck 9:f6a935be28e1 183
Jellehierck 6:5437cc97e1e6 184 // Initialize sample ticker
Jellehierck 8:ea3de43c9e8b 185 // tickSample.attach(&sample, Ts);
Jellehierck 6:5437cc97e1e6 186
Jellehierck 6:5437cc97e1e6 187 // Create BQ chains to reduce computations
Jellehierck 5:3d65f89e3755 188 bqc_notch_high.add( &bq_notch ).add( &bq_H1 ).add( &bq_H2 );
Jellehierck 5:3d65f89e3755 189 bqc_low.add( &bq_L1 ).add( &bq_L2 );
Jellehierck 4:09a01d2db8f7 190
Jellehierck 8:ea3de43c9e8b 191 led_b = 1; // Turn led off at startup
Jellehierck 8:ea3de43c9e8b 192 led_g = 1;
Jellehierck 8:ea3de43c9e8b 193
Jellehierck 6:5437cc97e1e6 194 // If any filter chain is unstable, red led will light up
Jellehierck 6:5437cc97e1e6 195 if (checkBQChainStable) {
Jellehierck 6:5437cc97e1e6 196 led_r = 1; // LED off
Jellehierck 6:5437cc97e1e6 197 } else {
Jellehierck 6:5437cc97e1e6 198 led_r = 0; // LED on
Jellehierck 6:5437cc97e1e6 199 }
Jellehierck 6:5437cc97e1e6 200
Jellehierck 8:ea3de43c9e8b 201 button1.fall( &calibrationMVC );
Jellehierck 8:ea3de43c9e8b 202
Jellehierck 4:09a01d2db8f7 203 while(true) {
Jellehierck 7:7a088536f1c9 204
Jellehierck 6:5437cc97e1e6 205 // Show that system is running
Jellehierck 8:ea3de43c9e8b 206 // led_g = !led_g;
Jellehierck 4:09a01d2db8f7 207 wait(0.5);
Jellehierck 4:09a01d2db8f7 208 }
Jellehierck 4:09a01d2db8f7 209 }