Diff: main/main.cpp

Revision:

11:c3299aca7d8f

Parent:

10:691c02b352cb

Child:

12:d80399686f32

--- a/main/main.cpp	Mon Feb 24 04:51:39 2020 +0000
+++ b/main/main.cpp	Mon Feb 24 05:36:03 2020 +0000
@@ -299,51 +299,54 @@
 void goal()
 { 
     invert_path();
-    int pointer = 0;   //path_length - 1
+    
+    leds = 0b0000;
     
     robot.lcd_clear();
     robot.lcd_print(path,100);
     
-    robot.stop();
-    while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
-    
-    leds = 0b1001;
-    wait(0.2);
-    leds = 0b0110;
-    wait(0.2);
-    leds = 0b1001;
-    wait(0.2);
-    leds = 0b0110;
-    wait(0.2);
-    
-    //robot.reverse(speed);
-//    back();
-    
-    while(pointer <= path_length) {
-        follow_line();
+    while(1) {
+        int pointer = 0;   //path_length - 1
+        robot.stop();
+        while (button_enter.read() == 1) {}  // keep looping waiting for Enter to be pressed
+        
+        leds = 0b1001;
+        wait(0.2);
+        leds = 0b0110;
+        wait(0.2);
+        leds = 0b1001;
+        wait(0.2);
+        leds = 0b0110;
+        wait(0.2);
+        
+        //robot.reverse(speed);
+    //    back();
         
-        if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {  // if junction found
-            turn_selector(path[pointer]);
-            if(path[pointer] == 'S') {      // make this better
-                robot.forward(speed);
-                leds = 0b1010;
-                wait(0.3);
-                // while((sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+        while(pointer <= path_length) {
+            follow_line();
+            
+            if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {  // if junction found
+                turn_selector(path[pointer]);
+                if(path[pointer] == 'S') {      // make this better
+                    robot.forward(speed);
+                    leds = 0b1010;
+                    while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+                }
+                pointer++;
             }
-            pointer++;
         }
-    }
-    
-    robot.stop();
-    while(1) {
-        leds = 0b1000;
-        wait(0.2);
-        leds = 0b0100;
-        wait(0.2);
-        leds = 0b0010;
-        wait(0.2);
-        leds = 0b0001;
-        wait(0.2);
+        
+        robot.stop();
+        while(button_enter.read() == 1) {
+            leds = 0b1000;
+            wait(0.2);
+            leds = 0b0100;
+            wait(0.2);
+            leds = 0b0010;
+            wait(0.2);
+            leds = 0b0001;
+            wait(0.2);
+        }
     }
 }