Diff: main/main.cpp

Revision:

43:ec047ba15db1

Parent:

42:69bffd3679bf

Child:

44:73a83a0fa467

Child:

48:76cf4521d342

diff -r 69bffd3679bf -r ec047ba15db1 main/main.cpp
--- a/main/main.cpp	Wed Apr 15 15:56:25 2020 +0000
+++ b/main/main.cpp	Wed Apr 15 17:37:23 2020 +0000
@@ -51,7 +51,7 @@
     
     robot.lcd_clear();
     
-    speed = (pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
+    speed = 0.3;//(pot_S*0.3)+0.2;   // have it so max is 0.5 and min is 0.2 (this lowest doesnt work)
     
     float dt = 1/50;           // updating 50 times a second
 
@@ -201,9 +201,8 @@
             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
-            point[total_points] = 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
             
             coords_x[total_points] = curr_coords[0];
             coords_y[total_points] = curr_coords[1];
@@ -287,7 +286,7 @@
     bool left = false;
     bool straight = false;
     bool right = false;
-    //bool goal = false;
+    bool goal = false;
     
     int _type = 0b0000;
     int _explored = 0b0000;
@@ -304,7 +303,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;
             }
         }
         robot.scan();
@@ -314,26 +313,21 @@
         }
     }
     
-//    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
-//    }
+    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;
+    }
     
-//    else {
+    else {
         int angle = 0;
         int reset_ang = 0;
         
@@ -379,7 +373,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;