Diff: main/main.cpp

Revision:

45:4fa8af658523

Parent:

44:73a83a0fa467

Child:

46:c770365cf5a6

--- a/main/main.cpp	Thu Apr 16 05:31:11 2020 +0000
+++ b/main/main.cpp	Thu Apr 16 06:12:07 2020 +0000
@@ -476,43 +476,111 @@
             // decrement the count (setting the previous as current node and the next previous as previous)
             // when previous node == point[pointer1] break
             // choose turn should then do all those turns and arrive at correct node
+        
+        // check if the current node exists before the discovery of the desired node
+        // if it does, check the number of nodes between before and after
+        // which ever is shorter, set the 0 of those nodes
+        
+        int curr_node = curr_index;
+        bool before = false;
+        bool after = false;
+        int before_index;
+        int after_index;
+        
+        bool desired_discovered = false;
+        for(int k = 0; k <= path_length; k++) {
+            if(looped_path[k] == point[pointer] && before == false) { after = true; break; }  // maybe set a variable
+            if(looped_path[k] == point[pointer]) { desired_discovered = true; }  // maybe set a variable
+            if(desired_discovered == false && looped_path[k] == point[curr_node]) { before = true; before_index = k; }
+            if(desired_discovered == true && looped_path[k] == point[curr_node]) { after_index = k; break; }
+        }
+        
+        if (after == false) { 
+            if( before_index < after_index ){
+                before = true;
+                after = false;
+            }
+            else{
+                before = false;
+                after = true;
+            }
+        }
+        
         char dir_diff;
-          
-        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 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';
+        
+        if(after == true) {
+             
+            for(int j = path_length; j >= 0; j--) {
+                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 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';
+                    }
                 }
-            } 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; 
                 }
-            }
-            
-            if( dir_diff == 'N' ) { 
-                explored[curr_node] &= 0b1011;
+                else if( dir_diff == 'S' ) { 
+                    explored[curr_node] &= 0b1110;
+                }
+                else if( dir_diff == 'W' ) { 
+                    explored[curr_node] &= 0b0111;
+                }
+    //            print_data("after bt");
+                if(point[prev_node] == point[pointer]) { break; }
             }
-            else if( dir_diff == 'E' ) { 
-                explored[curr_node] &= 0b1101; 
-            }
-            else if( dir_diff == 'S' ) { 
-                explored[curr_node] &= 0b1110;
+        }
+        
+        else if( before == true ) {
+            for(int j = before_index; j <= path_length; j++) {
+                curr_node = path_to_point_index(looped_path[j]);
+                
+                int next_node = path_to_point_index(looped_path[j+1]);
+                if(coords_x[next_node] != coords_x[curr_node]) {
+                    if(coords_x[next_node] - coords_x[curr_node] > 0){
+                        dir_diff = 'E';
+                    } else {
+                        dir_diff = 'W';
+                    }
+                } else if( coords_y[next_node] != coords_y[curr_node] ) {
+                    if(coords_y[next_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;
+                }
+                if(point[next_node] == point[pointer]) { break; }
             }
-            else if( dir_diff == 'W' ) { 
-                explored[curr_node] &= 0b0111;
-            }
-//            print_data("after bt");
-            if(point[prev_node] == point[pointer]) { break; }
         }
     }
 }
@@ -901,8 +969,8 @@
     // 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();
+    //return_to_start();
+//    simplify_looped();
     
     while( button_enter.read() == 1 ) {
         leds = 0b1001;