Nahuel Manterola / Mbed 2 deprecated EMGfilter_2

De EMG Lowpass maakt alle signalen gelijk

Dependencies:   HIDScope biquadFilter mbed

Fork of EMGfilter by Pascal van Baardwijk

main.cpp@6:da06585e106c, 2016-10-25 (annotated)

Committer:
pbaardwijk
Date:
Tue Oct 25 20:38:53 2016 +0000
Revision:
6:da06585e106c
Parent:
5:02b3550e1ff0
Child:
7:a928724ef731
EMG filter with low-pass at 200Hz to avoid alliasing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbaardwijk 0:ae0bec143f2d 1 #include "mbed.h"
pbaardwijk 0:ae0bec143f2d 2 #include "BiQuad.h"
pbaardwijk 0:ae0bec143f2d 3 #include "HIDScope.h"
pbaardwijk 0:ae0bec143f2d 4 //Enum with states
pbaardwijk 0:ae0bec143f2d 5 enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN};
pbaardwijk 0:ae0bec143f2d 6
pbaardwijk 0:ae0bec143f2d 7 //Variable called 'state'
pbaardwijk 0:ae0bec143f2d 8 states state = STATE_DEFAULT;
pbaardwijk 0:ae0bec143f2d 9
pbaardwijk 0:ae0bec143f2d 10 //Creating two scope channels
pbaardwijk 3:cdac0bfafc80 11 HIDScope scope(3);
pbaardwijk 0:ae0bec143f2d 12
pbaardwijk 6:da06585e106c 13 //Low-pass filter at 200Hz to avoid alliasing
pbaardwijk 6:da06585e106c 14 BiQuadChain low_pass_200;
pbaardwijk 6:da06585e106c 15 BiQuad bq1( 0.73638326364, 1.47272205824, 0.73638326364, 1.36239603912, 0.48048795767);
pbaardwijk 6:da06585e106c 16 BiQuad bq2( 0.83856791742, 1.67684080778, 0.83856791742, 1.68114429812, 0.7942832489);
pbaardwijk 6:da06585e106c 17
pbaardwijk 0:ae0bec143f2d 18 //Notch filter
pbaardwijk 0:ae0bec143f2d 19 BiQuadChain notch_50;
pbaardwijk 6:da06585e106c 20 BiQuad bq3( 1.00007369335,-1.61815834791, 1.00007369335, -1.60983662066, 0.98986119869);
pbaardwijk 6:da06585e106c 21 BiQuad bq4( 1.00000000000,-1.61803910919, 1.00000000000, -1.60936554071, 0.99545624832);
pbaardwijk 6:da06585e106c 22 BiQuad bq5( 0.99035034846,-1.60242559561, 0.99035034846, -1.61934542233, 0.9955088075);
pbaardwijk 0:ae0bec143f2d 23
pbaardwijk 0:ae0bec143f2d 24 //High pass filter
pbaardwijk 0:ae0bec143f2d 25 BiQuadChain high_pass;
pbaardwijk 6:da06585e106c 26 BiQuad bq6( 0.83315051810,-1.66630103620, 0.83315051810, -1.55025104412, 0.60696783282);
pbaardwijk 6:da06585e106c 27 BiQuad bq7( 0.86554941044,-1.73109882088, 0.86554941044, -1.74142633961, 0.78400451004);
pbaardwijk 6:da06585e106c 28 BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
pbaardwijk 0:ae0bec143f2d 29
pbaardwijk 5:02b3550e1ff0 30 //Low pass filter
pbaardwijk 5:02b3550e1ff0 31 BiQuadChain low_pass;
pbaardwijk 6:da06585e106c 32 BiQuad bq9( 0.00040400257, 0.00080800515, 0.00040400257,-1.92223307595, 0.92384908624);
pbaardwijk 6:da06585e106c 33 BiQuad bq10( 0.00040816681, 0.00081633362, 0.00040816681,-1.94204639240, 0.94367905964);
pbaardwijk 6:da06585e106c 34 BiQuad bq11( 0.00041558628, 0.00083117256, 0.00041558628,-1.97734803172, 0.9790103768);
pbaardwijk 5:02b3550e1ff0 35
pbaardwijk 0:ae0bec143f2d 36 //Ticker
pbaardwijk 0:ae0bec143f2d 37 Ticker emgSampleTicker;
pbaardwijk 0:ae0bec143f2d 38
pbaardwijk 0:ae0bec143f2d 39 //Timeout to change state after 5 seconds
pbaardwijk 0:ae0bec143f2d 40 Timeout change_state;
pbaardwijk 0:ae0bec143f2d 41
pbaardwijk 1:7fb4a74d33ff 42 //Timeout to change state after 15 seconds
pbaardwijk 0:ae0bec143f2d 43 Timeout change_state2;
pbaardwijk 0:ae0bec143f2d 44
pbaardwijk 1:7fb4a74d33ff 45 //LED
pbaardwijk 1:7fb4a74d33ff 46 DigitalOut led(LED_RED);
pbaardwijk 1:7fb4a74d33ff 47
pbaardwijk 0:ae0bec143f2d 48 //Emg input
pbaardwijk 0:ae0bec143f2d 49 AnalogIn emg0( A0 );
pbaardwijk 0:ae0bec143f2d 50 AnalogIn emg1( A1 );
pbaardwijk 0:ae0bec143f2d 51 AnalogIn emg2( A2 );
pbaardwijk 0:ae0bec143f2d 52
pbaardwijk 0:ae0bec143f2d 53 bool go_emgSample;
pbaardwijk 0:ae0bec143f2d 54 bool go_find_minmax;
pbaardwijk 0:ae0bec143f2d 55 double emg_sample[3];
pbaardwijk 6:da06585e106c 56 double emg_low_passed_200[3];
pbaardwijk 0:ae0bec143f2d 57 double emg_notch[3];
pbaardwijk 0:ae0bec143f2d 58 double emg_high_passed[3];
pbaardwijk 5:02b3550e1ff0 59 double emg_low_passed[3];
pbaardwijk 0:ae0bec143f2d 60 double min_emg[3];
pbaardwijk 0:ae0bec143f2d 61 double max_emg[3];
pbaardwijk 0:ae0bec143f2d 62
pbaardwijk 0:ae0bec143f2d 63 const int n = 200;
pbaardwijk 0:ae0bec143f2d 64 int counter = 0;
pbaardwijk 0:ae0bec143f2d 65 double RMSArray0[n] = {0};
pbaardwijk 0:ae0bec143f2d 66 double RMSArray1[n] = {0};
pbaardwijk 0:ae0bec143f2d 67 double RMSArray2[n] = {0};
pbaardwijk 0:ae0bec143f2d 68 double RMS0;
pbaardwijk 0:ae0bec143f2d 69 double RMS1;
pbaardwijk 0:ae0bec143f2d 70 double RMS2;
pbaardwijk 0:ae0bec143f2d 71 double SumRMS0;
pbaardwijk 0:ae0bec143f2d 72 double SumRMS1;
pbaardwijk 0:ae0bec143f2d 73 double SumRMS2;
pbaardwijk 0:ae0bec143f2d 74
pbaardwijk 0:ae0bec143f2d 75 double input_force0;
pbaardwijk 0:ae0bec143f2d 76 double input_force1;
pbaardwijk 0:ae0bec143f2d 77 double input_force2;
pbaardwijk 0:ae0bec143f2d 78
pbaardwijk 0:ae0bec143f2d 79 //count for emg min max
pbaardwijk 0:ae0bec143f2d 80 int start_calibration = 0;
pbaardwijk 0:ae0bec143f2d 81
pbaardwijk 0:ae0bec143f2d 82 void emgSample() {
pbaardwijk 0:ae0bec143f2d 83 go_emgSample = true;
pbaardwijk 0:ae0bec143f2d 84 }
pbaardwijk 0:ae0bec143f2d 85
pbaardwijk 0:ae0bec143f2d 86 void calibrate() {
pbaardwijk 0:ae0bec143f2d 87 state = STATE_CALIBRATION;
pbaardwijk 1:7fb4a74d33ff 88 led.write(0);
pbaardwijk 0:ae0bec143f2d 89 }
pbaardwijk 0:ae0bec143f2d 90
pbaardwijk 0:ae0bec143f2d 91 void run() {
pbaardwijk 0:ae0bec143f2d 92 state = STATE_RUN;
pbaardwijk 1:7fb4a74d33ff 93 led.write(1);
pbaardwijk 0:ae0bec143f2d 94 }
pbaardwijk 0:ae0bec143f2d 95
pbaardwijk 0:ae0bec143f2d 96 void EMG_filter();
pbaardwijk 0:ae0bec143f2d 97
pbaardwijk 0:ae0bec143f2d 98 int main() {
pbaardwijk 0:ae0bec143f2d 99 //combine biquads in biquad chains for notch/high- low-pass filters
pbaardwijk 6:da06585e106c 100 low_pass_200.add( &bq1 ).add( &bq2 );
pbaardwijk 6:da06585e106c 101 notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 );
pbaardwijk 6:da06585e106c 102 high_pass.add( &bq6 ).add( &bq7 ).add( &bq8 );
pbaardwijk 6:da06585e106c 103 low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 );
pbaardwijk 1:7fb4a74d33ff 104 led.write(1);
pbaardwijk 0:ae0bec143f2d 105
pbaardwijk 0:ae0bec143f2d 106 change_state.attach( &calibrate,5);
pbaardwijk 1:7fb4a74d33ff 107 change_state2.attach( &run,15);
pbaardwijk 0:ae0bec143f2d 108 emgSampleTicker.attach( &emgSample, 0.002);
pbaardwijk 0:ae0bec143f2d 109 while( true ){
pbaardwijk 0:ae0bec143f2d 110 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 111 EMG_filter();
pbaardwijk 0:ae0bec143f2d 112 }
pbaardwijk 0:ae0bec143f2d 113 }
pbaardwijk 0:ae0bec143f2d 114 }
pbaardwijk 0:ae0bec143f2d 115
pbaardwijk 0:ae0bec143f2d 116
pbaardwijk 0:ae0bec143f2d 117 void EMG_filter() {
pbaardwijk 0:ae0bec143f2d 118 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 119 //read the emg signal
pbaardwijk 0:ae0bec143f2d 120 emg_sample[0] = emg0.read();
pbaardwijk 0:ae0bec143f2d 121 emg_sample[1] = emg1.read();
pbaardwijk 0:ae0bec143f2d 122 emg_sample[2] = emg2.read();
pbaardwijk 0:ae0bec143f2d 123
pbaardwijk 0:ae0bec143f2d 124 for (int i = 0; i < 3; i++){
pbaardwijk 6:da06585e106c 125 //low pass at 200Hz to avoid alliasing
pbaardwijk 6:da06585e106c 126 emg_low_passed_200[i] = low_pass_200.step(emg_sample[i]);
pbaardwijk 6:da06585e106c 127
pbaardwijk 0:ae0bec143f2d 128 //filter out the 50Hz components with a notch filter
pbaardwijk 6:da06585e106c 129 emg_notch[i] = notch_50.step(emg_low_passed_200[i]);
pbaardwijk 0:ae0bec143f2d 130
pbaardwijk 0:ae0bec143f2d 131 //high pass the signal (removing motion artifacts and offset)
pbaardwijk 3:cdac0bfafc80 132 emg_high_passed[i] = high_pass.step(emg_notch[i]);
pbaardwijk 5:02b3550e1ff0 133
pbaardwijk 5:02b3550e1ff0 134 //low pass the rectified emg signal
pbaardwijk 5:02b3550e1ff0 135 emg_low_passed[i] = low_pass.step(fabs(emg_high_passed[i]));
pbaardwijk 0:ae0bec143f2d 136 }
pbaardwijk 0:ae0bec143f2d 137
pbaardwijk 0:ae0bec143f2d 138 //Calculating RMS
pbaardwijk 0:ae0bec143f2d 139 SumRMS0 -= pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 140 SumRMS1 -= pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 141 SumRMS2 -= pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 142
pbaardwijk 0:ae0bec143f2d 143 RMSArray0[counter] = emg_high_passed[0];
pbaardwijk 0:ae0bec143f2d 144 RMSArray1[counter] = emg_high_passed[1];
pbaardwijk 0:ae0bec143f2d 145 RMSArray2[counter] = emg_high_passed[2];
pbaardwijk 0:ae0bec143f2d 146
pbaardwijk 0:ae0bec143f2d 147 SumRMS0 += pow(RMSArray0[counter],2);
pbaardwijk 0:ae0bec143f2d 148 SumRMS1 += pow(RMSArray1[counter],2);
pbaardwijk 0:ae0bec143f2d 149 SumRMS2 += pow(RMSArray2[counter],2);
pbaardwijk 0:ae0bec143f2d 150
pbaardwijk 0:ae0bec143f2d 151 counter++;
pbaardwijk 0:ae0bec143f2d 152 if (counter == n){
pbaardwijk 0:ae0bec143f2d 153 counter = 0;
pbaardwijk 0:ae0bec143f2d 154 }
pbaardwijk 0:ae0bec143f2d 155
pbaardwijk 0:ae0bec143f2d 156 RMS0 = sqrt(SumRMS0/n);
pbaardwijk 0:ae0bec143f2d 157 RMS1 = sqrt(SumRMS1/n);
pbaardwijk 0:ae0bec143f2d 158 RMS2 = sqrt(SumRMS2/n);
pbaardwijk 0:ae0bec143f2d 159
pbaardwijk 0:ae0bec143f2d 160 //Calculating min value and max value of emg signal
pbaardwijk 0:ae0bec143f2d 161 if(state == STATE_CALIBRATION)
pbaardwijk 0:ae0bec143f2d 162 {
pbaardwijk 0:ae0bec143f2d 163 if (start_calibration == 0) {
pbaardwijk 0:ae0bec143f2d 164 min_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 165 max_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 166 min_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 167 max_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 168 min_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 169 max_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 170 start_calibration++;
pbaardwijk 0:ae0bec143f2d 171 }
pbaardwijk 0:ae0bec143f2d 172 else {
pbaardwijk 0:ae0bec143f2d 173 //finding min and max of emg0
pbaardwijk 0:ae0bec143f2d 174 if (RMS0 < min_emg[0]) {
pbaardwijk 0:ae0bec143f2d 175 min_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 176 }
pbaardwijk 0:ae0bec143f2d 177 else if (RMS0 > max_emg[0]) {
pbaardwijk 0:ae0bec143f2d 178 max_emg[0] = RMS0;
pbaardwijk 0:ae0bec143f2d 179 }
pbaardwijk 0:ae0bec143f2d 180
pbaardwijk 0:ae0bec143f2d 181 //finding min and max of emg1
pbaardwijk 0:ae0bec143f2d 182 if (RMS1 < min_emg[1]) {
pbaardwijk 0:ae0bec143f2d 183 min_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 184 }
pbaardwijk 0:ae0bec143f2d 185 else if (RMS1 > max_emg[1]) {
pbaardwijk 0:ae0bec143f2d 186 max_emg[1] = RMS1;
pbaardwijk 0:ae0bec143f2d 187 }
pbaardwijk 0:ae0bec143f2d 188
pbaardwijk 0:ae0bec143f2d 189 //finding min and max of emg2
pbaardwijk 0:ae0bec143f2d 190 if (RMS2 < min_emg[2]) {
pbaardwijk 0:ae0bec143f2d 191 min_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 192 }
pbaardwijk 0:ae0bec143f2d 193 else if (RMS2 > max_emg[2]) {
pbaardwijk 0:ae0bec143f2d 194 max_emg[2] = RMS2;
pbaardwijk 0:ae0bec143f2d 195 }
pbaardwijk 0:ae0bec143f2d 196 }
pbaardwijk 0:ae0bec143f2d 197 }
pbaardwijk 0:ae0bec143f2d 198
pbaardwijk 0:ae0bec143f2d 199 //calculating input_forces for controller
pbaardwijk 0:ae0bec143f2d 200 input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 0:ae0bec143f2d 201 input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 0:ae0bec143f2d 202 input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]);
pbaardwijk 0:ae0bec143f2d 203
pbaardwijk 0:ae0bec143f2d 204 //Send scope data
pbaardwijk 0:ae0bec143f2d 205 scope.set(0,emg_sample[0]);
pbaardwijk 5:02b3550e1ff0 206 scope.set(1,RMS0);
pbaardwijk 5:02b3550e1ff0 207 scope.set(2,emg_low_passed[0]);
pbaardwijk 0:ae0bec143f2d 208 //scope.set(3,input_force2);
pbaardwijk 0:ae0bec143f2d 209 scope.send();
pbaardwijk 0:ae0bec143f2d 210
pbaardwijk 0:ae0bec143f2d 211 go_emgSample = false;
pbaardwijk 0:ae0bec143f2d 212 }
pbaardwijk 0:ae0bec143f2d 213 }
pbaardwijk 0:ae0bec143f2d 214