Diff: main/main.cpp

Revision:

55:18ea9529afb2

Parent:

54:5ff74beb043e

Child:

56:1cd434b28322

--- a/main/main.cpp	Fri Apr 17 23:46:53 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 23:52:23 2020 +0000
@@ -480,22 +480,22 @@
         // if it does, check the number of nodes between before and after
         // which ever is shorter, set the 0 of those nodes
         
-        robot.stop();
-        char buf1[2], buf2[2];
-        sprintf(buf1,"%d",point[d_node]);
-        sprintf(buf2,"%d",point[curr_index]);
-        robot.lcd_print(buf1,2);
-        robot.lcd_goto_xy(0,1);
-        robot.lcd_print(buf2,2); 
-
-        wait(1);
-        robot.lcd_clear();
+//        robot.stop();
+//        char buf1[2], buf2[2];
+//        sprintf(buf1,"%d",point[d_node]);
+//        sprintf(buf2,"%d",point[curr_index]);
+//        robot.lcd_print(buf1,2);
+//        robot.lcd_goto_xy(0,1);
+//        robot.lcd_print(buf2,2); 
+//
+//        wait(1);
+//        robot.lcd_clear();
         
         
-        int short_length = dead_end_removal(point[d_node], point[curr_index]);
+        bool check = dead_end_removal(point[d_node], point[curr_index]);
         char dir_diff;
         
-        if(d_node == 2 && curr_index == 1) {
+        if(check) {
             for(int j = 0; j <= short_length; j++) {
                 int curr_node = path_to_point_index(shortest[j]);
                 int next_node = path_to_point_index(shortest[j+1]);
@@ -665,7 +665,7 @@
     }
 }
 
-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
+bool 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
@@ -695,6 +695,7 @@
     if(before) {
         index1 = before_index;
         index2 = d_index;
+        
     }else{
         index1 = d_index;
         index2 = after_index;
@@ -706,14 +707,14 @@
     
     for( int i = index1; i <= index2; i++ ) { shortest[i-index1] = looped_path[i]; }
     
-    int length = index2 - index1;
+    short_length = index2 - index1;
     
 //    robot.lcd_print("l bef",10);
 //    wait(2);
 //    robot.lcd_clear();
 //    
 //    char buffer[3];
-//    sprintf(buffer,"%d", length);
+//    sprintf(buffer,"%d", short_length);
 //    robot.lcd_print(buffer,3);
 //    wait(2);
 //    robot.lcd_clear();
@@ -725,9 +726,9 @@
     // or end when iterated through the entire array
     
     int i = 0;
-    while( i <= length ) {
+    while( i <= short_length ) {
         int count = i;
-        for( int j = 0; j <= length; j++ ) {
+        for( int j = 0; j <= short_length; j++ ) {
             if( shortest[i] == shortest[j] ){
                 count = j;
             }
@@ -743,42 +744,43 @@
                 ind++;
             }
             // clear the array
-            for( int x = 0; x <= length; x++ ) { shortest[x] = NULL; }
+            for( int x = 0; x <= short_length; x++ ) { shortest[x] = NULL; }
             
-            length -= (count-i);
+            short_length -= (count-i);
             i = -1;
             
-            for( int x = 0; x <= length; x++ ) { shortest[x] = temp_array[x]; }
+            for( int x = 0; x <= short_length; x++ ) { shortest[x] = temp_array[x]; }
             
-            for( int x = 0; x <= length; x++ ) { temp_array[x] = NULL; }
+            for( int x = 0; x <= short_length; x++ ) { temp_array[x] = NULL; }
         }
         i++;
     }
     
-    robot.lcd_print("l af",10);
-    wait(1);
-    robot.lcd_clear();
-    
-    char buffe[3];
-    sprintf(buffe,"%d", length);
-    robot.lcd_print(buffe,3);
-    wait(1);
-    robot.lcd_clear();
+//    robot.lcd_print("l af",10);
+//    wait(1);
+//    robot.lcd_clear();
+//    
+//    char buffe[3];
+//    sprintf(buffe,"%d", short_length);
+//    robot.lcd_print(buffe,3);
+//    wait(1);
+//    robot.lcd_clear();
+//    
+//    robot.lcd_print("path",10);
+//    wait(1);
+//    robot.lcd_clear();
+//    
+//    leds = 0b1111;
+//    for(int m = 0; m <= short_length; m++) {
+//        leds = ~leds;
+//        char buffer[3];
+//        sprintf(buffer,"%d", shortest[m]);
+//        robot.lcd_print(buffer,3);
+//        wait(1);
+//        robot.lcd_clear();
+//    }
     
-    robot.lcd_print("path",10);
-    wait(1);
-    robot.lcd_clear();
-    
-    leds = 0b1111;
-    for(int m = 0; m <= length; m++) {
-        leds = ~leds;
-        char buffer[3];
-        sprintf(buffer,"%d", shortest[m]);
-        robot.lcd_print(buffer,3);
-        wait(1);
-        robot.lcd_clear();
-    }
-    return length;
+    return before;
     // print these to check
     // add inverted sub path to the real path and increment the path length by real_len -1
 }
@@ -1058,7 +1060,7 @@
     // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
     // loop between start and goal
     robot.stop();
-    int na = dead_end_removal( point[0], point[total_points] );
+    bool na = dead_end_removal( point[0], point[total_points] );
     leds = 0b1001;
         wait(0.2);
         leds = 0b0110;