Diff: main/main.cpp

Revision:

26:582560881379

Parent:

25:7523239a2fc1

Child:

27:0a3f028f9365

--- a/main/main.cpp	Thu Apr 09 02:52:18 2020 +0000
+++ b/main/main.cpp	Thu Apr 09 12:16:42 2020 +0000
@@ -169,32 +169,32 @@
         point[total_points] = total_points;
         coords_x[total_points] = curr_coords[0];
         coords_y[total_points] = curr_coords[1];
-        type[total_points] = 0b1000;     // start is always 1 exit type in the north direction
-        explored[total_points] = 0b1000; // not sure if i set this as explored already or whether i do that at the next junction
-        looped_path[total_points] = 0; // start node is '0'
+        type[total_points] = 0b1000;              // start is always 1 exit type in the north direction
+        explored[total_points] = 0b1000;          // not sure if i set this as explored already or whether i do that at the next junction
+        looped_path[total_points] = 0;            // start node is '0'
     }
     
       // follow line until reaching a junction, determine its type and coordinates
-    if ( t_restart ){                // only start the timer if it isnt already started
-        t_coord.start();
-        t_restart = false;
-    }
+//    if ( t_restart ){                // only start the timer if it isnt already started
+//        t_coord.start();
+//        t_restart = false;
+//    }
     
     follow_line();
     
     if ( junction_detect() ) {
-        path_length++;   // increment the path position index
+        path_length++;                                  // increment the path position index
         
-        float time = t_coord.read();
-        int dist_est = floor(time);
-        t_coord.stop();
-        t_coord.reset();
-        t_restart = true; //restart the timer next loop
+//        float time = t_coord.read();
+//        int dist_est = floor(time);
+//        t_coord.stop();
+//        t_coord.reset();
+//        t_restart = true; //restart the timer next loop
         
-        if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
-        if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
-        if (dir == 'S'){ curr_coords[1] -= dist_est; }
-        if (dir == 'W'){ curr_coords[0] -= dist_est; }
+        if (dir == 'N'){ curr_coords[1] += 1; }  // y coord
+        if (dir == 'E'){ curr_coords[0] += 1; }  // x coord
+        if (dir == 'S'){ curr_coords[1] -= 1; }
+        if (dir == 'W'){ curr_coords[0] -= 1; }
         
         // 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 (should set a variable as true)
@@ -215,6 +215,9 @@
         // i.e. the function will take the node ID integer as an argument and go backwards through the path until reaching that node (appending each node along the way to the end of the path)
         
         
+        
+        // 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();
@@ -303,7 +306,8 @@
         }
     }
     
-    // robot.stop(); ?
+    robot.stop(); 
+    wait(0.5);
     
     int angle = 0;
     
@@ -350,7 +354,7 @@
     if ( east  ) { _type |= 0b0010; }
     if ( south ) { _type |= 0b0001; }
     
-    type[total_points] = _type; 
+    type[total_points] = _type;      // maybe update_index and use curr_index instead of total_points
     explored[total_points] = _explored;
     
     // figure out a way to check what directions correlate to which turns based on direction currently facing.
@@ -367,39 +371,43 @@
     
     int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
     
-    int curr_angle = 0;
-    if ( dir == 'E' ) { curr_angle = 90;}
-    else if ( dir == 'S' ) { curr_angle = 180;}
-    else if ( dir == 'W' ) { curr_angle = 270;}
+    if( unexp_paths == 0b0000 ) { set a variable that will allow the main loop to run a function that returns to the last node in the path with unexplored paths ; }
     
-    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 {
+        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;
+        }
+         
+        unsigned int turn_angle = (curr_angle + desired_angle) % 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'); } 
     }
-    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;
-    }
-     
-    unsigned int turn_angle = (curr_angle + desired_angle) % 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 follow_line()