Diff: main.cpp

Revision:

36:344588e69589

Parent:

35:4905144c1123

Child:

37:af85a7b57a25

--- a/main.cpp	Wed Oct 26 08:52:05 2016 +0000
+++ b/main.cpp	Wed Oct 26 10:04:38 2016 +0000
@@ -3,6 +3,8 @@
 #include "BiQuad.h"
 #include "MODSERIAL.h"
 
+MODSERIAL pc(USBTX, USBRX);
+
 //Define objects
 //Define the button interrupt for the calibration
 InterruptIn button_calibrate(PTA4);
@@ -17,8 +19,8 @@
 Ticker      sample_timer;
 
 HIDScope    scope( 6 );
-MODSERIAL pc(USBTX, USBRX);
 
+//Initialize all variables
 volatile bool sampletimer = false;
 volatile bool buttonflag = false;
 volatile bool newcase = false;
@@ -30,12 +32,15 @@
 double emg22;
 double ref_x=0.000;
 double ref_y=0.000;
-double speed=0.00002;
+double speed_emg=0.00002;
+double speed_key=0.000326;
+double speed;
+
 const int negative=-1;
 char key;
 
 
-// create a variable called 'state', define it
+// 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;
 
@@ -53,7 +58,10 @@
 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 );
+//BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
+BiQuad bq121( 0.8956,   -1.7911,    0.8956,    1.0000,   -1.7814,    0.7941);
+BiQuad bq122( 0.9192,   -1.8385,    0.9192,    1.0000,   -1.8319,    0.8450);
+BiQuad bq123( 0.9649,   -1.9298,    0.9649,    1.0000,   -1.9266,    0.9403);
 //Low pass filter
 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
 
@@ -64,6 +72,8 @@
 BiQuad bq213 = bq113;
 /*  High pass filter*/
 BiQuad bq221 = bq121;
+BiQuad bq222 = bq122;
+BiQuad bq223 = bq123;
 /*  Low pass filter*/
 BiQuad bq231 = bq131;
 
@@ -74,51 +84,54 @@
 BiQuad bq313 = bq113;
 //High pass filter
 BiQuad bq321 = bq121;
+BiQuad bq323 = bq122;
+BiQuad bq322 = bq123;
 //low pass filter
 BiQuad bq331 = bq131;
 
-
 void sampleflag()
 {
+    //This sets the go flag for when the function sample needs to be called
     sampletimer=true;
 }
 
-void buttonflag_go()
+void calibrate()
 {
-    buttonflag=true;
+    //This resets the reference signals so that the robot can be calibrated
+    ref_x=0.0;
+    ref_y=0.0;
 }
 
 void sample(states &mystate)
 {
     states myoldstate=mystate;
-    
+
+    //This checks if a key is pressed and adjusts the speed to the needed speed
     if (pc.readable()==1) {
         key=pc.getc();
-        speed=0.000326;
+        speed=speed_key;
     } else {
         key ='p';
-        speed=0.00002;
+        speed=speed_emg;
     }
 
-    /* Read the emg signals and filter it*/
+    //Read the emg signals and filter it
 
-    scope.set(0, emg1.read());    //original signal
-    emg02=bqc13.step(fabs(bqc11.step(emg1.read())));
+    scope.set(0, emg1.read());                          //original signal 0
+    emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
     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(2, emg2.read());                          //original signal 1
+    emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
     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(4, emg3.read());                          //original signal 2
+    emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
     scope.set(5, emg22);
 
-    /*   Ensure that enough channels are available (HIDScope scope( 2 ))
-     *   Finally, send all channels to the PC at once */
+    //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
+    //Finally, send all channels to the PC at once
     scope.send();
 
-
+    //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
     if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
         mystate = STATE_XY_NEG;
         ref_x=ref_x+speed*negative;
@@ -145,13 +158,10 @@
         mystate = STATE_Y;
         ref_y=ref_y+speed;
     } else {
-        mystate = STATE_PAUZE;
+        //mystate = STATE_PAUZE;
     }
 
-    if (buttonflag==true) {
-        mystate = STATE_CALIBRATION;
-    }
-
+    //change newcase so that the state will only be printed once
     if (myoldstate==mystate) {
         newcase=false;
 
@@ -162,37 +172,39 @@
 
 void my_pos()
 {
+    //This function is attached to a ticker so that the reference position is printed every second.
     pc.printf("x_pos=%.4f\ty_pos=%.4f\n\r",ref_x,ref_y);
 
 }
 
 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 : // run
+            case STATE_X : // X direction
                 pc.printf("X\n\r");
                 break;
-            case STATE_X_NEG : // run
+            case STATE_X_NEG : // negative X direction
                 pc.printf("Xneg\n\r");
                 break;
-            case STATE_Y : // execute mode 1
+            case STATE_Y : // Y direction
                 pc.printf("Y\n\r");
                 break;
-            case STATE_Y_NEG : // execute mode 1
+            case STATE_Y_NEG : // negative Y direction
                 pc.printf("Yneg\n\r");
                 break;
-            case STATE_XY : // execute mode 2
+            case STATE_XY : // X&Y direction
                 pc.printf("XY\n\r");
                 break;
-            case STATE_XY_NEG : // execute mode 2
+            case STATE_XY_NEG : // negative X&Y direction
                 pc.printf("XYneg\n\r");
                 break;
-            case STATE_PAUZE : // default
+            case STATE_PAUZE : // Pauze: do nothing
                 pc.printf("PAUZE\n\r");
                 break;
         }
@@ -204,25 +216,28 @@
     pc.printf("RESET\n\r");
     pc.baud(115200);
 
-    //make the Biquad chains
-    bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 );
+    //Initialize 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 );
+    bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
     bqc23.add( &bq231);
-    bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 );
+    bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
     bqc33.add( &bq331);
-    /*Attach the 'sample' function to the timer 'sample_timer'.
-      this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
-    */
+
+    //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);
+    //Attach the function calibrate to the button interrupt
+    button_calibrate.fall(&calibrate);
+    //Attach the function my_pos to the timer pos_timer.
+    //This ensures that the position is printed every second.
     pos_timer.attach(&my_pos, 1);
 
     while(1) {
+        //Only take samples when the go flag is true.
         if (sampletimer==true) {
-            //sample(mystate);
             sample(mystate);
-          //  print_state();
+            print_state();
             sampletimer = false;
         }
     }