Diff: main/main.cpp

Revision:

13:bd271266e161

Parent:

12:d80399686f32

Child:

14:87052bb35211

diff -r d80399686f32 -r bd271266e161 main/main.cpp
--- a/main/main.cpp	Mon Feb 24 06:07:03 2020 +0000
+++ b/main/main.cpp	Mon Feb 24 06:23:15 2020 +0000
@@ -68,8 +68,9 @@
 
         follow_line();
 
-        char turn = junction_detect(); // rename this function something else
+        char turn = junction_detect(); 
         turn_selector(turn);
+        
         if ( turn != 'S' ) {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
             path[path_length] = turn;
             path_length ++;
@@ -145,64 +146,23 @@
 {
     bool left = false;
     bool right = false;
-    bool straight = false;
     bool goal = false;
     
     if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
-        if (sensor[0] > sens_thresh) {
-            left = true;
-            while ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
-                robot.forward(speed);
-                robot.scan();
-                if (sensor[4] > sens_thresh) {
-                    right = true;
-                }
-            }
-            
+        while ( (sensor[0] > sens_thresh || sensor[4] > sens_thresh) && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
+            robot.forward(speed);
             robot.scan();
-            
-            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-                wait(0.05);
-                robot.scan();
-                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-                    goal = true;
-                }
-            }
-            
-            robot.scan();
-            
-            if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
-                straight = true;
-            }
-        }  
+            if ( sensor[0] > sens_thresh ) { left = true; }
+            if ( sensor[4] > sens_thresh ) { right = true; }
+        }
         
-        else if (sensor[4] > sens_thresh) {
-            right = true;
-            while ( sensor[4] > sens_thresh && (sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh) ) {
-                robot.forward(speed);
-                robot.scan();
-                if (sensor[0] > sens_thresh) {
-                    left = true;
-                }
-            }
-            
+        if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
+            wait(0.1);   // maybe replace or change somehow
             robot.scan();
-            
             if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-                wait(0.05);
-                robot.scan();
-                if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-                    goal = true;
-                }
+                goal = true;
             }
-            
-            robot.scan();
-            
-            if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
-                straight = true;
-            }
-        }  
-        
+        }
     } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
         return 'B';
     }
@@ -211,8 +171,6 @@
         return 'G';
     } else if (left) {
         return 'L';
-    } else if (straight) {
-        return 'S';
     } else if (right) {
         return 'R';
     } else {
@@ -246,7 +204,7 @@
 
     while (sensor[0] > 500) { robot.scan(); }
     
-    robot.spin_left(0.2);
+    robot.spin_left(turn_speed);
     wait(0.1);
     
     while (sensor[1] < 500) { robot.scan(); }
@@ -260,7 +218,7 @@
 
     while (sensor[4] > 500) { robot.scan(); }
     
-    robot.spin_right(0.2);
+    robot.spin_right(turn_speed);
     wait(0.1);
     
     while (sensor[3] < 500) { robot.scan(); }
@@ -273,7 +231,7 @@
     leds = 0b1111;
     robot.reverse(speed);
     wait(0.1);
-    robot.spin_right(0.2);
+    robot.spin_right(turn_speed);
     
     while (sensor[3] < 500) { robot.scan(); }