Diff: main/main.cpp

Revision:

39:005ad4610152

Parent:

38:b5b06625d06e

Child:

40:02f6e268011b

--- a/main/main.cpp	Tue Apr 14 22:33:06 2020 +0000
+++ b/main/main.cpp	Wed Apr 15 14:52:53 2020 +0000
@@ -309,7 +309,6 @@
                 //goal = true;
             }
         }
-        
         robot.scan();
         
         if ( sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH ) {
@@ -400,7 +399,7 @@
     else if ( dir == 'E' ) { explored[curr_index] |= 0b1000; }
     else if ( dir == 'S' ) { explored[curr_index] |= 0b0100; }
     else if ( dir == 'W' ) { explored[curr_index] |= 0b0010; }
-    print_data("enter junc");
+//    print_data("enter junc");
     int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
     
     if (unexp_paths == 0b0000) {
@@ -437,18 +436,18 @@
      
     int turn_angle = (desired_angle - curr_angle + 360) % 360;
     
-    robot.lcd_clear();
-    robot.lcd_print("turn" , 4);
-    wait(2);
+//    robot.lcd_clear();
+//    robot.lcd_print("turn" , 4);
+//    wait(2);
     
-    robot.lcd_clear();
-    if( turn_angle == 0)  { robot.lcd_print("S",1); } 
-    else if( turn_angle == 90)  { robot.lcd_print("R",1); } 
-    else if( turn_angle == 180)  { robot.lcd_print("B",1); } 
-    else if( turn_angle == 270)  { robot.lcd_print("L",1); } 
-    wait(2);
-    
-    print_data("After Turn");
+//    robot.lcd_clear();
+//    if( turn_angle == 0)  { robot.lcd_print("S",1); } 
+//    else if( turn_angle == 90)  { robot.lcd_print("R",1); } 
+//    else if( turn_angle == 180)  { robot.lcd_print("B",1); } 
+//    else if( turn_angle == 270)  { robot.lcd_print("L",1); } 
+//    wait(2);
+//    
+//    print_data("After Turn");
     
     if( turn_angle == 0)  { 
         robot.forward(speed);
@@ -456,7 +455,11 @@
         turn_select('S'); 
     } 
     else if( turn_angle == 90)  { turn_select('R'); } 
-    else if( turn_angle == 180)  { turn_select('B'); } 
+    else if( turn_angle == 180)  { 
+        robot.reverse(speed);
+        wait(0.1);
+        turn_select('B');
+    } 
     else if( turn_angle == 270)  { turn_select('L'); } 
 }
 
@@ -489,6 +492,19 @@
           
         for(int j = path_length; j >= 0; j--) {
             int curr_node = path_to_point_index(looped_path[j]);
+            // check if curr node is repeated, if it is skip to the earliest one thats infront of the unxplored node
+            int correct_ind = node_repeat_check(curr_node, point[pointer]);
+            while(j != correct_ind) { j--; }
+            
+            int test = path_to_point_index(looped_path[j]);
+            if( test != curr_node ) { 
+                while(1) {
+                    robot.lcd_clear();
+                    robot.lcd_print("broken",10);
+                    wait(10);
+                }
+            }
+            
             int prev_node = path_to_point_index(looped_path[j-1]);
             if(coords_x[prev_node] != coords_x[curr_node]) {
                 if(coords_x[prev_node] - coords_x[curr_node] > 0){
@@ -520,12 +536,41 @@
                 explored[curr_node] &= 0b0111;  
                 //explored[prev_node] &= 0b1101;
             }
-            print_data("after bt");
+//            print_data("after bt");
             if(point[prev_node] == point[pointer]) { break; }
         }
     }
 }
 
+int node_repeat_check(int curr_node, int desired_node)  // will skip deadends in bt
+{
+    int repeats = 0;
+    int index[10];   // array of all the indexes that contain the curr_node
+    int earliest_d;  // index of desired node
+    int result;
+    
+    for( int i = path_length; i >= 0; i-- ) {
+        if(looped_path[i] == point[curr_node]) { 
+            index[repeats] = i;
+            repeats++;
+        }
+        if(looped_path[i] == point[desired_node]) { 
+            earliest_d = i;
+        }
+    }
+    
+    // return the index of the earliest 'c' that comes after a 'd'
+    
+    for( int j = repeats - 1; j >= 0; j-- ) {
+        if( earliest_d < index[j] ) {
+            result = index[j];
+            break;
+        }
+    }
+    
+    return result;    // returns the correct path index of the node
+}
+
 void follow_line() 
 {
     robot.scan();
@@ -643,8 +688,8 @@
 
     while (sensor[0] > SENS_THRESH) { robot.scan(); }
     
-    robot.spin_left(0.2);
-    wait(0.1);
+    robot.spin_left(TURN_SPEED);
+    wait(0.2);
     
     while (sensor[1] < SENS_THRESH) { robot.scan(); }
     
@@ -658,7 +703,7 @@
     while (sensor[4] > SENS_THRESH) { robot.scan(); }
     
     robot.spin_right(TURN_SPEED);
-    wait(0.1);
+    wait(0.2);
     
     while (sensor[3] < SENS_THRESH) { robot.scan(); }
     
@@ -671,10 +716,10 @@
 //    robot.reverse(speed);
 //    wait(0.1);
     robot.spin_right(TURN_SPEED);
-    
-    while (sensor[3] < SENS_THRESH) { robot.scan(); }
-    
-    while (sensor[3] > SENS_THRESH) { robot.scan(); }
+    wait(0.6);
+//    while (sensor[3] < SENS_THRESH) { robot.scan(); }
+//    
+//    while (sensor[3] > SENS_THRESH) { robot.scan(); }
 }
 
 void simplify()