De EMG Lowpass maakt alle signalen gelijk
Dependencies: HIDScope biquadFilter mbed
Fork of EMGfilter by
Diff: main.cpp
- Revision:
- 0:ae0bec143f2d
- Child:
- 1:7fb4a74d33ff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 24 11:16:02 2016 +0000 @@ -0,0 +1,187 @@ +#include "mbed.h" +#include "BiQuad.h" +#include "HIDScope.h" +//Enum with states +enum states {STATE_DEFAULT , STATE_CALIBRATION, STATE_RUN}; + +//Variable called 'state' +states state = STATE_DEFAULT; + +//Creating two scope channels +HIDScope scope(2); + +//Notch filter +BiQuadChain notch_50; +BiQuad bq1( 1.00000000000, -1.60956348896, 1.00000000000, -1.40195621505, 0.74203282402); +BiQuad bq2( 1.00000000000, -1.60724786352, 1.00000000000, -1.33646101015, 0.85967899264); +BiQuad bq3( 1.00000000000, -1.61186693071, 1.00000000000, -1.64415455961, 0.89726621230); + +//High pass filter +BiQuadChain high_pass; +BiQuad bq4( 1.00000000000, -1.99999967822, 1.00000000000, -1.98388291862, 0.98395921205); +BiQuad bq5( 1.00000000000, -1.99999812453, 1.00000000000, -1.99324612474, 0.99332432675); + +//Ticker +Ticker emgSampleTicker; + +//Timeout to change state after 5 seconds +Timeout change_state; + +//Timeout to change state after 10 seconds +Timeout change_state2; + +//Emg input +AnalogIn emg0( A0 ); +AnalogIn emg1( A1 ); +AnalogIn emg2( A2 ); + +bool go_emgSample; +bool go_find_minmax; +double emg_sample[3]; +double emg_notch[3]; +double emg_high_passed[3]; +double emg_rectified; +double min_emg[3]; +double max_emg[3]; + +const int n = 200; +int counter = 0; +double RMSArray0[n] = {0}; +double RMSArray1[n] = {0}; +double RMSArray2[n] = {0}; +double RMS0; +double RMS1; +double RMS2; +double SumRMS0; +double SumRMS1; +double SumRMS2; + +double input_force0; +double input_force1; +double input_force2; + +//count for emg min max +int start_calibration = 0; + +void emgSample() { + go_emgSample = true; +} + +void calibrate() { + state = STATE_CALIBRATION; +} + +void run() { + state = STATE_RUN; +} + +void EMG_filter(); + +int main() { + //combine biquads in biquad chains for notch/high- low-pass filters + notch_50.add( &bq1 ).add( &bq2 ).add( &bq3 ); + high_pass.add( &bq4 ).add( &bq5 ); + + change_state.attach( &calibrate,5); + change_state2.attach( &run,10); + emgSampleTicker.attach( &emgSample, 0.002); + while( true ){ + if(go_emgSample == true){ + EMG_filter(); + } + } +} + + +void EMG_filter() { + if(go_emgSample == true){ + //read the emg signal + emg_sample[0] = emg0.read(); + emg_sample[1] = emg1.read(); + emg_sample[2] = emg2.read(); + + for (int i = 0; i < 3; i++){ + //filter out the 50Hz components with a notch filter + //emg_notch[i] = notch_50.step(emg_sample[i]); + + //high pass the signal (removing motion artifacts and offset) + emg_high_passed[i] = high_pass.step(emg_sample[i]); + } + + //Calculating RMS + SumRMS0 -= pow(RMSArray0[counter],2); + SumRMS1 -= pow(RMSArray1[counter],2); + SumRMS2 -= pow(RMSArray2[counter],2); + + RMSArray0[counter] = emg_high_passed[0]; + RMSArray1[counter] = emg_high_passed[1]; + RMSArray2[counter] = emg_high_passed[2]; + + SumRMS0 += pow(RMSArray0[counter],2); + SumRMS1 += pow(RMSArray1[counter],2); + SumRMS2 += pow(RMSArray2[counter],2); + + counter++; + if (counter == n){ + counter = 0; + } + + RMS0 = sqrt(SumRMS0/n); + RMS1 = sqrt(SumRMS1/n); + RMS2 = sqrt(SumRMS2/n); + + //Calculating min value and max value of emg signal + if(state == STATE_CALIBRATION) + { + if (start_calibration == 0) { + min_emg[0] = RMS0; + max_emg[0] = RMS0; + min_emg[1] = RMS1; + max_emg[1] = RMS1; + min_emg[2] = RMS2; + max_emg[2] = RMS2; + start_calibration++; + } + else { + //finding min and max of emg0 + if (RMS0 < min_emg[0]) { + min_emg[0] = RMS0; + } + else if (RMS0 > max_emg[0]) { + max_emg[0] = RMS0; + } + + //finding min and max of emg1 + if (RMS1 < min_emg[1]) { + min_emg[1] = RMS1; + } + else if (RMS1 > max_emg[1]) { + max_emg[1] = RMS1; + } + + //finding min and max of emg2 + if (RMS2 < min_emg[2]) { + min_emg[2] = RMS2; + } + else if (RMS2 > max_emg[2]) { + max_emg[2] = RMS2; + } + } + } + + //calculating input_forces for controller + input_force0 = (RMS0 - min_emg[0])/(max_emg[0]-min_emg[0]); + input_force1 = (RMS1 - min_emg[1])/(max_emg[1]-min_emg[1]); + input_force2 = (RMS2 - min_emg[2])/(max_emg[2]-min_emg[2]); + + //Send scope data + scope.set(0,emg_sample[0]); + scope.set(1,input_force0); + //scope.set(2,input_force1); + //scope.set(3,input_force2); + scope.send(); + + go_emgSample = false; + } + } +