Diff: main.cpp

Revision:

39:955929e25858

Parent:

38:0c8a6b0834da

Child:

40:1ca50c657cc5

--- a/main.cpp	Wed Oct 26 13:00:12 2016 +0000
+++ b/main.cpp	Wed Oct 26 13:33:11 2016 +0000
@@ -3,6 +3,7 @@
 #include "BiQuad.h"
 #include "MODSERIAL.h"
 
+MODSERIAL pc(USBTX, USBRX);
 
 //Define objects
 //Define the button interrupt for the calibration
@@ -14,42 +15,30 @@
 AnalogIn    emg3( A2 );
 
 //Define the Tickers
-Ticker      pos_timer;
-Ticker      sample_timer;
-
-HIDScope    scope( 6 );
-MODSERIAL pc(USBTX, USBRX);
+Ticker      pos_timer;                      // the timer which is used to print the position every second
+Ticker      sample_timer;                   // the timer which is used to decide when a sample needs to be taken
 
 //Initialize all variables
-volatile bool sampletimer = false;
-volatile bool buttonflag = false;
-volatile bool newcase = false;
+volatile bool sampletimer = false;          // a variable which is changed when a sample needs to be taken
 
-double threshold = 0.04;
-double samplefreq=0.002;
-double emg02;
-double emg12;
-double emg22;
-double ref_x=0.0000;
-double ref_y=0.0000;
-double old_ref_x;
-double old_ref_y;
-double speed_emg=0.00002;
-double speed_key=0.000326;
-double speed=0.00002;
-double theta=0.0;
-double radius=0.0;
+double threshold = 0.04;                    // the threshold which the emg signals need to surpass to do something
+double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz 
+double emg02;                               // the first emg signal
+double emg12;                               // the second emg signal
+double emg22;                               // the third emg signal
+double ref_x=0.0000;                        // the x reference position
+double ref_y=0.0000;                        // the y reference position
+double old_ref_x;                           // the old x reference
+double old_ref_y;                           // the old y reference
+double speed=0.00002;                       // the variable with which a speed is reached of 1cm/s
+double theta=0.0;                           // angle of the arm
+double radius=0.0;                          // radius of the arm
 
-const double minRadius=0.3;                // minimum radius of arm
-const double maxRadius=0.6;                // maximum radius of arm
-const double minAngle=-1.25;               // minimum angle for limiting controller
+const double minRadius=0.3;                 // minimum radius of arm
+const double maxRadius=0.6;                 // maximum radius of arm
+const double minAngle=-1.25;                // minimum angle for limiting controller
 
-char key;
-
-
-// create a variable called 'mystate', 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;
+char key;                                   // variable to place the keyboard input
 
 //Define the needed Biquad chains
 BiQuadChain bqc11;
@@ -77,11 +66,11 @@
 BiQuad bq211 = bq111;
 BiQuad bq212 = bq112;
 BiQuad bq213 = bq113;
-/*  High pass filter*/
+//High pass filter
 BiQuad bq221 = bq121;
 BiQuad bq222 = bq122;
 BiQuad bq223 = bq123;
-/*  Low pass filter*/
+//Low pass filter
 BiQuad bq231 = bq131;
 
 //Define the BiQuads for the filter of the third emg signal
@@ -99,7 +88,8 @@
 void sampleflag()
 {
     if (sampletimer==true) {
-        pc.printf("rate too high error in setgoflag\n\r");
+        // this if statement is used to see if the code takes too long before it is called again
+        pc.printf("rate too high error in sampleflag\n\r");
     }
     //This sets the go flag for when the function sample needs to be called
     sampletimer=true;
@@ -112,89 +102,60 @@
     ref_y=0.0000;
 }
 
-void sample(states &mystate)
+void sample()
 {
-    states myoldstate=mystate;
-
-    //This checks if a key is pressed and adjusts the speed to the needed speed
+    //This checks if a key is pressed and changes the variable key in the pressed key
     if (pc.readable()==1) {
-
         key=pc.getc();
-        //printf("%c\n\r",key);
     }
-
-
     //Read the emg signals and filter it
 
-    scope.set(0, emg1.read());                          //original signal 0
     emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
-    scope.set(1, emg02);
-    scope.set(2, emg2.read());                          //original signal 1
     emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
-    scope.set(3, emg12);
-    scope.set(4, emg3.read());                          //original signal 2
     emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
-    scope.set(5, emg22);
 
-    emg02=1;
-    //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
-    //Finally, send all channels to the PC at once
-    scope.send();
-
+    //remember what the reference was
     old_ref_x=ref_x;
     old_ref_y=ref_y;
-    //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
+    //look if the emg signals go over the threshold and change the reference accordingly
     if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
-        mystate = STATE_XY_NEG;
         ref_x=ref_x-speed;
         ref_y=ref_y-speed;
 
     } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
-        mystate = STATE_X_NEG;
         ref_x=ref_x-speed;
 
     } else if (emg02>threshold&&emg22>threshold || key == 's') {
-        mystate = STATE_Y_NEG;
         ref_y=ref_y-speed;
 
     } 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;
     }
 
-    // convert ref to gearbox angle
+    // convert the x and y reference to the theta and radius reference
     theta=atan(ref_y/(ref_x+minRadius));
     radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
 
+    //look if the new reference is outside the possible range and revert back to the old reference if it is outside  the range
     if (theta < minAngle) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
+        
     } else if (radius < minRadius) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
+        
     } else if ( radius > maxRadius) {
         ref_x=old_ref_x;
         ref_y=old_ref_y;
     }
-
-    //change newcase so that the state will only be printed once
-    if (myoldstate==mystate) {
-        newcase=false;
-
-    } else {
-        newcase=true;
-    }
 }
 
 void my_pos()
@@ -204,46 +165,12 @@
 
 }
 
-void print_state()
-{
-    //This code looks in which state the robot is in and prints it to the screen
-    if (newcase==true) {
-        switch (mystate) {
-            case STATE_CALIBRATION : { // calibration
-                pc.printf("calibration\n\r");
-                break;
-            }
-            case STATE_X : // X direction
-                pc.printf("X\n\r");
-                break;
-            case STATE_X_NEG : // negative X direction
-                pc.printf("Xneg\n\r");
-                break;
-            case STATE_Y : // Y direction
-                pc.printf("Y\n\r");
-                break;
-            case STATE_Y_NEG : // negative Y direction
-                pc.printf("Yneg\n\r");
-                break;
-            case STATE_XY : // X&Y direction
-                pc.printf("XY\n\r");
-                break;
-            case STATE_XY_NEG : // negative X&Y direction
-                pc.printf("XYneg\n\r");
-                break;
-            case STATE_PAUZE : // Pauze: do nothing
-                pc.printf("PAUZE\n\r");
-                break;
-        }
-    }
-}
-
 int main()
 {
     pc.printf("RESET\n\r");
     pc.baud(115200);
 
-    //Initialize the Biquad chains
+    //Attach the Biquads to the Biquad chains
     bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
     bqc13.add( &bq131);
     bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
@@ -261,11 +188,10 @@
     pos_timer.attach(&my_pos, 1);
 
     while(1) {
-        //Only take samples when the go flag is true.
+        //Only take a sample when the go flag is true.
         if (sampletimer==true) {
-            sample(mystate);
-            print_state();
-            sampletimer = false;
+            sample();
+            sampletimer = false;            //change sampletimer to false if sample() is finished
         }
     }
 }
\ No newline at end of file