Diff: main/main.cpp

Revision:

14:87052bb35211

Parent:

13:bd271266e161

Child:

15:6c461501d12d

diff -r bd271266e161 -r 87052bb35211 main/main.cpp
--- a/main/main.cpp	Mon Feb 24 06:23:15 2020 +0000
+++ b/main/main.cpp	Mon Feb 24 06:52:04 2020 +0000
@@ -24,8 +24,9 @@
 void init();
 void calibrate();
 void follow_line();
-char junction_detect();
-void turn_selector( char turn );
+bool junction_detect();
+char junction_logic();
+void turn_select( char turn );
 void left();
 void right();
 void back();
@@ -67,14 +68,15 @@
     while (1) {
 
         follow_line();
-
-        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
+        if ( junction_detect() ) {
+            char turn = junction_logic(); 
+            turn_select(turn);
+            
             path[path_length] = turn;
             path_length ++;
         }
+
         
         simplify();
         
@@ -142,8 +144,20 @@
     }
 }
 
-char junction_detect() 
+bool junction_detect() 
 {
+    if ( sensor[0] > sens_thresh || sensor[4] > sens_thresh ) {
+        return true;
+    } else if ( sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh ) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+char junction_logic() 
+{
+    bool straight = false;
     bool left = false;
     bool right = false;
     bool goal = false;
@@ -157,12 +171,19 @@
         }
         
         if ( sensor[0] > sens_thresh && sensor[4] > sens_thresh && sensor[2] < sens_thresh ) {
-            wait(0.1);   // maybe replace or change somehow
+            wait(0.1);        // maybe change or replace w something better
             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;
+        }
+        
     } else if (sensor[1] < sens_thresh && sensor[2] < sens_thresh && sensor[3] < sens_thresh) {
         return 'B';
     }
@@ -171,6 +192,8 @@
         return 'G';
     } else if (left) {
         return 'L';
+    } else if (straight) {
+        return 'S';
     } else if (right) {
         return 'R';
     } else {
@@ -179,7 +202,7 @@
 }
 
 
-void turn_selector( char turn )
+void turn_select( char turn )
 {
     switch(turn) {
         case 'G':
@@ -204,7 +227,7 @@
 
     while (sensor[0] > 500) { robot.scan(); }
     
-    robot.spin_left(turn_speed);
+    robot.spin_left(0.2);
     wait(0.1);
     
     while (sensor[1] < 500) { robot.scan(); }
@@ -218,7 +241,7 @@
 
     while (sensor[4] > 500) { robot.scan(); }
     
-    robot.spin_right(turn_speed);
+    robot.spin_right(0.2);
     wait(0.1);
     
     while (sensor[3] < 500) { robot.scan(); }
@@ -231,7 +254,7 @@
     leds = 0b1111;
     robot.reverse(speed);
     wait(0.1);
-    robot.spin_right(turn_speed);
+    robot.spin_right(0.2);
     
     while (sensor[3] < 500) { robot.scan(); }
     
@@ -279,7 +302,7 @@
         int pointer = 0;   //path_length - 1
         
         robot.stop();
-        while (button_enter.read() == 1) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
+        while ( button_enter.read() == 1 ) { speed = (pot_S*0.3)+0.2; }  // keep looping waiting for Enter to be pressed (can change speed)
         
         leds = 0b1001;
         wait(0.2);
@@ -293,8 +316,8 @@
         while(pointer <= path_length) {
             follow_line();
             
-            if (sensor[0] > sens_thresh || sensor[4] > sens_thresh) {  // if junction found
-                turn_selector(path[pointer]);
+            if ( junction_detect() ) {  // if junction found
+                turn_select(path[pointer]);
                 if(path[pointer] == 'S') {      // make this better
                     robot.forward(speed);
                     leds = 0b1010;
@@ -305,7 +328,7 @@
         }
         
         robot.stop();
-        while(button_enter.read() == 1) {
+        while( button_enter.read() == 1 ) {
             leds = 0b1001;
             wait(0.2);
             leds = 0b0110;