Diff: main/main.cpp

Revision:

6:c10b367747a0

Parent:

5:ae417756235a

Child:

7:7fefd782532d

--- a/main/main.cpp	Sat Feb 22 00:32:42 2020 +0000
+++ b/main/main.cpp	Sat Feb 22 01:12:00 2020 +0000
@@ -24,7 +24,8 @@
 void init();
 void calibrate();
 void follow_line( float speed );
-char junction_detect();
+bool junction_detect();
+char turn_logic();
 void turn_selector( char turn );
 void left();
 void right();
@@ -97,15 +98,16 @@
         //follow_line(speed);
         
         // 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 (junc_detect()) {
+        //if ( junction_detect() == true ){
             char turn = turn_logic(); // 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;
+                path_length ++;
+            }
         
-        if ( turn != 'S' && turn != 'R') {              // doesnt need 'S', also may not need 'R' as that is not technically a junction
-            path[path_length] = turn;
-            path_length ++;
-        }
+        //}
+        
         
  
         
@@ -152,14 +154,39 @@
 
 }
 
-char junction_detect() 
+bool junction_detect() 
+{
+    /*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;
+            
+        }
+        
+        
+    } else if ( sensor[4] > sens_thresh ) {
+        return true;
+    }
+    return false;*/
+    
+    if ( sensor[0] > sens_thresh || sensor [4] > sens_thresh || (sensor[0] < 500 && sensor[1] < 500 && sensor[2] < 500 && sensor[3] < 500 && sensor[4] < 500)) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+char turn_logic() 
 {
     
     // either increase the wait in main loop 50-> 20 
     
     //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 ) {
+    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 ) {
         return 'L';