Diff: main/main.cpp

Revision:

32:598bedb22c7c

Parent:

31:1e6d0ef05996

Child:

33:9fa9e09f2e8f

--- a/main/main.cpp	Sun Apr 12 18:54:48 2020 +0000
+++ b/main/main.cpp	Sun Apr 12 23:39:45 2020 +0000
@@ -1,4 +1,4 @@
- #include "main.h"
+#include "main.h"
 #include <math.h> 
 
 // API
@@ -198,6 +198,7 @@
         
         // check that the coordinates are not already in the list, if not add the point, if it is already return the point number and increment the explored
         if (coord_check()) {  // coord_check returns true if curr_coords coordinates are not present in coords_x and y
+            
             total_points++;
             node_logic(); // determines what junction type we are at updates the explored (path entered on) and type arrays accordingly
             
@@ -207,27 +208,32 @@
             
             coords_x[total_points] = curr_coords[0];
             coords_y[total_points] = curr_coords[1];
-            update_index();
-        
-            char buffer[1];
-        
-            sprintf(buffer,"%x",type[curr_index]);
-            robot.lcd_clear();
-            robot.lcd_print(buffer,1);  
-            wait(0.5);
         }
         
+        update_index();
+        
+        char buffer1[2];
+        char buffer2[2];
+        //char buffer3[2];
+        robot.lcd_clear();
+        sprintf(buffer1,"%d",curr_coords[0]);
+        sprintf(buffer2,"%d",curr_coords[1]);
+        //sprintf(buffer3,"%d",total_points);
+        robot.lcd_print(buffer1,2); 
+        robot.lcd_goto_xy(0,1);
+        robot.lcd_print(buffer2,2); 
+        robot.lcd_goto_xy(5,0);
+        char *b = &dir;
+        robot.lcd_print(b,2); 
+        
+        robot.stop();
+        wait(2);
+
         // use current coords to find which point to place in path
-        
     
         looped_path[path_length] = point[curr_index]; //returns an int of which point we are at what its called
         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? and put the retrace check in node logic
-            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 
-        }
-        
         if (complete) {
             // once complete, have it return to start node and add those turns to path return_to_start();
             looped_goal();
@@ -242,24 +248,7 @@
         // needs a function that checks if current node has any paths left to explore, if not, then it must return via the path to a node that isnt fully explored and continue from there
         
     }
-    
-//    robot.lcd_clear();
-//    char xcors[100];
-//    char ycors[100];
-//    
-//    for( int a = 0; a <= total_points; a += 1 )
-//    {
-//        char x[1];
-//        char y[1];
-//        sprintf(x,"%d",coords_x[a]);
-//        sprintf(y,"%d",coords_y[a]);
-//        xcors[a] = x[0];
-//        ycors[a] = y[0];
-//    }
-//    robot.lcd_goto_xy(0,0);
-//    robot.lcd_print(xcors,100);
-//    robot.lcd_goto_xy(0,1);
-//    robot.lcd_print(ycors,100);
+
 
 //    robot.lcd_clear();
 //    char *b = &dir;
@@ -349,8 +338,8 @@
         }
     }
     
-    robot.stop(); 
-    wait(1);
+//    robot.stop(); 
+//    wait(1);
     
 //    if(goal) {
 //        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
@@ -431,57 +420,53 @@
     
     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; 
+    if (unexp_paths == 0b0000) {
+        back_track();
     }
+
+    int curr_angle = 0;
+    if ( dir == 'E' ) { curr_angle = 90;}
+    else if ( dir == 'S' ) { curr_angle = 180;}
+    else if ( dir == 'W' ) { curr_angle = 270;}
     
