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