the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
main.cpp
- Committer:
- RiP
- Date:
- 2016-10-26
- Revision:
- 35:4905144c1123
- Parent:
- 33:fcd4568f1c86
- Child:
- 36:344588e69589
File content as of revision 35:4905144c1123:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "MODSERIAL.h" //Define objects //Define the button interrupt for the calibration InterruptIn button_calibrate(PTA4); //Define the EMG inputs AnalogIn emg1( A0 ); AnalogIn emg2( A1 ); AnalogIn emg3( A2 ); //Define the Tickers Ticker pos_timer; Ticker sample_timer; HIDScope scope( 6 ); MODSERIAL pc(USBTX, USBRX); volatile bool sampletimer = false; volatile bool buttonflag = false; volatile bool newcase = false; double threshold = 0.04; double samplefreq=0.002; double emg02; double emg12; double emg22; double ref_x=0.000; double ref_y=0.000; double speed=0.00002; const int negative=-1; char key; // create a variable called 'state', define it typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states; states mystate = STATE_PAUZE; //Define the needed Biquad chains BiQuadChain bqc11; BiQuadChain bqc13; BiQuadChain bqc21; BiQuadChain bqc23; BiQuadChain bqc31; BiQuadChain bqc33; //Define the BiQuads for the filter of the first emg signal //Notch filter BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); //High pass filter BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Low pass filter BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); //Define the BiQuads for the filter of the second emg signal //Notch filter BiQuad bq211 = bq111; BiQuad bq212 = bq112; BiQuad bq213 = bq113; /* High pass filter*/ BiQuad bq221 = bq121; /* Low pass filter*/ BiQuad bq231 = bq131; //Define the BiQuads for the filter of the third emg signal //notch filter BiQuad bq311 = bq111; BiQuad bq312 = bq112; BiQuad bq313 = bq113; //High pass filter BiQuad bq321 = bq121; //low pass filter BiQuad bq331 = bq131; void sampleflag() { sampletimer=true; } void buttonflag_go() { buttonflag=true; } void sample(states &mystate) { states myoldstate=mystate; if (pc.readable()==1) { key=pc.getc(); speed=0.000326; } else { key ='p'; speed=0.00002; } /* Read the emg signals and filter it*/ scope.set(0, emg1.read()); //original signal emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); scope.set(1, emg02); /* Read the second emg signal and filter it*/ scope.set(2, emg2.read()); //original signal emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); scope.set(3, emg12); /* Read the third emg signal and filter it*/ scope.set(4, emg3.read()); //original signal emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); scope.set(5, emg22); /* Ensure that enough channels are available (HIDScope scope( 2 )) * Finally, send all channels to the PC at once */ scope.send(); if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') { mystate = STATE_XY_NEG; ref_x=ref_x+speed*negative; ref_y=ref_y+speed*negative; } else if (emg02>threshold&&emg12>threshold || key == 'a' ) { mystate = STATE_X_NEG; ref_x=ref_x+speed*negative; } else if (emg02>threshold&&emg22>threshold || key == 's') { mystate = STATE_Y_NEG; ref_y=ref_y+speed*negative; } else if (emg12>threshold&&emg22>threshold || key == 'e' ) { mystate = STATE_XY; ref_x=ref_x+speed; ref_y=ref_y+speed; } else if (emg12>threshold || key == 'q' ) { mystate = STATE_X; ref_x=ref_x+speed; } else if (emg22>threshold || key == 'w') { mystate = STATE_Y; ref_y=ref_y+speed; } else { mystate = STATE_PAUZE; } if (buttonflag==true) { mystate = STATE_CALIBRATION; } if (myoldstate==mystate) { newcase=false; } else { newcase=true; } } void my_pos() { pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y); } void print_state() { if (newcase==true) { switch (mystate) { case STATE_CALIBRATION : { // calibration pc.printf("calibration\n\r"); break; } case STATE_X : // run pc.printf("X\n\r"); break; case STATE_X_NEG : // run pc.printf("Xneg\n\r"); break; case STATE_Y : // execute mode 1 pc.printf("Y\n\r"); break; case STATE_Y_NEG : // execute mode 1 pc.printf("Yneg\n\r"); break; case STATE_XY : // execute mode 2 pc.printf("XY\n\r"); break; case STATE_XY_NEG : // execute mode 2 pc.printf("XYneg\n\r"); break; case STATE_PAUZE : // default pc.printf("PAUZE\n\r"); break; } } } int main() { pc.printf("RESET\n\r"); pc.baud(115200); //make the Biquad chains bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ); bqc13.add( &bq131); bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ); bqc23.add( &bq231); bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ); bqc33.add( &bq331); /*Attach the 'sample' function to the timer 'sample_timer'. this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz */ sample_timer.attach(&sampleflag, samplefreq); button_calibrate.fall(&buttonflag_go); pos_timer.attach(&my_pos, 1); while(1) { if (sampletimer==true) { //sample(mystate); sample(mystate); // print_state(); sampletimer = false; } } }