![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
De EMG Lowpass maakt alle signalen gelijk
Dependencies: HIDScope biquadFilter mbed
Fork of EMGfilter by
main.cpp@9:81637351bbd1, 2016-10-27 (annotated)
- Committer:
- pbaardwijk
- Date:
- Thu Oct 27 10:27:08 2016 +0000
- Revision:
- 9:81637351bbd1
- Parent:
- 7:a928724ef731
- Child:
- 10:93ff2f23b901
Working EMG filter with 6Hz low-pass cleaned up
Who changed what in which revision?
User | Revision | Line number | New 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 | 7:a928724ef731 | 15 | BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621); |
pbaardwijk | 7:a928724ef731 | 16 | BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725); |
pbaardwijk | 7:a928724ef731 | 17 | BiQuad bq5( 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 | 7:a928724ef731 | 21 | BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630); |
pbaardwijk | 7:a928724ef731 | 22 | BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694); |
pbaardwijk | 7:a928724ef731 | 23 | //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620); |
pbaardwijk | 0:ae0bec143f2d | 24 | |
pbaardwijk | 5:02b3550e1ff0 | 25 | //Low pass filter |
pbaardwijk | 5:02b3550e1ff0 | 26 | BiQuadChain low_pass; |
pbaardwijk | 7:a928724ef731 | 27 | BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240); |
pbaardwijk | 7:a928724ef731 | 28 | BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411); |
pbaardwijk | 7:a928724ef731 | 29 | BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909); |
pbaardwijk | 0:ae0bec143f2d | 30 | //Ticker |
pbaardwijk | 0:ae0bec143f2d | 31 | Ticker emgSampleTicker; |
pbaardwijk | 0:ae0bec143f2d | 32 | |
pbaardwijk | 0:ae0bec143f2d | 33 | //Timeout to change state after 5 seconds |
pbaardwijk | 0:ae0bec143f2d | 34 | Timeout change_state; |
pbaardwijk | 0:ae0bec143f2d | 35 | |
pbaardwijk | 1:7fb4a74d33ff | 36 | //Timeout to change state after 15 seconds |
pbaardwijk | 0:ae0bec143f2d | 37 | Timeout change_state2; |
pbaardwijk | 0:ae0bec143f2d | 38 | |
pbaardwijk | 1:7fb4a74d33ff | 39 | //LED |
pbaardwijk | 1:7fb4a74d33ff | 40 | DigitalOut led(LED_RED); |
pbaardwijk | 1:7fb4a74d33ff | 41 | |
pbaardwijk | 0:ae0bec143f2d | 42 | //Emg input |
pbaardwijk | 0:ae0bec143f2d | 43 | AnalogIn emg0( A0 ); |
pbaardwijk | 0:ae0bec143f2d | 44 | AnalogIn emg1( A1 ); |
pbaardwijk | 0:ae0bec143f2d | 45 | AnalogIn emg2( A2 ); |
pbaardwijk | 0:ae0bec143f2d | 46 | |
pbaardwijk | 9:81637351bbd1 | 47 | //Go flag for emg sample |
pbaardwijk | 0:ae0bec143f2d | 48 | bool go_emgSample; |
pbaardwijk | 9:81637351bbd1 | 49 | |
pbaardwijk | 9:81637351bbd1 | 50 | //Variables for intermediate filter values |
pbaardwijk | 0:ae0bec143f2d | 51 | double emg_sample[3]; |
pbaardwijk | 6:da06585e106c | 52 | double emg_low_passed_200[3]; |
pbaardwijk | 0:ae0bec143f2d | 53 | double emg_notch[3]; |
pbaardwijk | 0:ae0bec143f2d | 54 | double emg_high_passed[3]; |
pbaardwijk | 5:02b3550e1ff0 | 55 | double emg_low_passed[3]; |
pbaardwijk | 0:ae0bec143f2d | 56 | double min_emg[3]; |
pbaardwijk | 0:ae0bec143f2d | 57 | double max_emg[3]; |
pbaardwijk | 0:ae0bec143f2d | 58 | |
pbaardwijk | 9:81637351bbd1 | 59 | //Calculating normalized outputs |
pbaardwijk | 0:ae0bec143f2d | 60 | double input_force0; |
pbaardwijk | 0:ae0bec143f2d | 61 | double input_force1; |
pbaardwijk | 0:ae0bec143f2d | 62 | double input_force2; |
pbaardwijk | 0:ae0bec143f2d | 63 | |
pbaardwijk | 0:ae0bec143f2d | 64 | //count for emg min max |
pbaardwijk | 0:ae0bec143f2d | 65 | int start_calibration = 0; |
pbaardwijk | 0:ae0bec143f2d | 66 | |
pbaardwijk | 0:ae0bec143f2d | 67 | void emgSample() { |
pbaardwijk | 0:ae0bec143f2d | 68 | go_emgSample = true; |
pbaardwijk | 0:ae0bec143f2d | 69 | } |
pbaardwijk | 0:ae0bec143f2d | 70 | |
pbaardwijk | 0:ae0bec143f2d | 71 | void calibrate() { |
pbaardwijk | 0:ae0bec143f2d | 72 | state = STATE_CALIBRATION; |
pbaardwijk | 1:7fb4a74d33ff | 73 | led.write(0); |
pbaardwijk | 0:ae0bec143f2d | 74 | } |
pbaardwijk | 0:ae0bec143f2d | 75 | |
pbaardwijk | 0:ae0bec143f2d | 76 | void run() { |
pbaardwijk | 0:ae0bec143f2d | 77 | state = STATE_RUN; |
pbaardwijk | 1:7fb4a74d33ff | 78 | led.write(1); |
pbaardwijk | 0:ae0bec143f2d | 79 | } |
pbaardwijk | 0:ae0bec143f2d | 80 | |
pbaardwijk | 0:ae0bec143f2d | 81 | void EMG_filter(); |
pbaardwijk | 0:ae0bec143f2d | 82 | |
pbaardwijk | 0:ae0bec143f2d | 83 | int main() { |
pbaardwijk | 0:ae0bec143f2d | 84 | //combine biquads in biquad chains for notch/high- low-pass filters |
pbaardwijk | 6:da06585e106c | 85 | notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 ); |
pbaardwijk | 7:a928724ef731 | 86 | high_pass.add( &bq6 ).add( &bq7 ); |
pbaardwijk | 6:da06585e106c | 87 | low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 ); |
pbaardwijk | 1:7fb4a74d33ff | 88 | led.write(1); |
pbaardwijk | 0:ae0bec143f2d | 89 | |
pbaardwijk | 0:ae0bec143f2d | 90 | change_state.attach( &calibrate,5); |
pbaardwijk | 1:7fb4a74d33ff | 91 | change_state2.attach( &run,15); |
pbaardwijk | 7:a928724ef731 | 92 | emgSampleTicker.attach( &emgSample, 0.005); //200Hz |
pbaardwijk | 0:ae0bec143f2d | 93 | while( true ){ |
pbaardwijk | 0:ae0bec143f2d | 94 | if(go_emgSample == true){ |
pbaardwijk | 0:ae0bec143f2d | 95 | EMG_filter(); |
pbaardwijk | 0:ae0bec143f2d | 96 | } |
pbaardwijk | 0:ae0bec143f2d | 97 | } |
pbaardwijk | 0:ae0bec143f2d | 98 | } |
pbaardwijk | 0:ae0bec143f2d | 99 | |
pbaardwijk | 0:ae0bec143f2d | 100 | |
pbaardwijk | 0:ae0bec143f2d | 101 | void EMG_filter() { |
pbaardwijk | 0:ae0bec143f2d | 102 | if(go_emgSample == true){ |
pbaardwijk | 0:ae0bec143f2d | 103 | //read the emg signal |
pbaardwijk | 0:ae0bec143f2d | 104 | emg_sample[0] = emg0.read(); |
pbaardwijk | 0:ae0bec143f2d | 105 | emg_sample[1] = emg1.read(); |
pbaardwijk | 0:ae0bec143f2d | 106 | emg_sample[2] = emg2.read(); |
pbaardwijk | 0:ae0bec143f2d | 107 | |
pbaardwijk | 7:a928724ef731 | 108 | //filter out the 50Hz components with a notch filter |
pbaardwijk | 7:a928724ef731 | 109 | emg_notch[0] = notch_50.step(emg_sample[0]); |
pbaardwijk | 0:ae0bec143f2d | 110 | |
pbaardwijk | 7:a928724ef731 | 111 | //high pass the signal (removing motion artifacts and offset) |
pbaardwijk | 7:a928724ef731 | 112 | emg_high_passed[0] = high_pass.step(emg_notch[0]); |
pbaardwijk | 5:02b3550e1ff0 | 113 | |
pbaardwijk | 7:a928724ef731 | 114 | //low pass the rectified emg signal |
pbaardwijk | 7:a928724ef731 | 115 | emg_low_passed[0] = low_pass.step(fabs(emg_high_passed[0])); |
pbaardwijk | 0:ae0bec143f2d | 116 | |
pbaardwijk | 0:ae0bec143f2d | 117 | |
pbaardwijk | 0:ae0bec143f2d | 118 | //Calculating min value and max value of emg signal |
pbaardwijk | 0:ae0bec143f2d | 119 | if(state == STATE_CALIBRATION) |
pbaardwijk | 0:ae0bec143f2d | 120 | { |
pbaardwijk | 0:ae0bec143f2d | 121 | if (start_calibration == 0) { |
pbaardwijk | 9:81637351bbd1 | 122 | min_emg[0] = emg_low_passed[0]; |
pbaardwijk | 9:81637351bbd1 | 123 | max_emg[0] = emg_low_passed[0]; |
pbaardwijk | 9:81637351bbd1 | 124 | min_emg[1] = emg_low_passed[1]; |
pbaardwijk | 9:81637351bbd1 | 125 | max_emg[1] = emg_low_passed[1]; |
pbaardwijk | 9:81637351bbd1 | 126 | min_emg[2] = emg_low_passed[2]; |
pbaardwijk | 9:81637351bbd1 | 127 | max_emg[2] = emg_low_passed[2]; |
pbaardwijk | 0:ae0bec143f2d | 128 | start_calibration++; |
pbaardwijk | 0:ae0bec143f2d | 129 | } |
pbaardwijk | 0:ae0bec143f2d | 130 | else { |
pbaardwijk | 0:ae0bec143f2d | 131 | //finding min and max of emg0 |
pbaardwijk | 9:81637351bbd1 | 132 | if (emg_low_passed[0] < min_emg[0]) { |
pbaardwijk | 9:81637351bbd1 | 133 | min_emg[0] = emg_low_passed[0]; |
pbaardwijk | 0:ae0bec143f2d | 134 | } |
pbaardwijk | 9:81637351bbd1 | 135 | else if (emg_low_passed[0] > max_emg[0]) { |
pbaardwijk | 9:81637351bbd1 | 136 | max_emg[0] = emg_low_passed[0]; |
pbaardwijk | 0:ae0bec143f2d | 137 | } |
pbaardwijk | 0:ae0bec143f2d | 138 | |
pbaardwijk | 0:ae0bec143f2d | 139 | //finding min and max of emg1 |
pbaardwijk | 9:81637351bbd1 | 140 | if (emg_low_passed[1] < min_emg[1]) { |
pbaardwijk | 9:81637351bbd1 | 141 | min_emg[1] = emg_low_passed[1]; |
pbaardwijk | 0:ae0bec143f2d | 142 | } |
pbaardwijk | 9:81637351bbd1 | 143 | else if (emg_low_passed[1] > max_emg[1]) { |
pbaardwijk | 9:81637351bbd1 | 144 | max_emg[1] = emg_low_passed[1]; |
pbaardwijk | 0:ae0bec143f2d | 145 | } |
pbaardwijk | 0:ae0bec143f2d | 146 | |
pbaardwijk | 0:ae0bec143f2d | 147 | //finding min and max of emg2 |
pbaardwijk | 9:81637351bbd1 | 148 | if (emg_low_passed[2] < min_emg[2]) { |
pbaardwijk | 9:81637351bbd1 | 149 | min_emg[2] = emg_low_passed[2]; |
pbaardwijk | 0:ae0bec143f2d | 150 | } |
pbaardwijk | 9:81637351bbd1 | 151 | else if (emg_low_passed[2] > max_emg[2]) { |
pbaardwijk | 9:81637351bbd1 | 152 | max_emg[2] = emg_low_passed[2]; |
pbaardwijk | 0:ae0bec143f2d | 153 | } |
pbaardwijk | 0:ae0bec143f2d | 154 | } |
pbaardwijk | 0:ae0bec143f2d | 155 | } |
pbaardwijk | 0:ae0bec143f2d | 156 | |
pbaardwijk | 0:ae0bec143f2d | 157 | //calculating input_forces for controller |
pbaardwijk | 9:81637351bbd1 | 158 | input_force0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]); |
pbaardwijk | 9:81637351bbd1 | 159 | input_force1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]); |
pbaardwijk | 9:81637351bbd1 | 160 | input_force2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]); |
pbaardwijk | 0:ae0bec143f2d | 161 | |
pbaardwijk | 0:ae0bec143f2d | 162 | //Send scope data |
pbaardwijk | 0:ae0bec143f2d | 163 | scope.set(0,emg_sample[0]); |
pbaardwijk | 7:a928724ef731 | 164 | scope.set(1,emg_notch[0]); |
pbaardwijk | 7:a928724ef731 | 165 | scope.set(2,emg_high_passed[0]); |
pbaardwijk | 9:81637351bbd1 | 166 | scope.set(3,input_force0); |
pbaardwijk | 7:a928724ef731 | 167 | //scope.set(2,emg_low_passed[0]); |
pbaardwijk | 0:ae0bec143f2d | 168 | //scope.set(3,input_force2); |
pbaardwijk | 0:ae0bec143f2d | 169 | scope.send(); |
pbaardwijk | 0:ae0bec143f2d | 170 | |
pbaardwijk | 0:ae0bec143f2d | 171 | go_emgSample = false; |
pbaardwijk | 0:ae0bec143f2d | 172 | } |
pbaardwijk | 0:ae0bec143f2d | 173 | } |
pbaardwijk | 0:ae0bec143f2d | 174 |