Diff: main.cpp

Revision:

45:653370fa8b67

Parent:

44:fd7b3ace6c19

Child:

46:c03f2c576630

--- a/main.cpp	Mon Nov 02 12:24:45 2015 +0000
+++ b/main.cpp	Mon Nov 02 13:14:59 2015 +0000
@@ -241,19 +241,24 @@
     }
 
 }
-//adjust controller values when sw2 is pressed
+//adjust controller values (and wheter or not demo is shown) when sw2 is pressed
 void valuechange()
 {
-    mycontroller.STOP(); // stop motors
-    pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
-    pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1
+    if (modecounter==4) {// cannot be changed if demo loop is running
+        pc.printf("Not now!\n");
+    } else {
+        mycontroller.STOP(); // stop motors
+        pc.printf("Extra gain for rigth arm is now %f, enter new value\n",filter_extragain1); // print extra gain for rigth arm
+        pc.scanf("%f", &filter_extragain1); //read the input from serial and write to filter_extragain1
 
-    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("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
+        pc.printf("Do you want to enter demo mode after button control mode 1/0? (1 for yes, 0 for no.\n");
+        pc.scanf("%i", &demo_go);//read the input from serial and write to filter_extragain2
 
+        pc.printf("Done\n");        
+    }
 }
 
 // shoot the pod forward
@@ -411,8 +416,10 @@
     } else {
         modecounter=modecounter;
     }
+
 }
 
+// function takes x,y and time as input and moves pod to the desired coordinates in the desired time
 void gotopos(double desired_x_pos, double desired_y_pos, double time_to_pos)
 {
     double timepos=0;
@@ -425,9 +432,9 @@
 
     double pos_x_moving=pos_x_start;
     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;
 
@@ -440,9 +447,7 @@
         mycontroller.PID(error1,error2,Kp_shoot,Ki_shoot,Kd_shoot);;// send errors to controller
 
         timepos+=(Ts_control);// add time it should take to calculated time
-
         wait(timepos-postimer.read());// IMPORTANT wait until the loop has taken the time it should need, if this is not done the loop wil go to fast and the motors can't keep up
-
     }
 }
 
@@ -483,7 +488,7 @@
                 pc.printf("Program running\n");// print to serial
                 led1=led2=ledgreen=0;// green led on leds on top off
                 ledred=ledblue=1;//rest off
-                gotopos(0,demo_y_goal,1);
+                gotopos(0,y_start,1);
                 desired_position=0;
                 switchedmode=false;
             }
@@ -597,13 +602,20 @@
             }
         }
         ///////////////////////////////////DEMO MODE
+        if  (modecounter==4 && demo_go==!1) {//if demo mode not enabled, move to next mode
+            changemode();
+        }
         if (modecounter==4 && demo_go==1) {
             if (switchedmode==true) {
                 pc.printf("Demo mode, the pod moves around the field\n");
-                led1=led2=ledred=ledblue=ledgreen=0; // rgb on leds on top off
+                led1=led2=ledred=ledblue=ledgreen=0; // rgb on, leds on top off
                 gotopos(0,demo_y_goal,1); // go to goal @x=0
                 switchedmode=false;
             }
+
+            // loop trough positions to make a path along the field, con of this method is that the mode is only changed when the loop is completed.
+            // the button is read, but only when the loop is completed it is checked in what mode the robot has to be, that's why this mode is made optional,
+            // because this mode is only neccesary for the presentation on 30-10-2015 (or for fun)
             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
@@ -613,8 +625,10 @@
             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
+            gotopos(0,y_start,0.5);
             demo_go=0;
         }
+
         //////////////////////////////////EMG GAIN AND THRESHOLD CALIBRATION MODE
         if(modecounter==5) {
             if (switchedmode==true) {