the emg filtering part of the program
Dependencies: HIDScope biquadFilter mbed MODSERIAL
Fork of EMG by
Diff: main.cpp
- Revision:
- 31:9c3b022f1dc3
- Parent:
- 30:7cc564d38bc4
- Child:
- 32:e1e5f03e9faf
diff -r 7cc564d38bc4 -r 9c3b022f1dc3 main.cpp --- a/main.cpp Tue Oct 25 08:04:01 2016 +0000 +++ b/main.cpp Tue Oct 25 10:39:11 2016 +0000 @@ -4,25 +4,27 @@ #include "MODSERIAL.h" //Define objects +//Define the button interrupt for the calibration +InterruptIn button_calibrate(PTA4); +InterruptIn button_change_mode(PTC6); -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); -DigitalOut led(LED1); volatile bool sampletimer = false; volatile bool buttonflag = false; volatile bool newcase = false; -volatile bool newcaseprint = false; -volatile bool change_ref_timer = false; -double threshold = 0.04; +double threshold = 0.11; double samplefreq=0.002; double emg02; double emg12; @@ -30,55 +32,56 @@ double ref_x=0.000; double ref_y=0.000; double speed=0.2; +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; -//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*/ + +//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*/ +//High pass filter BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); -/* low pass filter*/ +//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); +//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( 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 ); +BiQuad bq221 = bq121; +/* Low pass filter*/ +BiQuad bq231 = bq131; -/* 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 ); - +//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; - change_ref_timer=true; } void buttonflag_go() @@ -86,94 +89,6 @@ 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; - } -} - void sample(states &mystate) { states myoldstate=mystate; @@ -196,31 +111,40 @@ /* 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 */ + if (pc.readable()){ + key=pc.getc(); + } + else { + key='p'; + } - led = !led; - if (emg02>threshold) { - if (emg12>threshold&&emg22>threshold) { - mystate = STATE_XY_NEG; - } else if (emg12>threshold) { - mystate = STATE_X_NEG; + 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 (emg22>threshold) { - mystate = STATE_Y_NEG; - } else { - mystate = STATE_PAUZE; - } + } 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 { - 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; - } + mystate = STATE_PAUZE; } if (buttonflag==true) { @@ -228,31 +152,13 @@ } if (myoldstate==mystate) { - newcaseprint=false; newcase=false; + } else { - newcaseprint=true; newcase=true; } } -void change_x(int direction) -{ - ref_x=ref_x+speed*direction; - -} - -void change_y(int direction) -{ - ref_y=ref_y+speed*direction; - -} -void change_xy(int direction) -{ - ref_x=ref_x+speed*direction; - ref_y=ref_y+speed*direction; -} - void my_pos() { pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y); @@ -261,8 +167,7 @@ void print_state() { - // switch, case - if (newcaseprint==true) { + if (newcase==true) { switch (mystate) { case STATE_CALIBRATION : { // calibration pc.printf("calibration\n\r"); @@ -291,36 +196,14 @@ break; } } - } -void change_ref() -{ - if (change_ref_timer==true) { - if (mystate == STATE_X) { - change_x(1); - } else if (mystate == STATE_X_NEG) { - change_x(-1); - } else if (mystate == STATE_Y) { - change_y(1); - } else if (mystate == STATE_Y_NEG) { - change_y(-1); - } else if (mystate == STATE_XY) { - change_xy(1); - } else if (mystate == STATE_XY_NEG) { - change_xy(-1); - } - change_ref_timer=false; - } -} - - int main() { pc.printf("RESET\n\r"); pc.baud(115200); - //de biquad chains instellen + //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 ); @@ -330,20 +213,16 @@ /*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); - + pos_timer.attach(&my_pos, 1); while(1) { if (sampletimer==true) { //sample(mystate); - sample_button(mystate); + sample(mystate); print_state(); - change_ref(); sampletimer = false; - - } } } \ No newline at end of file