Diff: Robot-bae8eb81a9d7/main.cpp

Revision:

4:a9823e6dab12

Parent:

3:4284d7cdb68e

--- a/Robot-bae8eb81a9d7/main.cpp	Thu Jun 04 12:18:04 2015 +0000
+++ b/Robot-bae8eb81a9d7/main.cpp	Mon Jun 08 11:23:07 2015 +0000
@@ -9,9 +9,9 @@
 #define MAX 0.5
 #define MIN 0
 
-#define P_TERM 2.5
-#define I_TERM 2
-#define D_TERM 23
+#define P_TERM 3
+#define I_TERM 0
+#define D_TERM 27
 
 void turnRight();
 void turnLeft();
@@ -54,7 +54,7 @@
 
 
     time_wait.start();
-    
+     turns = blah[(6-1)*6+(5-1)];  
 
     while(Run) {
         time_wait.reset();
@@ -64,7 +64,7 @@
 
         //insert end and beginning locations
 
-        turns = blah[(end-1)*6+(start-1)];
+  
 
         //Check to make sure battery isn't low
         if (robot.battery() < 2.4) {
@@ -74,7 +74,7 @@
 
         
         
-        if((x[0]>300 || x[4]>300) && (direction < turns.size()) && !passed) {
+        if(((x[0]>300 && (turns.at(direction) == 'L')) || (x[4]>300 && (turns.at(direction) == 'R')) || ((x[0] > 300 || x[4] > 300) && turns.at(direction) == 'S')) && (direction < turns.size()) && !passed) {
             
             
             if(turns.at(direction) == 'L'){
@@ -89,7 +89,7 @@
                     turnLeft();
                     robot.calibrated_sensor(x);
                 }
-               
+            
                 while((x[0] > 300) || (x[2] < 300)){
                     turnLeft();
                     robot.calibrated_sensor(x);
@@ -142,6 +142,10 @@
             robot.stop();
             robot.cls();
             robot.printf("Try again");
+            while(x[2] < 300 || x[0] < 300 || x[4] < 300){
+                turnRight();
+                robot.calibrated_sensor(x);
+            }
             break;
         }
         else if (x[0]>300 || x[4]>300 && passed)