Diff: main/main.cpp

Revision:

16:96c7dc8a1119

Parent:

15:6c461501d12d

Child:

17:77b8515a9568

diff -r 6c461501d12d -r 96c7dc8a1119 main/main.cpp
--- a/main/main.cpp	Tue Feb 25 15:53:53 2020 +0000
+++ b/main/main.cpp	Tue Feb 25 16:15:58 2020 +0000
@@ -303,8 +303,8 @@
     leds = 0b0000;
     
     robot.lcd_clear();
-    robot.lcd_print(path,100);
-    //robot.lcd_print(inv_path,100);
+    //robot.lcd_print(path,100);
+    robot.lcd_print(inv_path,100);
     
     while(1) {
         int pointer = 0;
@@ -318,10 +318,9 @@
         
         robot.reverse(speed);
         while(sensor[0] > sens_thresh || sensor[4] > sens_thresh) { robot.scan(); }
+        wait(0.05);
         
-        wait(0.1);
         robot.spin_right(0.2);
-        
         while(sensor[2] > sens_thresh) { robot.scan(); }
         while(sensor[3] < sens_thresh) { robot.scan(); } 
         while(sensor[3] > sens_thresh) { robot.scan(); } 
@@ -332,6 +331,7 @@
             follow_line();
             
             if ( junction_detect() ) {  // if junction found
+                char na = junction_logic();   // aids turing fluidity (char not necessary therefore could clean up a bit) 
                 turn_select(inv_path[pointer]);
                 if(inv_path[pointer] == 'S') {      // make this better
                     robot.forward(speed);
@@ -362,6 +362,7 @@
             follow_line();
             
             if ( junction_detect() ) {  // if junction found
+                char na = junction_logic();   // aids turing fluidity (char not necessary therefore could clean up a bit) 
                 turn_select(path[pointer]);
                 if(path[pointer] == 'S') {      // make this better
                     robot.forward(speed);
@@ -377,9 +378,9 @@
 void invert_path()
 {
     // only call once then can use infinitely
-    for( int i = 0; i <= path_length; i++ ){
-        inv_path[i] = path[path_length-1-i];
-        if( inv_path[i] == 'L' ) { inv_path[i] = 'R'; }
-        if( inv_path[i] == 'R' ) { inv_path[i] = 'L'; }
+    for( int i = 0; i < path_length; i++ ){
+        if ( path[path_length-1-i] == 'L' ) { inv_path[i] = 'R'; }
+        else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
+        else { inv_path[i] = path[path_length-1-i]; }
     }
 }
\ No newline at end of file