Diff: main/main.cpp

Revision:

23:71e84953b3f3

Parent:

22:02dda79d50b4

Child:

24:adb946be4ce5

--- a/main/main.cpp	Wed Apr 08 06:03:58 2020 +0000
+++ b/main/main.cpp	Wed Apr 08 21:57:10 2020 +0000
@@ -148,7 +148,6 @@
         path[path_length] = turn;
         path_length ++;
     }
-
     
     simplify();
     
@@ -158,8 +157,23 @@
     //robot.display_data();
 }
 
-void looped()
+void looped()  
 {
+    if(first) {   // init the start node
+        first = false;
+        curr_coords[0] = 0;
+        curr_coords[1] = 0;
+        dir = 'N';
+        total_points = 0;
+        point[total_points] = total_points;
+        coords_x[total_points] = curr_coords[0];
+        coords_y[total_points] = curr_coords[1];
+        type[total_points] = 0b1000;
+        explored[total_points = 0b0000; // 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'
+        //total_points++;
+    }
+    
       // 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();
@@ -169,13 +183,13 @@
     follow_line();
     
     if ( junction_detect() ) {
-        index++;   // index to path position basically path_length
+        path_length++;   // index to path position
         
-        int dist_est = t_coord.read();
+        float time = t_coord.read();
+        int dist_est = round(time);
         t_coord.stop();
         t_coord.reset();
-        
-        //auto dist_est = time - (time%1); // round the value (nearest second)
+        t_restart = true; //restart the timer
         
         if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
         if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
@@ -188,18 +202,19 @@
             point[total_points] = total_points; // numbered 1 -> x
             coords_x[total_points] = curr_coords[0];
             coords_y[total_points] = curr_coords[1];
-            node_logic(); // determines what junction type we are at updates the explored and type arrays accordingly
+            node_logic(); // determines what junction type we are at updates the explored (path entered on) and type arrays accordingly
             // update type and explored aswell might need to be done in node logic
         }
         
         // use current coords to find which point to place in path
-        looped_path[index] = coord_to_node(); //returns an int of which point we are at
-        do_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn
+        looped_path[path_length] = coord_to_node(); //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
         
         check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
+        // if not, make a func that traverses back through the bath until reaching that node, then explore the unexplored path
         
         // assign new node with new coordinates
-        t_restart = true; //restart the timer
+        
             
     }
     
@@ -224,6 +239,32 @@
     //robot.display_data();
 }
 
+bool coord_check()
+{
+    bool result = true;
+    //returns true if the current coords dont match a previous point
+    for(int i = 0; i <= total_points; i++) {
+        if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) { 
+            result = false; 
+        }
+    }
+    
+    return result;
+}
+
+int coord_to_node() 
+{
+    int result;
+    // checks the curr_coords againts the coords array, returns the index to relate to the point array
+    for(int i = 0; i <= total_points; i++) {
+        if(curr_coords[0] == coords_x[i] && curr_coords[1] == coords_y[i]) { 
+            result = i; 
+        }
+    }
+    
+    return point[i];
+}
+
 void node_logic()
 {
     bool north = false;