Diff: main/main.cpp

Revision:

9:952586accbf9

Parent:

8:b862106f1162

Child:

10:691c02b352cb

diff -r b862106f1162 -r 952586accbf9 main/main.cpp
--- a/main/main.cpp	Sat Feb 22 04:46:51 2020 +0000
+++ b/main/main.cpp	Sat Feb 22 05:01:27 2020 +0000
@@ -164,12 +164,7 @@
     bool left = false;
     bool right = false;
     bool straight = false;
-    bool goal_test1 = false;
-    bool goal_test2 = false;
-    
-    if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
-        return 'B';
-    }
+    bool goal = false;
     
     if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {
         if (sensor[0] > sens_thresh) {
@@ -180,19 +175,17 @@
                 if (sensor[4] > sens_thresh) {
                     right = true;
                 }
-                
             }
             
+            robot.scan();
+            
             if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
-                robot.forward(speed);
                 if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
-                    goal_test1 = true;
+                    goal = true;
                 }
-                
-                
-                robot.scan();
             }
             
+            robot.scan();
             
             if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
                 straight = true;
@@ -207,35 +200,32 @@
                 if (sensor[0] > sens_thresh) {
                     left = true;
                 }
-                
             }
             
+            robot.scan();
+            
             if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
                 straight = true;
             }
         }  
         
-        
-        if (goal_test1) {
-            return 'G';
-        } else if (left) {
-            return 'L';
-        } else if (straight) {
-            return 'S';
-        } else if (right) {
-            return 'R';
-        }
-        
+    } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
+        return 'B';
     }
     
-    return 'S';
+    
     
-    /*
-    if ( sensor[0] > sens_thresh || sensor [4] > sens_thresh || (sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500) ) {
-        return true;
+    if (goal) {
+        return 'G';
+    } else if (left) {
+        return 'L';
+    } else if (straight) {
+        return 'S';
+    } else if (right) {
+        return 'R';
     } else {
-        return false;
-    }*/
+        return 'S';
+    }
 }
 
 char turn_logic()