De EMG Lowpass maakt alle signalen gelijk
Dependencies: HIDScope biquadFilter mbed
Fork of EMGfilter by
main.cpp
- Committer:
- pbaardwijk
- Date:
- 2016-10-25
- Revision:
- 6:da06585e106c
- Parent:
- 5:02b3550e1ff0
- Child:
- 7:a928724ef731
File content as of revision 6:da06585e106c:
#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(3); //Low-pass filter at 200Hz to avoid alliasing BiQuadChain low_pass_200; BiQuad bq1( 0.73638326364, 1.47272205824, 0.73638326364, 1.36239603912, 0.48048795767); BiQuad bq2( 0.83856791742, 1.67684080778, 0.83856791742, 1.68114429812, 0.7942832489); //Notch filter BiQuadChain notch_50; BiQuad bq3( 1.00007369335,-1.61815834791, 1.00007369335, -1.60983662066, 0.98986119869); BiQuad bq4( 1.00000000000,-1.61803910919, 1.00000000000, -1.60936554071, 0.99545624832); BiQuad bq5( 0.99035034846,-1.60242559561, 0.99035034846, -1.61934542233, 0.9955088075); //High pass filter BiQuadChain high_pass; BiQuad bq6( 0.83315051810,-1.66630103620, 0.83315051810, -1.55025104412, 0.60696783282); BiQuad bq7( 0.86554941044,-1.73109882088, 0.86554941044, -1.74142633961, 0.78400451004); BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620); //Low pass filter BiQuadChain low_pass; BiQuad bq9( 0.00040400257, 0.00080800515, 0.00040400257,-1.92223307595, 0.92384908624); BiQuad bq10( 0.00040816681, 0.00081633362, 0.00040816681,-1.94204639240, 0.94367905964); BiQuad bq11( 0.00041558628, 0.00083117256, 0.00041558628,-1.97734803172, 0.9790103768); //Ticker Ticker emgSampleTicker; //Timeout to change state after 5 seconds Timeout change_state; //Timeout to change state after 15 seconds Timeout change_state2; //LED DigitalOut led(LED_RED); //Emg input AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); AnalogIn emg2( A2 ); bool go_emgSample; bool go_find_minmax; double emg_sample[3]; double emg_low_passed_200[3]; double emg_notch[3]; double emg_high_passed[3]; double emg_low_passed[3]; 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; led.write(0); } void run() { state = STATE_RUN; led.write(1); } void EMG_filter(); int main() { //combine biquads in biquad chains for notch/high- low-pass filters low_pass_200.add( &bq1 ).add( &bq2 ); notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 ); high_pass.add( &bq6 ).add( &bq7 ).add( &bq8 ); low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 ); led.write(1); change_state.attach( &calibrate,5); change_state2.attach( &run,15); 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++){ //low pass at 200Hz to avoid alliasing emg_low_passed_200[i] = low_pass_200.step(emg_sample[i]); //filter out the 50Hz components with a notch filter emg_notch[i] = notch_50.step(emg_low_passed_200[i]); //high pass the signal (removing motion artifacts and offset) emg_high_passed[i] = high_pass.step(emg_notch[i]); //low pass the rectified emg signal emg_low_passed[i] = low_pass.step(fabs(emg_high_passed[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,RMS0); scope.set(2,emg_low_passed[0]); //scope.set(3,input_force2); scope.send(); go_emgSample = false; } }