Diff: main.cpp

Revision:

31:9c3b022f1dc3

Parent:

30:7cc564d38bc4

Child:

32:e1e5f03e9faf

--- 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