Diff: main.cpp

Revision:

44:fd7b3ace6c19

Parent:

43:a5f47a1f11d2

Child:

45:653370fa8b67

--- a/main.cpp	Mon Nov 02 11:48:52 2015 +0000
+++ b/main.cpp	Mon Nov 02 12:24:45 2015 +0000
@@ -37,7 +37,7 @@
 const double readbuttoncalibrate_frequency=50;//frequency at which the buttons are read when in calibration mode
 const double ledblink_frequency=4;//frequency at which the green led and leds on top blink when in resp button or calibration mode
 
-DigitalIn changemodebutton(PTA4);// button to change mode (sw3)
+InterruptIn changemodebutton(PTA4);// button to change mode (sw3)
 Ticker readbuttoncalibrate_ticker;//ticker for reading out the buttons when calibrating
 Ticker ledblink_ticker;// ticker for blinking the leds
 
@@ -125,9 +125,12 @@
 const double demo_goal_edge_rigth=0.1+demo_safety;// x edges of goal
 const double demo_goal_edge_left=-(0.1+demo_safety);
 const double demo_y_goal=0.1+demo_safety;// y position in front of goal
-const double demo_y_back=0.05+demo_safety;//back edge of the field 
+const double demo_y_back=0.05+demo_safety;//back edge of the field
 const double demo_y_front=y_punch;
 
+int demo_go=0;
+
+
 
 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
 volatile bool scopedata_go=false,
@@ -247,6 +250,10 @@
 
     pc.printf("Extra gain for left arm is now %f, enter new value\n",filter_extragain2); // print extra gain for left arm
     pc.scanf("%f", &filter_extragain2);//read the input from serial and write to filter_extragain2
+
+    pc.printf("Do you want to enter demo mode after button control mode 1/0?");
+    pc.scanf("%i", &demo_go);//read the input from serial and write to filter_extragain2
+
 }
 
 // shoot the pod forward
@@ -404,8 +411,6 @@
     } else {
         modecounter=modecounter;
     }
-    wait(1);// needed because else it is checked too fast if the button is pressed and modes change too fast
-    // tried it with interruptin but didn't work, bt this works good
 }
 
 void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
@@ -422,6 +427,7 @@
     double pos_y_moving=pos_y_start;
     postimer.start();
     while(timepos<=time_to_pos) {
+        changemodebutton.rise(&changemode);// interruptin for next mode
         pos_x_moving-=pos_stepsize_x;
         pos_y_moving-=pos_stepsize_y;
 
@@ -457,11 +463,10 @@
 
     while(1) {// while (1) so continious loop
         valuechangebutton.fall(&valuechange);// used to change the filter gains
+        changemodebutton.rise(&changemode);// interruptin for next mode
         checktimer.reset();// reset the timer to check the time it takes to run the entire control loop
         checktimer.start();
-        if (changemodebutton==0) {// check if the change mode button is pressed
-            changemode();//call changemode
-        }
+
         if (scopedata_go==true) {//send scopedata
             //TIME THIS LOOP TAKES: 0.000008 SEC (PEAKS AT 0.000015)
             scopedata(y_start);// call scopedata, use baseline as desired y position
@@ -476,8 +481,8 @@
         if(modecounter==0) {
             if (switchedmode==true) {
                 pc.printf("Program running\n");// print to serial
-                ledgreen=0;// green led on
-                led1=led2=ledred=ledblue=1;//rest of
+                led1=led2=ledgreen=0;// green led on leds on top off
+                ledred=ledblue=1;//rest off
                 gotopos(0,demo_y_goal,1);
                 desired_position=0;
                 switchedmode=false;
@@ -592,30 +597,30 @@
             }
         }
         ///////////////////////////////////DEMO MODE
-        if (modecounter==4) {
+        if (modecounter==4 && demo_go==1) {
             if (switchedmode==true) {
                 pc.printf("Demo mode, the pod moves around the field\n");
-                ledred=ledblue=ledgreen=0;
+                led1=led2=ledred=ledblue=ledgreen=0; // rgb on leds on top off
                 gotopos(0,demo_y_goal,1); // go to goal @x=0
-                led1=led2=0;
                 switchedmode=false;
             }
-            gotopos(demo_goal_edge_left,demo_y_goal,0.25);
-            gotopos(demo_goal_edge_left,demo_y_back,0.25);
-            gotopos(demo_x_edge_left,demo_y_back,0.5);
-            gotopos(demo_x_edge_left,demo_y_front,2);
-            gotopos(demo_x_edge_rigth,demo_y_front,2);
-            gotopos(demo_x_edge_rigth,demo_y_back,2);
-            gotopos(demo_goal_edge_rigth,demo_y_back,1.25);
-            gotopos(demo_goal_edge_rigth,demo_y_goal,0.25);
-            gotopos(0,demo_y_goal,0.5);
+            gotopos(demo_goal_edge_left,demo_y_goal,0.5);// in front of goal to left edge of goal
+            gotopos(demo_goal_edge_left,demo_y_back,0.25); // to back along left egde of goal
+            gotopos(demo_x_edge_left,demo_y_back,0.5); //to left edge along back
+            gotopos(demo_x_edge_left,demo_y_front,2);//from back to front along left edge
+            gotopos(demo_x_edge_rigth,demo_y_front,2);// from left to rigth along front edge
+            gotopos(demo_x_edge_rigth,demo_y_back,2);// from front to back along rigth edge
+            gotopos(demo_goal_edge_rigth,demo_y_back,0.5);//from rigth edge to rigth edge of goal along back
+            gotopos(demo_goal_edge_rigth,demo_y_goal,0.25); // from back to in front of goal along rigth edge goal
+            gotopos(0,demo_y_goal,0.5); // from rigth edge goal to middle in front of goal
+            demo_go=0;
         }
         //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
         if(modecounter==5) {
             if (switchedmode==true) {
                 pc.printf("Calibrate the EMG signals and threshold\n");
-                ledgreen=1;
-                ledred=ledblue=0;
+                ledblue=ledgreen=1;
+                led1=led2=ledred=0;
                 switchedmode=false;
             }
             if(ledblink_go==true) {