Diff: main/main.cpp

Revision:

27:0a3f028f9365

Parent:

26:582560881379

Child:

28:63ff8290964a

diff -r 582560881379 -r 0a3f028f9365 main/main.cpp
--- a/main/main.cpp	Thu Apr 09 12:16:42 2020 +0000
+++ b/main/main.cpp	Fri Apr 10 14:00:50 2020 +0000
@@ -1,4 +1,4 @@
-#include "main.h"
+ #include "main.h"
 #include <math.h> 
 
 // API
@@ -197,12 +197,14 @@
         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)
+        if (coord_check()) {  // coord_check returns true if curr_coords coordinates are not present in coords_x and y
             total_points++;
-            point[total_points] = total_points; // numbered 0 -> 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
             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 (path entered on) and type arrays accordingly
+            
         }
         
         // use current coords to find which point to place in path
@@ -210,6 +212,13 @@
         looped_path[path_length] = point[curr_index]; //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
         
+        if (retrace) {
+            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) {
+            goal();
+        }
         // 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
         // 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)
@@ -220,23 +229,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[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();
+    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.display_data();
 }
@@ -282,6 +291,7 @@
     bool left = false;
     bool straight = false;
     bool right = false;
+    bool goal = false;
     
     int _type = 0b0000;
     int _explored = 0b0000;
@@ -297,6 +307,9 @@
         if ( sensor[0] > SENS_THRESH && sensor[4] > SENS_THRESH && sensor[2] < SENS_THRESH ) {
             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;
+            }
         }
         
         robot.scan();
@@ -309,55 +322,72 @@
     robot.stop(); 
     wait(0.5);
     
-    int angle = 0;
-    
-    if (dir == 'E') { angle = 90;}
-    else if (dir == 'S') { angle = 180;}
-    else if (dir == 'W') { angle = 270;}
-    
-    if (left) { 
-        angle += 270;
-        angle = angle % 360;
-        if (angle == 0) { north = true; }
-        if (angle == 180) { south = true; }
-        if (angle == 90) { east = true; }
-        if (angle == 270) { west = true; }
-        angle -= 270;
-        angle = angle % 360;
+    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 (right) { 
-        angle += 90;
-        angle = angle % 360;
-        if (angle == 0) { north = true; }
-        if (angle == 180) { south = true; }
-        if (angle == 90) { east = true; }
-        if (angle == 270) { west = true; }
-        angle -= 90;
-        angle = angle % 360;
-    }
-      
-    if (straight) { 
-        if (angle == 0) { north = true; }
-        if (angle == 180) { south = true; }
-        if (angle == 90) { east = true; }
-        if (angle == 270) { west = true; }
-    }
-    
-    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; }
-    
+    else {
+        int angle = 0;
+        
+        if (dir == 'E') { angle = 90;}
+        else if (dir == 'S') { angle = 180;}
+        else if (dir == 'W') { angle = 270;}
+        
+        if (left) { 
+            angle += 270;
+            angle = angle % 360;
+            if (angle == 0) { north = true; }
+            if (angle == 180) { south = true; }
+            if (angle == 90) { east = true; }
+            if (angle == 270) { west = true; }
+            angle -= 270;
+            angle = angle % 360;
+        }
+        
+        if (right) { 
+            angle += 90;
+            angle = angle % 360;
+            if (angle == 0) { north = true; }
+            if (angle == 180) { south = true; }
+            if (angle == 90) { east = true; }
+            if (angle == 270) { west = true; }
+            angle -= 90;
+            angle = angle % 360;
+        }
+          
+        if (straight) { 
+            if (angle == 0) { north = true; }
+            if (angle == 180) { south = true; }
+            if (angle == 90) { east = true; }
+            if (angle == 270) { west = true; }
+        }
+        
+        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; }
+    }   
+     
     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.
 }
 
 void choose_turn() 
@@ -371,9 +401,11 @@
     
     int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
     
-    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 ; }
+    if( unexp_paths == 0b0000 ) { retrace = true; }
     
     else {
+        retrace = false;
+        
         int curr_angle = 0;
         if ( dir == 'E' ) { curr_angle = 90;}
         else if ( dir == 'S' ) { curr_angle = 180;}