-    else {
-        retrace = false;
-        
-        int curr_angle = 0;
-        if ( dir == 'E' ) { curr_angle = 90;}
-        else if ( dir == 'S' ) { curr_angle = 180;}
-        else if ( dir == 'W' ) { curr_angle = 270;}
-        
-        int desired_angle = 0;
-        if ( (unexp_paths & 0b1000) == 0b1000) { 
-            desired_angle = 270; 
-            dir = 'W'; 
-            explored[curr_index] |= 0b1000;
-        }
-        else if ( (unexp_paths & 0b0100) == 0b0100) { 
-            desired_angle = 0;
-            dir = 'N';
-            explored[curr_index] |= 0b0100;
-        }
-        else if ( (unexp_paths & 0b0010) == 0b0010) { 
-            desired_angle = 90; 
-            dir = 'E';
-            explored[curr_index] |= 0b0010;
-        }
-        else if ( (unexp_paths & 0b0001) == 0b0001) { 
-            desired_angle = 180;
-            dir = 'S';
-            explored[curr_index] |= 0b0001;
-        }
-         
-        int turn_angle = (desired_angle - curr_angle + 360) % 360;
-        
-        if( turn_angle == 0)  { 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'); } 
+    int desired_angle = 0;
+    if ( (unexp_paths & 0b1000) == 0b1000) { 
+        desired_angle = 270; 
+        dir = 'W'; 
+        explored[curr_index] |= 0b1000;
+    }
+    else if ( (unexp_paths & 0b0100) == 0b0100) { 
+        desired_angle = 0;
+        dir = 'N';
+        explored[curr_index] |= 0b0100;
     }
+    else if ( (unexp_paths & 0b0010) == 0b0010) { 
+        desired_angle = 90; 
+        dir = 'E';
+        explored[curr_index] |= 0b0010;
+    }
+    else if ( (unexp_paths & 0b0001) == 0b0001) { 
+        desired_angle = 180;
+        dir = 'S';
+        explored[curr_index] |= 0b0001;
+    }
+     
+    int turn_angle = (desired_angle - curr_angle + 360) % 360;
+    
+    if( turn_angle == 0)  { 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'); } 
 }
 
 void back_track()
 {
-    // find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
+    //// find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
     // also if no nodes have unexplored paths set complete to true
     
     bool check = false;
-    int pointer1;
-    int pointer2;
+    int pointer1;   // an index to the most recent node with unexplored paths
+    int pointer2;   // a pointer to the previous node in that path that must be visited to reach the node before
     int new_dir;
     int count = path_length; // to be added to path length at the end
     
@@ -504,20 +489,20 @@
             // update current coords to this nodes coords
             // update the current direction, path length and path
             
-        for(int j = count - 1; j >= pointer1; j--) { // starts at the previous node to current
-             
+        while(curr_coords[0] != coords_x[pointer1] || curr_coords[1] != coords_y[pointer1]) { // starts at the previous node to current
+            count--;
             // convert looped_path[j] into an index for that nodes coords
-            pointer2 = path_to_point_index(looped_path[j]); 
+            pointer2 = path_to_point_index(looped_path[count]); 
             
             // then do coords - curr coords
             if(coords_y[pointer2] != curr_coords[1]) { 
                 // if its positive, go north, negative, go south
-                if( (coords_y[pointer2] - curr_coords[1]) > 0 ) { new_dir = 0; }
-                else { new_dir = 2; }
+                if( (coords_y[pointer2] - curr_coords[1]) > 0 ) { new_dir = 0; }  // north
+                else { new_dir = 2; } // south
             } else if(coords_x[pointer2] != curr_coords[0]) { 
                 // if its positive, go east, negative, go west
-                if( (coords_x[pointer2] - curr_coords[0]) > 0 ) { new_dir = 1; }
-                else { new_dir = 3; }
+                if( (coords_x[pointer2] - curr_coords[0]) > 0 ) { new_dir = 1; } // east
+                else { new_dir = 3; } // west
             }
             
             int curr_angle = 0;
@@ -529,26 +514,24 @@
             // east = 1
             // south = 2, etc. then i can just add 90*dir to the current angle and modulo 360 then divide by 4
             
-            curr_angle = ((new_dir*90) - curr_angle + 360) % 360;
+            int turn_ang = ((new_dir*90) - curr_angle + 360) % 360;
             
-            if( curr_angle == 0 ) { turn_select('S'); }
-            else if( curr_angle == 90 ) { turn_select('R'); }
-            else if( curr_angle == 180 ) { 
-//                robot.reverse(speed); 
-//                wait(0.1); 
+            if( turn_ang == 0 ) { turn_select('S'); }
+            else if( turn_ang == 90 ) { turn_select('R'); }
+            else if( turn_ang == 180 ) { 
+                robot.reverse(speed); 
+                wait(0.1); 
                 turn_select('B');
             }
             else if( curr_angle == 270 ) { turn_select('L'); }
             
-            bool junc = false;
-            
-            while(junc == false) {
+            while(junction_detect() == false) {
                 follow_line();
-                if( junction_detect() ){ 
-                    junc = true; 
-                }
             }
             
+            robot.stop();
+            wait(0.5);
+            
             curr_coords[0] = coords_x[pointer2];
             curr_coords[1] = coords_y[pointer2];
             
@@ -561,11 +544,8 @@
             
             path_length++;
             looped_path[path_length] = point[pointer2];
-
-        }
-        
-        choose_turn();   // if back_track is placed before choose turn this can be removed  
-    }         
+        } 
+    } 
 }
 
 void follow_line() 
@@ -710,8 +690,8 @@
 void back() 
 {
     leds = 0b1111;
-    robot.reverse(speed);
-    wait(0.1);
+//    robot.reverse(speed);
+//    wait(0.1);
     robot.spin_right(TURN_SPEED);
     
     while (sensor[3] < SENS_THRESH) { robot.scan(); }