Diff: main/main.cpp

Revision:

53:dbb63b181faf

Parent:

52:79f21d15ede1

Child:

54:5ff74beb043e

--- a/main/main.cpp	Fri Apr 17 19:07:29 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 19:44:40 2020 +0000
@@ -478,148 +478,148 @@
         // 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);
-//        wait(2);
-//        robot.lcd_clear();
-//        robot.lcd_print(buf2,2);
-//        wait(2); 
-//        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);
+        wait(2);
+        robot.lcd_clear();
+        robot.lcd_print(buf2,2);
+        wait(2); 
+        robot.lcd_clear();
+        
+        int short_length = dead_end_removal(point[d_node], point[curr_index]);
+        char dir_diff;
+        
+        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]);
+            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[d_node]) { break; }
+        }
+
+        //int curr_node = curr_index;
+//        bool before = false;
+//        bool after = false;
+//        int before_index;
+//        int after_index;
 //        
-//        int short_length = dead_end_removal(point[d_node], point[curr_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; }
+//            if(desired_discovered == true && looped_path[k] == point[curr_node]) { after_index = k; break; }
+//        }
+//        
+//        
+//        if( before_index < after_index ){
+//            before = true;
+//            after = false;
+//        }
+//        else{
+//            before = false;
+//            after = true;
+//        }
+//        
+//        
 //        char dir_diff;
 //        
-//        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]);
-//            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(after == true) {
+//             
+//            for(int j = after_index; j >= 0; j--) {
+//                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; 
+//                
+//                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[prev_node] == point[d_node]) { break; }
 //            }
-//            else if( dir_diff == 'S' ) { 
-//                explored[curr_node] &= 0b1110;
-//            }
-//            else if( dir_diff == 'W' ) { 
-//                explored[curr_node] &= 0b0111;
-//            }
-//            if(point[next_node] == point[d_node]) { break; }
 //        }
-
-        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[d_node]) { 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( before_index < after_index ){
-            before = true;
-            after = false;
-        }
-        else{
-            before = false;
-            after = true;
-        }
-        
-        
-        char dir_diff;
-        
-        if(after == true) {
-             
-            for(int j = after_index; j >= 0; j--) {
-                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;
-                }
-                if(point[prev_node] == point[d_node]) { break; }
-            }
-        }
-        
-        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[d_node]) { break; }
-            }
-        }
+//        
+//        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[d_node]) { break; }
+//            }
+//        }
     }
 }
 
@@ -666,11 +666,15 @@
     
     int length = index2 - index1;
     
-//    char buffer[3];
-//    sprintf(buffer,"%d", length);
-//    robot.lcd_print(buffer,3);
-//    wait(2);
-//    robot.lcd_clear();
+    robot.lcd_print("l bef",10);
+    wait(2);
+    robot.lcd_clear();
+    
+    char buffer[3];
+    sprintf(buffer,"%d", length);
+    robot.lcd_print(buffer,3);
+    wait(2);
+    robot.lcd_clear();
     
     // for every node in path, check if repeated
     // if rpeated, move everyting before the first occurence to the final occurence
@@ -709,15 +713,29 @@
         i++;
     }
     
-//    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(2);
-//        robot.lcd_clear();
-//    }
+    robot.lcd_print("l af",10);
+    wait(2);
+    robot.lcd_clear();
+    
+    char buffe[3];
+    sprintf(buffe,"%d", length);
+    robot.lcd_print(buffe,3);
+    wait(2);
+    robot.lcd_clear();
+    
+    robot.lcd_print("path",10);
+    wait(2);
+    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(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
@@ -869,7 +887,7 @@
 //    wait(0.1);
     robot.spin_right(TURN_SPEED);
     if(loop_check == false) {  // works better for looped
-        wait(0.7);
+        wait(0.65);
     }
     else {
         while (sensor[3] < SENS_THRESH) { robot.scan(); }