Diff: main/main.cpp

Revision:

54:5ff74beb043e

Parent:

53:dbb63b181faf

Child:

55:18ea9529afb2

--- a/main/main.cpp	Fri Apr 17 19:44:40 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 23:46:53 2020 +0000
@@ -431,7 +431,7 @@
 //    robot.lcd_clear();
 //    robot.lcd_print("turn" , 4);
 //    wait(2);
-    
+//    robot.stop();
 //    robot.lcd_clear();
 //    if( turn_angle == 0)  { robot.lcd_print("S",1); } 
 //    else if( turn_angle == 90)  { robot.lcd_print("R",1); } 
@@ -441,7 +441,9 @@
 //    
 //    print_data("After Turn");
     
-    if( turn_angle == 0)  { turn_select('S'); } 
+//    if( turn_angle != 180 ) { robot.forward(speed); wait(0.05); } // maybe remove
+    
+    if( turn_angle == 0)  { robot.forward(speed); wait(0.1); turn_select('S'); } 
     else if( turn_angle == 90)  { turn_select('R'); } 
     else if( turn_angle == 180)  { turn_select('B'); } 
     else if( turn_angle == 270)  { turn_select('L'); } 
@@ -483,46 +485,86 @@
         sprintf(buf1,"%d",point[d_node]);
         sprintf(buf2,"%d",point[curr_index]);
         robot.lcd_print(buf1,2);
-        wait(2);
+        robot.lcd_goto_xy(0,1);
+        robot.lcd_print(buf2,2); 
+
+        wait(1);
         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';
+        if(d_node == 2 && curr_index == 1) {
+            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]);
+                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;
                 }
-            } 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(point[next_node] == point[d_node]) { break; }
+            }  
+        }
+        
+        else { 
+        
+            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; }
             }
-            
-            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; }
         }
+        
+ //       print_data("aft bt");
 
         //int curr_node = curr_index;
 //        bool before = false;
@@ -666,15 +708,15 @@
     
     int length = index2 - index1;
     
-    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();
+//    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
@@ -714,17 +756,17 @@
     }
     
     robot.lcd_print("l af",10);
-    wait(2);
+    wait(1);
     robot.lcd_clear();
     
     char buffe[3];
     sprintf(buffe,"%d", length);
     robot.lcd_print(buffe,3);
-    wait(2);
+    wait(1);
     robot.lcd_clear();
     
     robot.lcd_print("path",10);
-    wait(2);
+    wait(1);
     robot.lcd_clear();
     
     leds = 0b1111;
@@ -733,7 +775,7 @@
         char buffer[3];
         sprintf(buffer,"%d", shortest[m]);
         robot.lcd_print(buffer,3);
-        wait(2);
+        wait(1);
         robot.lcd_clear();
     }
     return length;
@@ -1056,11 +1098,11 @@
     char buffer2[2];
     char buffer3[2];
     robot.lcd_clear();
-//    sprintf(buffer1,"%x",type[curr_index]);
-//    sprintf(buffer2,"%x",explored[curr_index]);
+    sprintf(buffer1,"%x",type[curr_index]);
+    sprintf(buffer2,"%x",explored[curr_index]);
     
-    sprintf(buffer1,"%d",curr_coords[0]);
-    sprintf(buffer2,"%d",curr_coords[1]);
+//    sprintf(buffer1,"%d",curr_coords[0]);
+//    sprintf(buffer2,"%d",curr_coords[1]);
     sprintf(buffer3,"%d",curr_index);
     robot.lcd_print(buffer1,2); 
     robot.lcd_goto_xy(0,1);