Diff: main/main.cpp

Revision:

28:63ff8290964a

Parent:

27:0a3f028f9365

Child:

29:ecf497c3fdc0

--- a/main/main.cpp	Fri Apr 10 14:00:50 2020 +0000
+++ b/main/main.cpp	Fri Apr 10 15:14:51 2020 +0000
@@ -213,6 +213,7 @@
         choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
         
         if (retrace) {
+            // maybe do backtrack before choose turn?
             back_track();  // if no node exists with unexplored paths, set complete to true true, else return to a previous node by going back through the path, increment the path length and add nodes appropriately and return false 
         }
         
@@ -239,8 +240,8 @@
         char y[1];
         sprintf(x,"%d",coords_x[a]);
         sprintf(y,"%d",coords_y[a]);
-        xcors[a] = x[1];
-        ycors[a] = y[];
+        xcors[a] = x[0];
+        ycors[a] = y[0];
     }
     robot.lcd_goto_xy(0,0);
     robot.lcd_print(xcors,100);
@@ -320,7 +321,7 @@
     }
     
     robot.stop(); 
-    wait(0.5);
+    wait(0.05);
     
     if(goal) {
         if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
@@ -340,10 +341,11 @@
     
     else {
         int angle = 0;
+        int reset_ang = 0;
         
-        if (dir == 'E') { angle = 90;}
-        else if (dir == 'S') { angle = 180;}
-        else if (dir == 'W') { angle = 270;}
+        if (dir == 'E') { angle = 90; reset_ang = 90; }
+        else if (dir == 'S') { angle = 180; reset_ang = 180; }
+        else if (dir == 'W') { angle = 270; reset_ang = 270; }
         
         if (left) { 
             angle += 270;
@@ -352,8 +354,7 @@
             if (angle == 180) { south = true; }
             if (angle == 90) { east = true; }
             if (angle == 270) { west = true; }
-            angle -= 270;
-            angle = angle % 360;
+            angle = reset_ang;
         }
         
         if (right) { 
@@ -363,8 +364,7 @@
             if (angle == 180) { south = true; }
             if (angle == 90) { east = true; }
             if (angle == 270) { west = true; }
-            angle -= 90;
-            angle = angle % 360;
+            angle = reset_ang;
         }
           
         if (straight) { 
@@ -397,8 +397,6 @@
     // sets the explored of the current node to 1 in whatever path is chosen
     // also update dir
     
-    update_index();
-    
     int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
     
     if( unexp_paths == 0b0000 ) { retrace = true; }
@@ -442,6 +440,11 @@
     }
 }
 
+void back_track()
+{
+    //find the colsest previous node and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
+}
+
 void follow_line() 
 {
     robot.scan();
@@ -715,4 +718,4 @@
         else if ( path[path_length-1-i] == 'R' ) { inv_path[i] = 'L'; }
         else { inv_path[i] = path[path_length-1-i]; }
     }
-}
\ No newline at end of file
+}