De EMG Lowpass maakt alle signalen gelijk
Dependencies: HIDScope biquadFilter mbed
Fork of EMGfilter by
main.cpp
- Committer:
- pbaardwijk
- Date:
- 2016-10-27
- Revision:
- 9:81637351bbd1
- Parent:
- 7:a928724ef731
- Child:
- 10:93ff2f23b901
File content as of revision 9:81637351bbd1:
#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(4); //Notch filter BiQuadChain notch_50; BiQuad bq3( 0.98116526140, 0.00000000044, 0.98116526140, 0.00000000043, 0.95391787621); BiQuad bq4( 0.97224232015, 0.00000000043, 0.97224232015, -0.04036799459, 0.97670000725); BiQuad bq5( 1.00000000000, 0.00000000044, 1.00000000000, 0.04036799547, 0.9767000072); //High pass filter BiQuadChain high_pass; BiQuad bq6( 0.80254782780,-1.60509565560, 0.80254782780, -1.58011656361, 0.63006219630); BiQuad bq7( 0.90006571973,-1.80013143945, 0.900065719734, -1.77213098592, 0.8281459694); //BiQuad bq8( 0.92490714701,-1.84981429401, 0.92490714701, -1.90032503529, 0.9352152620); //Low pass filter BiQuadChain low_pass; BiQuad bq9( 0.00801840797, 0.01603681594, 0.00801840797,-1.65212256130, 0.68416767240); BiQuad bq10( 0.00836524486, 0.01673048973, 0.00836524486,-1.72511837232, 0.75857933411); BiQuad bq11( 0.00905039996, 0.01810079992, 0.00905039996,-1.86807725180, 0.9043110909); //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 ); //Go flag for emg sample bool go_emgSample; //Variables for intermediate filter values 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]; //Calculating normalized outputs 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 notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 ); high_pass.add( &bq6 ).add( &bq7 ); 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.005); //200Hz 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(); //filter out the 50Hz components with a notch filter emg_notch[0] = notch_50.step(emg_sample[0]); //high pass the signal (removing motion artifacts and offset) emg_high_passed[0] = high_pass.step(emg_notch[0]); //low pass the rectified emg signal emg_low_passed[0] = low_pass.step(fabs(emg_high_passed[0])); //Calculating min value and max value of emg signal if(state == STATE_CALIBRATION) { if (start_calibration == 0) { min_emg[0] = emg_low_passed[0]; max_emg[0] = emg_low_passed[0]; min_emg[1] = emg_low_passed[1]; max_emg[1] = emg_low_passed[1]; min_emg[2] = emg_low_passed[2]; max_emg[2] = emg_low_passed[2]; start_calibration++; } else { //finding min and max of emg0 if (emg_low_passed[0] < min_emg[0]) { min_emg[0] = emg_low_passed[0]; } else if (emg_low_passed[0] > max_emg[0]) { max_emg[0] = emg_low_passed[0]; } //finding min and max of emg1 if (emg_low_passed[1] < min_emg[1]) { min_emg[1] = emg_low_passed[1]; } else if (emg_low_passed[1] > max_emg[1]) { max_emg[1] = emg_low_passed[1]; } //finding min and max of emg2 if (emg_low_passed[2] < min_emg[2]) { min_emg[2] = emg_low_passed[2]; } else if (emg_low_passed[2] > max_emg[2]) { max_emg[2] = emg_low_passed[2]; } } } //calculating input_forces for controller input_force0 = (emg_low_passed[0] - min_emg[0])/(max_emg[0]-min_emg[0]); input_force1 = (emg_low_passed[1] - min_emg[1])/(max_emg[1]-min_emg[1]); input_force2 = (emg_low_passed[2] - min_emg[2])/(max_emg[2]-min_emg[2]); //Send scope data scope.set(0,emg_sample[0]); scope.set(1,emg_notch[0]); scope.set(2,emg_high_passed[0]); scope.set(3,input_force0); //scope.set(2,emg_low_passed[0]); //scope.set(3,input_force2); scope.send(); go_emgSample = false; } }