the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
main.cpp
- Committer:
- RiP
- Date:
- 2016-10-24
- Revision:
- 28:3b1b29193851
- Parent:
- 27:1ff7fa636f1c
- Child:
- 29:ac08c1a32c54
File content as of revision 28:3b1b29193851:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "MODSERIAL.h" #include <string.h> //Define objects InterruptIn button_calibrate(PTA4); AnalogIn emg1( A0 ); AnalogIn emg2( A1 ); AnalogIn emg3( A2 ); Ticker pos_timer; Ticker sample_timer; HIDScope scope( 6 ); MODSERIAL pc(USBTX, USBRX); DigitalOut led(LED1); volatile bool sampletimer = false; volatile bool buttonflag = false; volatile bool newcase = false; volatile bool newcaseprint = 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; // 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; BiQuadChain bqc11; BiQuadChain bqc13; BiQuadChain bqc21; BiQuadChain bqc23; BiQuadChain bqc31; BiQuadChain bqc33; //BiQuad bq11( 9.87589e-01, -1.59795e+00, 9.87589e-01, -1.59795e+00, 9.75178e-01 ); //oude BiQuad waardes /* BiQuads for filter emg1 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 ); /* BiQuads for filter emg2 notch filter*/ BiQuad bq211(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); BiQuad bq212(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); BiQuad bq213(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); /* High pass filter*/ BiQuad bq221( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); /* low pass filter*/ BiQuad bq231( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); /* BiQuads for filter emg3 notch filter*/ BiQuad bq311(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589); BiQuad bq312(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787); BiQuad bq313(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798); /* High pass filter*/ BiQuad bq321( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); /* low pass filter*/ BiQuad bq331( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); void sampleflag() { sampletimer=true; } void buttonflag_go() { buttonflag=true; } void sample_button(states &mystate) { states myoldstate=mystate; char key=pc.getc(); // pc.printf("%c/n",key); switch (key) { case 'p' : // run emg02=0.0; emg12=0.0; emg22=0.0; break; case 'q' : // run emg02=0.0; emg12=1.0; emg22=0.0; break; case 'w' : // run emg02=0.0; emg12=0.0; emg22=1.0; break; case 'e' : // run emg02=0.0; emg12=1.0; emg22=1.0; break; case 'a' : // run emg02=1.0; emg12=1.0; emg22=0.0; break; case 's' : // run emg02=1.0; emg12=0.0; emg22=1.0; break; case 'd' : // run emg02=1.0; emg12=1.0; emg22=1.0; break; } led = !led; if (emg02>threshold) { if (emg12>threshold&&emg22>threshold) { mystate = STATE_XY_NEG; } else if (emg12>threshold) { mystate = STATE_X_NEG; } else if (emg22>threshold) { mystate = STATE_Y_NEG; } else { mystate = STATE_PAUZE; } } else { if (emg12>threshold&&emg22>threshold) { mystate = STATE_XY; } else if (emg12>threshold) { mystate = STATE_X; } else if (emg22>threshold) { mystate = STATE_Y; } else { mystate = STATE_PAUZE; } } if (buttonflag==true) { mystate = STATE_CALIBRATION; } if (myoldstate==mystate) { newcase=false; newcaseprint=false; } else { newcase=true; newcaseprint=true; } sampletimer = false; } void sample(states &mystate) { states myoldstate=mystate; /* 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); //pc.printf("Hello World!\n"); /* Ensure that enough channels are available (HIDScope scope( 2 )) * Finally, send all channels to the PC at once */ scope.send(); /* To indicate that the function is working, the LED is toggled */ led = !led; if (emg02>threshold) { if (emg12>threshold&&emg22>threshold) { mystate = STATE_XY_NEG; } else if (emg12>threshold) { mystate = STATE_X_NEG; } else if (emg22>threshold) { mystate = STATE_Y_NEG; } else { mystate = STATE_PAUZE; } } else { if (emg12>threshold&&emg22>threshold) { mystate = STATE_XY; } else if (emg12>threshold) { mystate = STATE_X; } else if (emg22>threshold) { mystate = STATE_Y; } else { mystate = STATE_PAUZE; } } if (buttonflag==true) { mystate = STATE_CALIBRATION; } if (myoldstate==mystate) { newcaseprint=false; newcase=false; } else { newcaseprint=true; newcase=true; } sampletimer = false; } void change_x(int direction) { ref_x=ref_x+0.0002*direction; } void change_y(int direction) { ref_y=ref_y+0.1*direction; } void change_xy(int direction) { ref_x=ref_x+0.1*direction; ref_y=ref_y+0.1*direction; } void my_pos() { pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y); } void print_state(states mystate) { // switch, case if (newcaseprint==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.baud(115200); //de biquad chains instellen 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 */ pos_timer.attach(&my_pos, 1); sample_timer.attach(&sampleflag, samplefreq); button_calibrate.fall(&buttonflag_go); while(1) { if (sampletimer==true) { //sample(mystate); sample_button(mystate); print_state(mystate); } } }