Diff: main/main.cpp

Revision:

29:ecf497c3fdc0

Parent:

28:63ff8290964a

Child:

30:d62f122e8d60

--- a/main/main.cpp	Fri Apr 10 15:14:51 2020 +0000
+++ b/main/main.cpp	Sat Apr 11 22:56:31 2020 +0000
@@ -213,13 +213,14 @@
         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) {
-            // maybe do backtrack before choose turn?
+            // 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 
         }
         
         if (complete) {
-            goal();
+            looped_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)
@@ -274,6 +275,17 @@
     }
 }
 
+int path_to_point_index( int path_point )
+{
+    for(int i = 0; i <= total_points; i++) {
+        if(path_point == point[i]) { 
+            return i;
+        } 
+    }
+    
+    return curr_index;  // default 
+}
+
 void node_logic()
 {
     // is done when a new node is discovered, needs to update the nodes type and the path explored upon entry
@@ -431,7 +443,7 @@
             explored[curr_index] |= 0b0001;
         }
          
-        unsigned int turn_angle = (curr_angle + desired_angle) % 360;
+        int turn_angle = (curr_angle - desired_angle + 360) % 360;
         
         if( turn_angle == 0)  { turn_select('S'); } 
         else if( turn_angle == 90)  { turn_select('R'); } 
@@ -442,7 +454,96 @@
 
 void back_track()
 {
-    //find the colsest previous node and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
+    // find the closest previous node with unexplored paths and go back through the path until reaching that node, updating the path and directions appropriately, then choose turn
+    // also if no nodes have unexplored paths set complete to true
+    
+    bool check = false;
+    int pointer1;
+    int pointer2;
+    int new_dir;
+    int count = path_length; // to be added to path length at the end
+    
+    for(int i = total_points; i >= 0; i--) {    // start from the most recently discovered node
+        if( explored[i] != type[i] ) {
+            check = true;
+            pointer1 = i;
+            break;
+        }
+    }
+    
+    if( check == false ) { complete = true; }
+    
+    else {
+            // compare current coordinates to previous node coordinates
+            // determine which direction to turn based on diretion currently facing
+            // do the appropriate turn
+            // follow line
+            // detect junction
+            // update current coords to this nodes coords
+            // update the current direction, path length and path
+            
+        for(int j = count - 1; j >= pointer1; j--) { // starts at the previous node to current
+             
+            // convert looped_path[j] into an index for that nodes coords
+            pointer2 = path_to_point_index(looped_path[j]); 
+            
+            // then do coords - curr coords
+            if(coords_y[pointer2] != curr_coords[1]) { 
+                // if its positive, go north, negative, go south
+                if( (coords_y[pointer2] - curr_coords[1]) > 0 ) { new_dir = 0; }
+                else { new_dir = 2; }
+            } else if(coords_x[pointer2] != curr_coords[0]) { 
+                // if its positive, go east, negative, go west
+                if( (coords_x[pointer2] - curr_coords[0]) > 0 ) { new_dir = 1; }
+                else { new_dir = 3; }
+            }
+            
+            int curr_angle = 0;
+            if ( dir == 'E' ) { curr_angle = 90;}
+            else if ( dir == 'S' ) { curr_angle = 180;}
+            else if ( dir == 'W' ) { curr_angle = 270;}
+            
+            // change so north = 0
+            // east = 1
+            // south = 2, etc. then i can just add 90*dir to the current angle and modulo 360 then divide by 4
+            
+            curr_angle = ((new_dir*90) - curr_angle + 360) % 360;
+            
+            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); 
+                turn_select('B');
+            }
+            else if( curr_angle == 270 ) { turn_select('L'); }
+            
+            bool junc = false;
+            
+            while(junc == false) {
+                follow_line();
+                if( junction_detect() ){ 
+                    junc = true; 
+                }
+            }
+            
+            curr_coords[0] = coords_x[pointer2];
+            curr_coords[1] = coords_y[pointer2];
+            
+            // dir = new_dir;  // use if dir is changed to int 0123
+            
+            if (new_dir == 0) { dir = 'N'; }
+            else if (new_dir == 1) { dir = 'E'; }
+            else if (new_dir == 2) { dir = 'S'; }
+            else if (new_dir == 3) { dir = 'W'; }
+            
+            path_length++;
+            looped_path[path_length] = point[pointer2];
+
+        }
+        
+        choose_turn();   // if back_track is placed before choose turn this can be removed  
+    }         
 }
 
 void follow_line() 
@@ -710,6 +811,16 @@
     }
 }
 
+void looped_goal() 
+{
+    while(1) {
+        leds = 0b1001;
+        wait(0.2);
+        leds = 0b0110;
+        wait(0.2);
+    }
+}
+
 void invert_path()
 {
     // only call once then can use infinitely