Diff: main/main.cpp

Revision:

22:02dda79d50b4

Parent:

21:54ea75f7984f

Child:

23:71e84953b3f3

--- a/main/main.cpp	Fri Apr 03 19:55:09 2020 +0000
+++ b/main/main.cpp	Wed Apr 08 06:03:58 2020 +0000
@@ -33,9 +33,9 @@
     bool loop_check;
     
     robot.lcd_goto_xy(0,0);
-    robot.lcd_print("A=simple", 10);
+    robot.lcd_print("A=SIMPLE", 10);
     robot.lcd_goto_xy(0,1);
-    robot.lcd_print("B=looped", 10);
+    robot.lcd_print("B=LOOPED", 10);
     
     while(button_A.read() == 1 && button_B.read() == 1) {}
     
@@ -169,7 +169,8 @@
     follow_line();
     
     if ( junction_detect() ) {
-                
+        index++;   // index to path position basically path_length
+        
         int dist_est = t_coord.read();
         t_coord.stop();
         t_coord.reset();
@@ -182,26 +183,44 @@
         if (dir == 'W'){ curr_coords[0] -= dist_est; }
         
         // 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
-        point[total_points] = total_points+1; // numbered 1 -> x
-        
-        coords_x[total_points] = curr_coords[0];
-        coords_y[total_points] = curr_coords[1];
+        if (coord_check()) {  // coord_check returns true if curr_coords coordinates are not present in (should set a variable as true)
+            total_points++;
+            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
+            // update type and explored aswell might need to be done in node logic
+        }
         
-        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
         
-        // always try left first if there is a left. if node has already been explored once, then dont go left, go straight, if no straight then right
+        check_explored(); // iterates through all existing points, if all explored match type, then mapping is complete
         
-        total_points += 1;
         // assign new node with new coordinates
         t_restart = true; //restart the timer
             
     }
     
-    // simplify();
+//    robot.lcd_clear();
+//    char xcors[100];
+//    char ycors[100];
+//    
+//    for( unsigned 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[1];
+//        ycors[a] = y[];
+//    }
+//    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();
-    robot.lcd_print(path,100);
-
     //robot.display_data();
 }