Diff: main/main.cpp

Revision:

51:3148875bb9e5

Parent:

50:fa124ea1df9f

Child:

52:79f21d15ede1

diff -r fa124ea1df9f -r 3148875bb9e5 main/main.cpp
--- a/main/main.cpp	Fri Apr 17 16:07:23 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 16:50:10 2020 +0000
@@ -478,12 +478,12 @@
         // if it does, check the number of nodes between before and after
         // which ever is shorter, set the 0 of those nodes
         
-        //int short_length = dead_end_removal(point[d_node], point[curr_index]);
+//        int short_length = dead_end_removal(point[d_node], point[curr_node]);
 //        char dir_diff;
 //        
-//        for(int j = 0; j < short_length; j++) {
+//        for(int j = short_length; j > 0; j--) {
 //            int curr_node = path_to_point_index(shortest[j]);
-//            int next_node = path_to_point_index(shortest[j+1]);
+//            int next_node = path_to_point_index(shortest[j-1]);
 //            if(coords_x[next_node] != coords_x[curr_node]) {
 //                if(coords_x[next_node] - coords_x[curr_node] > 0){
 //                    dir_diff = 'E';
@@ -512,8 +512,6 @@
 //            }
 //            if(point[next_node] == point[d_node]) { break; }
 //        }
-        
-        
 
         int curr_node = curr_index;
         bool before = false;
@@ -522,7 +520,6 @@
         int after_index;
         
         bool desired_discovered = false;
-        
         for(int k = 0; k <= path_length; k++) {
             if(looped_path[k] == point[d_node]) { desired_discovered = true; }  // maybe set a variable
             if(desired_discovered == false && looped_path[k] == point[curr_node]) { before = true; before_index = k; }
@@ -539,6 +536,7 @@
             after = true;
         }
         
+        
         char dir_diff;
         
         if(after == true) {
@@ -614,12 +612,14 @@
     }
 }
 
-void dead_end_removal( int point1, int point2 )   // change into dead_end_removal and have it take two indexes and a path and return an array
+int dead_end_removal( int point1, int point2 )   // change into dead_end_removal and have it take two indexes and a path and return an array
 {   
     // dead end removal between start and final node
     // add the inverse of the result to end of the looped_path
     // then separate into before and after, simplify both and compare
     
+    robot.stop();
+    
     int index1, index2;
     int d_index, before_index, after_index;
     bool before = false;
@@ -632,11 +632,13 @@
         if(desired_discovered == true && looped_path[k] == point[point2]) { after_index = k; break; }
     }
     
-    if( before_index < after_index ){
-        before = true;
-    }
-    else{
-        before = false;
+    if(before){
+        if( (d_index - before_index) <= (after_index - d_index) ){
+            before = true;
+        }
+        else{
+            before = false;
+        }
     }
     
     if(before) {
@@ -707,7 +709,7 @@
         wait(2);
         robot.lcd_clear();
     }
-    
+    return length;
     // print these to check
     // add inverted sub path to the real path and increment the path length by real_len -1
 }
@@ -987,17 +989,17 @@
     // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
     // loop between start and goal
     robot.stop();
-    dead_end_removal( point[0], point[total_points] );
+    int na = dead_end_removal( point[0], point[total_points] );
     leds = 0b1001;
         wait(0.2);
         leds = 0b0110;
         wait(0.2);
-    dead_end_removal( point[3], point[4] );
+    na = dead_end_removal( point[3], point[2] );
     leds = 0b1001;
         wait(0.2);
         leds = 0b0110;
         wait(0.2);
-    dead_end_removal( point[2], point[4] );
+    na = dead_end_removal( point[2], point[3] );
     while( button_enter.read() == 1 ) {
         leds = 0b1001;
         wait(0.2);