Diff: main/main.cpp

Revision:

30:d62f122e8d60

Parent:

29:ecf497c3fdc0

Child:

31:1e6d0ef05996

--- a/main/main.cpp	Sat Apr 11 22:56:31 2020 +0000
+++ b/main/main.cpp	Sun Apr 12 12:49:56 2020 +0000
@@ -200,8 +200,11 @@
         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
-            if(goal_node) { point[total_points] = 100; goal_node = false; }  // 100 will be the indicator for the goal node that we can visit once mapping is complete
-            else { point[total_points] = total_points; } // numbered 0 -> total_points
+            
+//            if(goal_node) { point[total_points] = 100; goal_node = false; }  // 100 will be the indicator for the goal node that we can visit once mapping is complete
+//            else { point[total_points] = total_points; } // numbered 0 -> total_points
+            point[total_points] = total_points;
+            
             coords_x[total_points] = curr_coords[0];
             coords_y[total_points] = curr_coords[1];
             
@@ -214,10 +217,11 @@
         
         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 
+            // 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();
         }
         
@@ -231,23 +235,23 @@
         
     }
     
-    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[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 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.display_data();
 }
@@ -304,7 +308,7 @@
     bool left = false;
     bool straight = false;
     bool right = false;
-    bool goal = false;
+    //bool goal = false;
     
     int _type = 0b0000;
     int _explored = 0b0000;
@@ -321,7 +325,7 @@
             wait(0.05);        // maybe change or replace w something better
             robot.scan();
             if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
-                goal = true;
+                //goal = true;
             }
         }
         
@@ -333,25 +337,28 @@
     }
     
     robot.stop(); 
-    wait(0.05);
+    wait(1);
     
-    if(goal) {
-        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
-        else if ( dir == 'E' ) { west = true;  _explored |= 0b1000; }
-        else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
-        else if ( dir == 'W' ) { east = true;  _explored |= 0b0010; }
-        
-        if ( west  ) { _type |= 0b1000; }
-        if ( north ) { _type |= 0b0100; }
-        if ( east  ) { _type |= 0b0010; }
-        if ( south ) { _type |= 0b0001; }
-        
-        goal_node = true;
-        
-        // set a variable so that a different value is set in the point array
-    }
+//    if(goal) {
+//        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
+//        else if ( dir == 'E' ) { west = true;  _explored |= 0b1000; }
+//        else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
+//        else if ( dir == 'W' ) { east = true;  _explored |= 0b0010; }
+//        
+//        if ( west  ) { _type |= 0b1000; }
+//        if ( north ) { _type |= 0b0100; }
+//        if ( east  ) { _type |= 0b0010; }
+//        if ( south ) { _type |= 0b0001; }
+//        
+//        goal_node = true;
+//        
+//        robot.reverse(speed);
+//        wait(0.1);
+//        
+//        // set a variable so that a different value is set in the point array
+//    }
     
-    else {
+//    else {
         int angle = 0;
         int reset_ang = 0;
         
@@ -395,7 +402,7 @@
         if ( north ) { _type |= 0b0100; }
         if ( east  ) { _type |= 0b0010; }
         if ( south ) { _type |= 0b0001; }
-    }   
+//    }   
      
     type[total_points] = _type;      // maybe update_index and use curr_index instead of total_points
     explored[total_points] = _explored;
@@ -411,7 +418,12 @@
     
     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 ) { 
+        retrace = true; 
+        while(1) {
+            robot.stop();
+        }
+    }
     
     else {
         retrace = false;
@@ -443,7 +455,7 @@
             explored[curr_index] |= 0b0001;
         }
          
-        int turn_angle = (curr_angle - desired_angle + 360) % 360;
+        int turn_angle = (desired_angle - curr_angle + 360) % 360;
         
         if( turn_angle == 0)  { turn_select('S'); } 
         else if( turn_angle == 90)  { turn_select('R'); } 
@@ -512,8 +524,8 @@
             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.3); 
+//                robot.reverse(speed); 
+//                wait(0.1); 
                 turn_select('B');
             }
             else if( curr_angle == 270 ) { turn_select('L'); }