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@8:dc0858fead9f, 2016-10-27 (annotated)

Committer:
pbaardwijk
Date:
Thu Oct 27 10:13:44 2016 +0000
Revision:
8:dc0858fead9f
Parent:
7:a928724ef731 
Working EMG filter, 6Hz low-pass 3 emg's

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