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@10:93ff2f23b901, 2016-10-28 (annotated)

Committer:
NahuelM
Date:
Fri Oct 28 10:08:43 2016 +0000
Revision:
10:93ff2f23b901
Parent:
9:81637351bbd1 
EMG HELEMAAL KAPOT!!!

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
NahuelM 10:93ff2f23b901 11 HIDScope scope(6);
pbaardwijk 6:da06585e106c 12
pbaardwijk 0:ae0bec143f2d 13 //Notch filter
NahuelM 10:93ff2f23b901 14 BiQuadChain notch_50_0;
NahuelM 10:93ff2f23b901 15 BiQuadChain notch_50_1;
NahuelM 10:93ff2f23b901 16 BiQuadChain notch_50_2;
pbaardwijk 7:a928724ef731 17 BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621);
pbaardwijk 7:a928724ef731 18 BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725);
pbaardwijk 7:a928724ef731 19 BiQuad bq5( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072);
pbaardwijk 0:ae0bec143f2d 20
pbaardwijk 0:ae0bec143f2d 21 //High pass filter
NahuelM 10:93ff2f23b901 22 BiQuadChain high_pass_0;
NahuelM 10:93ff2f23b901 23 BiQuadChain high_pass_1;
NahuelM 10:93ff2f23b901 24 BiQuadChain high_pass_2;
pbaardwijk 7:a928724ef731 25 BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630);
pbaardwijk 7:a928724ef731 26 BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694);
pbaardwijk 7:a928724ef731 27 //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620);
pbaardwijk 0:ae0bec143f2d 28
pbaardwijk 5:02b3550e1ff0 29 //Low pass filter
NahuelM 10:93ff2f23b901 30 BiQuadChain low_pass_0;
NahuelM 10:93ff2f23b901 31 BiQuadChain low_pass_1;
NahuelM 10:93ff2f23b901 32 BiQuadChain low_pass_2;
pbaardwijk 7:a928724ef731 33 BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240);
pbaardwijk 7:a928724ef731 34 BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411);
pbaardwijk 7:a928724ef731 35 BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909);
NahuelM 10:93ff2f23b901 36
NahuelM 10:93ff2f23b901 37
pbaardwijk 0:ae0bec143f2d 38 //Ticker
pbaardwijk 0:ae0bec143f2d 39 Ticker emgSampleTicker;
pbaardwijk 0:ae0bec143f2d 40
pbaardwijk 0:ae0bec143f2d 41 //Timeout to change state after 5 seconds
pbaardwijk 0:ae0bec143f2d 42 Timeout change_state;
pbaardwijk 0:ae0bec143f2d 43
pbaardwijk 1:7fb4a74d33ff 44 //Timeout to change state after 15 seconds
pbaardwijk 0:ae0bec143f2d 45 Timeout change_state2;
pbaardwijk 0:ae0bec143f2d 46
pbaardwijk 1:7fb4a74d33ff 47 //LED
pbaardwijk 1:7fb4a74d33ff 48 DigitalOut led(LED_RED);
pbaardwijk 1:7fb4a74d33ff 49
pbaardwijk 0:ae0bec143f2d 50 //Emg input
pbaardwijk 0:ae0bec143f2d 51 AnalogIn emg0( A0 );
pbaardwijk 0:ae0bec143f2d 52 AnalogIn emg1( A1 );
pbaardwijk 0:ae0bec143f2d 53 AnalogIn emg2( A2 );
pbaardwijk 0:ae0bec143f2d 54
pbaardwijk 9:81637351bbd1 55 //Go flag for emg sample
pbaardwijk 0:ae0bec143f2d 56 bool go_emgSample;
pbaardwijk 9:81637351bbd1 57
pbaardwijk 9:81637351bbd1 58 //Variables for intermediate filter values
pbaardwijk 0:ae0bec143f2d 59 double emg_sample[3];
pbaardwijk 6:da06585e106c 60 double emg_low_passed_200[3];
pbaardwijk 0:ae0bec143f2d 61 double emg_notch[3];
pbaardwijk 0:ae0bec143f2d 62 double emg_high_passed[3];
pbaardwijk 5:02b3550e1ff0 63 double emg_low_passed[3];
pbaardwijk 0:ae0bec143f2d 64 double min_emg[3];
pbaardwijk 0:ae0bec143f2d 65 double max_emg[3];
pbaardwijk 0:ae0bec143f2d 66
pbaardwijk 9:81637351bbd1 67 //Calculating normalized outputs
pbaardwijk 0:ae0bec143f2d 68 double input_force0;
pbaardwijk 0:ae0bec143f2d 69 double input_force1;
pbaardwijk 0:ae0bec143f2d 70 double input_force2;
pbaardwijk 0:ae0bec143f2d 71
pbaardwijk 0:ae0bec143f2d 72 //count for emg min max
pbaardwijk 0:ae0bec143f2d 73 int start_calibration = 0;
pbaardwijk 0:ae0bec143f2d 74
pbaardwijk 0:ae0bec143f2d 75 void emgSample() {
pbaardwijk 0:ae0bec143f2d 76 go_emgSample = true;
pbaardwijk 0:ae0bec143f2d 77 }
pbaardwijk 0:ae0bec143f2d 78
pbaardwijk 0:ae0bec143f2d 79 void calibrate() {
pbaardwijk 0:ae0bec143f2d 80 state = STATE_CALIBRATION;
pbaardwijk 1:7fb4a74d33ff 81 led.write(0);
pbaardwijk 0:ae0bec143f2d 82 }
pbaardwijk 0:ae0bec143f2d 83
pbaardwijk 0:ae0bec143f2d 84 void run() {
pbaardwijk 0:ae0bec143f2d 85 state = STATE_RUN;
pbaardwijk 1:7fb4a74d33ff 86 led.write(1);
pbaardwijk 0:ae0bec143f2d 87 }
pbaardwijk 0:ae0bec143f2d 88
pbaardwijk 0:ae0bec143f2d 89 void EMG_filter();
pbaardwijk 0:ae0bec143f2d 90
pbaardwijk 0:ae0bec143f2d 91 int main() {
pbaardwijk 0:ae0bec143f2d 92 //combine biquads in biquad chains for notch/high- low-pass filters
NahuelM 10:93ff2f23b901 93 notch_50_0.add( &bq3 ).add( &bq4 ).add( &bq5 );
NahuelM 10:93ff2f23b901 94 notch_50_1.add( &bq3 ).add( &bq4 ).add( &bq5 );
NahuelM 10:93ff2f23b901 95 notch_50_2.add( &bq3 ).add( &bq4 ).add( &bq5 );
NahuelM 10:93ff2f23b901 96 high_pass_0.add( &bq6 ).add( &bq7 );
NahuelM 10:93ff2f23b901 97 high_pass_1.add( &bq6 ).add( &bq7 );
NahuelM 10:93ff2f23b901 98 high_pass_2.add( &bq6 ).add( &bq7 );
NahuelM 10:93ff2f23b901 99 low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
NahuelM 10:93ff2f23b901 100 low_pass_1.add( &bq9 ).add( &bq10 ).add( &bq11 );
NahuelM 10:93ff2f23b901 101 low_pass_2.add( &bq9 ).add( &bq10 ).add( &bq11 );
pbaardwijk 1:7fb4a74d33ff 102 led.write(1);
pbaardwijk 0:ae0bec143f2d 103
pbaardwijk 0:ae0bec143f2d 104 change_state.attach( &calibrate,5);
pbaardwijk 1:7fb4a74d33ff 105 change_state2.attach( &run,15);
pbaardwijk 7:a928724ef731 106 emgSampleTicker.attach( &emgSample, 0.005); //200Hz
pbaardwijk 0:ae0bec143f2d 107 while( true ){
pbaardwijk 0:ae0bec143f2d 108 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 109 EMG_filter();
pbaardwijk 0:ae0bec143f2d 110 }
pbaardwijk 0:ae0bec143f2d 111 }
pbaardwijk 0:ae0bec143f2d 112 }
pbaardwijk 0:ae0bec143f2d 113
pbaardwijk 0:ae0bec143f2d 114
pbaardwijk 0:ae0bec143f2d 115 void EMG_filter() {
pbaardwijk 0:ae0bec143f2d 116 if(go_emgSample == true){
pbaardwijk 0:ae0bec143f2d 117 //read the emg signal
pbaardwijk 0:ae0bec143f2d 118 emg_sample[0] = emg0.read();
pbaardwijk 0:ae0bec143f2d 119 emg_sample[1] = emg1.read();
pbaardwijk 0:ae0bec143f2d 120 emg_sample[2] = emg2.read();
NahuelM 10:93ff2f23b901 121 //pc.printf("%f - %f - %f \r\n", emg_sample[0], emg_sample[1], emg_sample[2]);
pbaardwijk 7:a928724ef731 122 //filter out the 50Hz components with a notch filter
NahuelM 10:93ff2f23b901 123 emg_notch[0] = notch_50_0.step(emg_sample[0]);
NahuelM 10:93ff2f23b901 124 emg_notch[1] = notch_50_1.step(emg_sample[1]);
NahuelM 10:93ff2f23b901 125 emg_notch[2] = notch_50_2.step(emg_sample[2]);
pbaardwijk 7:a928724ef731 126 //high pass the signal (removing motion artifacts and offset)
NahuelM 10:93ff2f23b901 127 emg_high_passed[0] = high_pass_0.step(emg_notch[0]);
NahuelM 10:93ff2f23b901 128 emg_high_passed[1] = high_pass_1.step(emg_notch[1]);
NahuelM 10:93ff2f23b901 129 emg_high_passed[2] = high_pass_2.step(emg_notch[2]);
NahuelM 10:93ff2f23b901 130 float emg_fabs[3];
NahuelM 10:93ff2f23b901 131 emg_fabs[0] = fabs(emg_high_passed[0]);
NahuelM 10:93ff2f23b901 132 emg_fabs[1] = fabs(emg_high_passed[1]);
NahuelM 10:93ff2f23b901 133 emg_fabs[2] = fabs(emg_high_passed[2]);
pbaardwijk 7:a928724ef731 134 //low pass the rectified emg signal
NahuelM 10:93ff2f23b901 135 emg_low_passed[0] = low_pass_0.step(emg_fabs[0]);
NahuelM 10:93ff2f23b901 136 emg_low_passed[1] = low_pass_1.step(emg_fabs[1]);
NahuelM 10:93ff2f23b901 137 emg_low_passed[2] = low_pass_2.step(emg_fabs[2]);
NahuelM 10:93ff2f23b901 138 // pc.printf("%f - %f - %f \r\n", emg_low_passed[0], emg_low_passed[1], emg_low_passed[2]);
pbaardwijk 0:ae0bec143f2d 139 //Calculating min value and max value of emg signal
pbaardwijk 0:ae0bec143f2d 140 if(state == STATE_CALIBRATION)
pbaardwijk 0:ae0bec143f2d 141 {
pbaardwijk 0:ae0bec143f2d 142 if (start_calibration == 0) {
pbaardwijk 9:81637351bbd1 143 min_emg[0] = emg_low_passed[0];
pbaardwijk 9:81637351bbd1 144 max_emg[0] = emg_low_passed[0];
pbaardwijk 9:81637351bbd1 145 min_emg[1] = emg_low_passed[1];
pbaardwijk 9:81637351bbd1 146 max_emg[1] = emg_low_passed[1];
pbaardwijk 9:81637351bbd1 147 min_emg[2] = emg_low_passed[2];
pbaardwijk 9:81637351bbd1 148 max_emg[2] = emg_low_passed[2];
pbaardwijk 0:ae0bec143f2d 149 start_calibration++;
pbaardwijk 0:ae0bec143f2d 150 }
pbaardwijk 0:ae0bec143f2d 151 else {
pbaardwijk 0:ae0bec143f2d 152 //finding min and max of emg0
pbaardwijk 9:81637351bbd1 153 if (emg_low_passed[0] < min_emg[0]) {
pbaardwijk 9:81637351bbd1 154 min_emg[0] = emg_low_passed[0];
pbaardwijk 0:ae0bec143f2d 155 }
pbaardwijk 9:81637351bbd1 156 else if (emg_low_passed[0] > max_emg[0]) {
pbaardwijk 9:81637351bbd1 157 max_emg[0] = emg_low_passed[0];
pbaardwijk 0:ae0bec143f2d 158 }
pbaardwijk 0:ae0bec143f2d 159
pbaardwijk 0:ae0bec143f2d 160 //finding min and max of emg1
pbaardwijk 9:81637351bbd1 161 if (emg_low_passed[1] < min_emg[1]) {
pbaardwijk 9:81637351bbd1 162 min_emg[1] = emg_low_passed[1];
pbaardwijk 0:ae0bec143f2d 163 }
pbaardwijk 9:81637351bbd1 164 else if (emg_low_passed[1] > max_emg[1]) {
pbaardwijk 9:81637351bbd1 165 max_emg[1] = emg_low_passed[1];
pbaardwijk 0:ae0bec143f2d 166 }
pbaardwijk 0:ae0bec143f2d 167
pbaardwijk 0:ae0bec143f2d 168 //finding min and max of emg2
pbaardwijk 9:81637351bbd1 169 if (emg_low_passed[2] < min_emg[2]) {
pbaardwijk 9:81637351bbd1 170 min_emg[2] = emg_low_passed[2];
pbaardwijk 0:ae0bec143f2d 171 }
pbaardwijk 9:81637351bbd1 172 else if (emg_low_passed[2] > max_emg[2]) {
pbaardwijk 9:81637351bbd1 173 max_emg[2] = emg_low_passed[2];
pbaardwijk 0:ae0bec143f2d 174 }
pbaardwijk 0:ae0bec143f2d 175 }
pbaardwijk 0:ae0bec143f2d 176 }
pbaardwijk 0:ae0bec143f2d 177
pbaardwijk 0:ae0bec143f2d 178 //calculating input_forces for controller
pbaardwijk 9:81637351bbd1 179 input_force0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]);
pbaardwijk 9:81637351bbd1 180 input_force1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]);
pbaardwijk 9:81637351bbd1 181 input_force2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]);
pbaardwijk 0:ae0bec143f2d 182
pbaardwijk 0:ae0bec143f2d 183 //Send scope data
NahuelM 10:93ff2f23b901 184 scope.set(0,emg_high_passed[0]);
NahuelM 10:93ff2f23b901 185 scope.set(1,emg_high_passed[1]);
NahuelM 10:93ff2f23b901 186 scope.set(2,emg_high_passed[2]);
NahuelM 10:93ff2f23b901 187 scope.set(3,emg_low_passed[0]);
NahuelM 10:93ff2f23b901 188 scope.set(4,emg_low_passed[1]);
NahuelM 10:93ff2f23b901 189 scope.set(5,emg_low_passed[2]);
NahuelM 10:93ff2f23b901 190 //scope.set(3,input_force0);
pbaardwijk 7:a928724ef731 191 //scope.set(2,emg_low_passed[0]);
pbaardwijk 0:ae0bec143f2d 192 //scope.set(3,input_force2);
pbaardwijk 0:ae0bec143f2d 193 scope.send();
pbaardwijk 0:ae0bec143f2d 194
pbaardwijk 0:ae0bec143f2d 195 go_emgSample = false;
pbaardwijk 0:ae0bec143f2d 196 }
pbaardwijk 0:ae0bec143f2d 197 }
pbaardwijk 0:ae0bec143f2d 198