Diff: main/main.cpp

Revision:

7:7fefd782532d

Parent:

6:c10b367747a0

Child:

8:b862106f1162

--- a/main/main.cpp	Sat Feb 22 01:12:00 2020 +0000
+++ b/main/main.cpp	Sat Feb 22 03:21:56 2020 +0000
@@ -24,7 +24,7 @@
 void init();
 void calibrate();
 void follow_line( float speed );
-bool junction_detect();
+char junction_detect();
 char turn_logic();
 void turn_selector( char turn );
 void left();
@@ -96,10 +96,15 @@
         }
         
         //follow_line(speed);
-        
+        if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
+            wait(0.05);
+            if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
+                goal();
+            }
+        }
         // add if juntion detected, move forward until both 0 and 4 or the middle three are low, then check the type of junction and turn accordingly (lines below)
         //if ( junction_detect() == true ){
-            char turn = turn_logic(); // rename this function something else
+            char turn = junction_detect(); // rename this function something else
             turn_selector(turn);
             if ( turn != 'S' && turn != 'R') {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
                 path[path_length] = turn;
@@ -154,29 +159,107 @@
 
 }
 
-bool junction_detect() 
+char junction_detect() 
 {
-    /*bool left = false;
+    bool left = false;
     bool right = false;
-    // then move forward until either three middle are low, or left is low or right is low, then return what junction it was.
-    if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
-        while (1) { 
-            robot.scan(); 
-            if (sens[0] < sens_thresh) { break;
+    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';
+    }
+    
+    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;
+                }
+                
+            }
+            robot.stop();
+            wait(0.2);
+            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;
+                }
+                
+                
+                robot.scan();
+            }
+            
+            robot.stop();
             
+            if ( sensor[1] > sens_thresh || sensor[2] > sens_thresh || sensor[3] > sens_thresh ) {
+                straight = 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;
+                }
+                
+            }
+            
+            robot.stop();
+            wait(0.2);
+            robot.scan();
+            
+            if ( sensor[0] > sens_thresh && (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) ) {
+                robot.forward(speed);
+                goal_test1 = true;
+                while ( sensor[0] > sens_thresh && sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) { 
+                    robot.scan();
+                }
+                robot.scan();
+                if ( sensor[0] > sens_thresh && (sensor[1] > sens_thresh && sensor[2] > sens_thresh && sensor[3] > sens_thresh)) {
+                    goal_test2 = true;
+                    //goal();
+                }
+                robot.scan();
+            }
+            
+            robot.stop();
+            
+            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[4] > sens_thresh ) {
-        return true;
     }
-    return false;*/
+    
+    return 'S';
     
-    if ( sensor[0] > sens_thresh || sensor [4] > sens_thresh || (sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500)) {
+    /*
+    if ( sensor[0] > sens_thresh || sensor [4] > sens_thresh || (sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500) ) {
         return true;
     } else {
         return false;
-    }
+    }*/
 }
 
 char turn_logic() 
@@ -186,9 +269,7 @@
     
     //or have it so it inches forward at a junction (if junction detected, then inch forward and decide
     
-    if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
-        return 'G';
-    }  else if ( sensor[0] > 500 ) {
+    if ( sensor[0] > 500 ) {
         return 'L';
     } else if ( sensor[1] > 500 || sensor[2] > 500 || sensor[3] > 500 ) {
         return 'S';
@@ -226,8 +307,6 @@
 
     while (sensor[0] > 500) { robot.scan(); }
     
-    if (speed <= 0.20) { wait(0.1); }
-    
     robot.spin_left(0.2);
     wait(0.1);
     
@@ -242,8 +321,6 @@
 
     while (sensor[4] > 500) { robot.scan(); }
     
-    if (speed <= 0.20) { wait(0.1); }
-    
     robot.spin_right(0.2);
     wait(0.1);