Diff: main/main.cpp

Revision:

44:73a83a0fa467

Parent:

43:ec047ba15db1

Child:

45:4fa8af658523

--- a/main/main.cpp	Wed Apr 15 17:37:23 2020 +0000
+++ b/main/main.cpp	Thu Apr 16 05:31:11 2020 +0000
@@ -209,7 +209,7 @@
         }
         
         update_index();
-
+        
         // use current coords to find which point to place in path
     
         looped_path[path_length] = point[curr_index]; //returns an int of which point we are at what its called
@@ -219,8 +219,6 @@
         // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
         // i.e. the function will take the node ID integer as an argument and go backwards through the path until reaching that node (appending each node along the way to the end of the path)
         
-        
-        
         // needs a function that checks if current node has any paths left to explore, if not, then it must return via the path to a node that isnt fully explored and continue from there
         
     }
@@ -394,10 +392,8 @@
 //    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) {
-        back_track();
-        unexp_paths = type[curr_index] & ~( explored[curr_index] );
-    }
+    if ( unexp_paths == 0b0000 ) { back_track(); }
+    unexp_paths = type[curr_index] & ~( explored[curr_index] );
 
     int curr_angle = 0;
     if ( dir == 'E' ) { curr_angle = 90;}
@@ -460,7 +456,7 @@
     // find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
     // also if no nodes have unexplored paths set complete to true
     
-    bool complete = false;
+    complete = false;
     int pointer;   // an index to the most recent node with unexplored paths
     
     for(int i = total_points; i >= 0; i--) {    // start from the most recently discovered node
@@ -504,20 +500,16 @@
             }
             
             if( dir_diff == 'N' ) { 
-                explored[curr_node] &= 0b1011; 
-                //explored[prev_node] &= 0b1110; 
+                explored[curr_node] &= 0b1011;
             }
             else if( dir_diff == 'E' ) { 
                 explored[curr_node] &= 0b1101; 
-                //explored[prev_node] &= 0b0111; 
             }
             else if( dir_diff == 'S' ) { 
-                explored[curr_node] &= 0b1110; 
-                //explored[prev_node] &= 0b1011;
+                explored[curr_node] &= 0b1110;
             }
             else if( dir_diff == 'W' ) { 
-                explored[curr_node] &= 0b0111;  
-                //explored[prev_node] &= 0b1101;
+                explored[curr_node] &= 0b0111;
             }
 //            print_data("after bt");
             if(point[prev_node] == point[pointer]) { break; }
@@ -525,7 +517,87 @@
     }
 }
 
-int node_repeat_check(int curr_node, int desired_node)  // will skip deadends in bt
+void return_to_start()
+{
+    
+    // for bt, check the path between the node before the desired node and after
+    // which ever is shorter, do the 0 on that path
+    
+    // for return to start just do it from the start 
+    int pointer;
+    char dir_diff;
+    
+    for( int i = 0; i <= path_length; i++ ) {
+        if( looped_path[i] == point[curr_index] ) {
+            pointer = i; 
+            break;
+        }
+    }
+    
+    for(int j = pointer; j >= 0; j--) {
+        int curr_node = path_to_point_index(looped_path[j]);
+        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){
+                dir_diff = 'E';
+            } else {
+                dir_diff = 'W';
+            }
+        } else if( coords_y[prev_node] != coords_y[curr_node] ) {
+            if(coords_y[prev_node] - coords_y[curr_node] > 0){
+                dir_diff = 'N';
+            } else {
+                dir_diff = 'S';
+            }
+        }
+        
+        if( dir_diff == 'N' ) { 
+            explored[curr_node] &= 0b1011;
+        }
+        else if( dir_diff == 'E' ) { 
+            explored[curr_node] &= 0b1101; 
+        }
+        else if( dir_diff == 'S' ) { 
+            explored[curr_node] &= 0b1110;
+        }
+        else if( dir_diff == 'W' ) { 
+            explored[curr_node] &= 0b0111;
+        }
+    }
+    
+    while( point[curr_index] != 0 ) {
+        
+        if ( t_restart ){                // only start the timer if it isnt already started
+            t_coord.start();
+            t_restart = false;
+        }
+        
+        follow_line();
+        
+        if ( junction_detect() ) {
+            path_length++;                                  // increment the path position index
+            
+            float time = t_coord.read();
+            int dist_est = ceil(time*2); 
+            t_coord.stop();
+            t_coord.reset();
+            t_restart = true; 
+            
+            if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
+            if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
+            if (dir == 'S'){ curr_coords[1] -= dist_est; }
+            if (dir == 'W'){ curr_coords[0] -= dist_est; }
+            
+            update_index();
+            looped_path[path_length] = point[curr_index]; 
+            choose_turn();  
+        }
+        
+        wait(1/50);
+    }
+}
+
+int node_repeat_check(int curr_node, int desired_node)  // will skip deadends in bt (COULD ALSO CHECK NODES EARLIER THAN DESIRED AS IT MIGHT BE A QUICKER ROUTE)
 {
     int repeats = 0;
     int index[10];   // array of all the indexes that contain the curr_node
@@ -825,9 +897,14 @@
 
 void looped_goal() 
 {
+    // return to start (adding nodes to path)
+    // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
+    // loop between start and goal
     robot.stop();
+    return_to_start();
+    simplify();
     
-    while(1) {
+    while( button_enter.read() == 1 ) {
         leds = 0b1001;
         wait(0.2);
         leds = 0b0110;