Diff: main/main.cpp

Revision:

25:7523239a2fc1

Parent:

24:adb946be4ce5

Child:

26:582560881379

diff -r adb946be4ce5 -r 7523239a2fc1 main/main.cpp
--- a/main/main.cpp	Thu Apr 09 00:52:46 2020 +0000
+++ b/main/main.cpp	Thu Apr 09 02:52:18 2020 +0000
@@ -170,7 +170,7 @@
         coords_x[total_points] = curr_coords[0];
         coords_y[total_points] = curr_coords[1];
         type[total_points] = 0b1000;     // start is always 1 exit type in the north direction
-        explored[total_points] = 0b0000; // not sure if i set this as explored already or whether i do that at the next junction
+        explored[total_points] = 0b1000; // 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'
     }
     
@@ -206,15 +206,15 @@
         }
         
         // use current coords to find which point to place in path
-        looped_path[path_length] = coord_to_node(); //returns an int of which point we are at what its called
+        update_index();
+        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
         
         // 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)
         
-        // assign new node with new coordinates
         
-            
     }
     
 //    robot.lcd_clear();
@@ -251,22 +251,26 @@
     return result;
 }
 
-int coord_to_node() 
+void update_index() // update index (pointer to current point/type/coords/explored)
 {
-    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; 
+            curr_index = i; 
         }
     }
-    
-    return point[result];
 }
 
 void node_logic()
 {
     // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
+    
+    // first determine what turns are available relative to the robots current direction (left, straight etc.)
+    // convert these relative available turns into available absolute diections (N,E etc.)
+    // set _type  to the appropriate value based on available directions (including entry direction = opposite of current)
+    // set _explored entry path as 1
+    // set type[total_points] = _type; & explored[total_points] = _explored;
+    
     bool north = false;
     bool south = false;
     bool east = false;
@@ -276,7 +280,8 @@
     bool straight = false;
     bool right = false;
     
-    int total = 1;   // starts at 1 because path entered on is counted
+    int _type = 0b0000;
+    int _explored = 0b0000;
     
     if (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) {
         while ( (sensor[0] > SENS_THRESH || sensor[4] > SENS_THRESH) && (sensor[1] > SENS_THRESH || sensor[2] > SENS_THRESH || sensor[3] > SENS_THRESH) ) {
@@ -298,11 +303,7 @@
         }
     }
     
-    if (left) { total += 1; }
-    if (straight) { total += 1; }
-    if (right) { total += 1; }
-    
-    type[total_points] = total; 
+    // robot.stop(); ?
     
     int angle = 0;
     
@@ -339,41 +340,66 @@
         if (angle == 270) { west = true; }
     }
     
-    int turn_count = type[total_points] - explored[total_points];
-    
-    if (west == false) { turn_count += 1; }
-    if (north == false) { turn_count += 1; }
-    if (east == false) { turn_count += 1; }
-    if (south == false) { turn_count += 1; }
-    
-    turn_count = turn_count%4;
-    
-    char do_turn = turn_priority[turn_count];
+    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 (dir == 'E') { angle = 90;}
-    else if (dir == 'S') { angle = 180;}
-    else if (dir == 'W') { angle = 270;}
-    
-    unsigned int result = 0;
+    if ( west  ) { _type |= 0b1000; }
+    if ( north ) { _type |= 0b0100; }
+    if ( east  ) { _type |= 0b0010; }
+    if ( south ) { _type |= 0b0001; }
     
-    if (do_turn == 'N') { dir = 'N'; result = 0; }
-    else if (do_turn == 'S') { dir = 'S'; result = 180; }
-    else if (do_turn == 'E') { dir = 'E'; result = 90; }
-    else if (do_turn == 'W') { dir = 'W'; result = 270; }
+    type[total_points] = _type; 
+    explored[total_points] = _explored;
     
-    result = result - angle;
-    
-    if (result == 0) { turn_select('S'); }
-    else if (result == 90) { turn_select('R'); }
-    else if (result == 180) { turn_select('B'); }
-    else if (result == 270) { turn_select('L'); }
     // figure out a way to check what directions correlate to which turns based on direction currently facing.
 }
 
 void choose_turn() 
 {
+    // look at cuurent coords, find what node we are at
     // looks at the type vs the explored and does the turn that is equivalent to the first 1 in type that is a 0 in explored (WNES priority)
+    // sets the explored of the current node to 1 in whatever path is chosen
+    // also update dir
     
+    update_index();
+    
+    int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
+    
+    int curr_angle = 0;
+    if ( dir == 'E' ) { curr_angle = 90;}
+    else if ( dir == 'S' ) { curr_angle = 180;}
+    else if ( dir == 'W' ) { curr_angle = 270;}
+    
+    int desired_angle = 0;
+    if ( (unexp_paths & 0b1000) == 0b1000) { 
+        desired_angle = 270; 
+        dir = 'W'; 
+        explored[curr_index] |= 0b1000;
+    }
+    else if ( (unexp_paths & 0b0100) == 0b0100) { 
+        desired_angle = 0;
+        dir = 'N';
+        explored[curr_index] |= 0b0100;
+    }
+    else if ( (unexp_paths & 0b0010) == 0b0010) { 
+        desired_angle = 90; 
+        dir = 'E';
+        explored[curr_index] |= 0b0010;
+    }
+    else if ( (unexp_paths & 0b0001) == 0b0001) { 
+        desired_angle = 180;
+        dir = 'S';
+        explored[curr_index] |= 0b0001;
+    }
+     
+    unsigned int turn_angle = (curr_angle + desired_angle) % 360;
+    
+    if( turn_angle == 0)  { turn_select('S'); } 
+    else if( turn_angle == 90)  { turn_select('R'); } 
+    else if( turn_angle == 180)  { turn_select('B'); } 
+    else if( turn_angle == 270)  { turn_select('L'); } 
 }
 
 void follow_line()