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