Diff: main.cpp

Revision:

18:acc404d8488c

Parent:

17:4e63e8eacad7

--- a/main.cpp	Mon Jun 08 11:27:29 2015 +0000
+++ b/main.cpp	Mon Jun 08 14:11:25 2015 +0000
@@ -98,11 +98,9 @@
         
 void TurnAround() {
     
-    m3pi.stop();
-    wait(1.0);
             TurnTime.reset();
             TurnTime.start();
-            
+            m3pi.stop();
                 while (1) {
             
                 m3pi.left_motor(0.25);
@@ -125,8 +123,8 @@
         
         while(1) {
             
-            m3pi.right_motor(0.5);
-            m3pi.left_motor(0.5);
+            m3pi.right_motor(MAX);
+            m3pi.left_motor(MAX);
             
             if(TurnTime.read() > 0.02) {
                 
@@ -280,7 +278,7 @@
         rga.at(4).at(5) = "RLLLRLX";
         
         rga.at(5).at(0) = "LRLLRX";
-        rga.at(5).at(1) = "LSLX";
+        rga.at(5).at(1) = "LRSLRX";
         rga.at(5).at(2) = "LRSSSX";
         rga.at(5).at(3) = "RLRLSX";
         rga.at(5).at(4) = "RLRRRLX";
@@ -361,6 +359,7 @@
     m3pi.cls();
     m3pi.printf("GO!");
     wait(.5);
+    m3pi.cls();
     
     LapTime.start();
     
@@ -457,10 +456,10 @@
                         
                         m3pi.left_motor(0.5);
                         m3pi.right_motor(0.5);
-                        wait(0.07);
+                        //wait(0.05);
+
                         btbee.printf("Reached goal. Turning around\n");
                         TurnAround();
-                        wait(1.0);
                         
                         location = goal;
                         
@@ -476,9 +475,9 @@
                             location = command.at(i);
                             i++;
                             goal = command.at(i);
-                            
+                            TotalTime.reset();
                         
-                        }
+                        }                        
                         
                         path = rga.at(location-1).at(goal-1);