Diff: main/main.cpp

Revision:

33:9fa9e09f2e8f

Parent:

32:598bedb22c7c

Child:

34:63f7c61ee4da

--- a/main/main.cpp	Sun Apr 12 23:39:45 2020 +0000
+++ b/main/main.cpp	Mon Apr 13 15:31:30 2020 +0000
@@ -216,8 +216,11 @@
         char buffer2[2];
         //char buffer3[2];
         robot.lcd_clear();
-        sprintf(buffer1,"%d",curr_coords[0]);
-        sprintf(buffer2,"%d",curr_coords[1]);
+        sprintf(buffer1,"%x",type[curr_index]);
+        sprintf(buffer2,"%x",explored[curr_index]);
+        
+//        sprintf(buffer1,"%d",curr_coords[0]);
+//        sprintf(buffer2,"%d",curr_coords[1]);
         //sprintf(buffer3,"%d",total_points);
         robot.lcd_print(buffer1,2); 
         robot.lcd_goto_xy(0,1);
@@ -421,7 +424,44 @@
     int unexp_paths = type[curr_index] & ~( explored[curr_index] );  // produces a binary of 1's in the available unexplored paths
     
     if (unexp_paths == 0b0000) {
-        back_track();
+        // back_track();
+        turn_select('B');
+        
+        leds = 0b1001;
+        
+        if (dir == 'S') { dir = 'N'; curr_coords[1] +=1; }
+        else if (dir == 'W') { dir = 'E'; curr_coords[0] +=1; }
+        else if (dir == 'N') { dir = 'S'; curr_coords[1] -=1; }
+        else if (dir == 'E') { dir = 'W'; curr_coords[0] -=1; }
+        
+        while(junction_detect() == false) {
+            follow_line();
+        }
+        
+        update_index();
+        
+        path_length++;
+        looped_path[path_length] = point[curr_index];
+        
+        char buffer1[2];
+        char buffer2[2];
+        //char buffer3[2];
+        robot.lcd_clear();
+        sprintf(buffer1,"%x",type[curr_index]);
+        sprintf(buffer2,"%x",explored[curr_index]);
+        
+//        sprintf(buffer1,"%d",curr_coords[0]);
+//        sprintf(buffer2,"%d",curr_coords[1]);
+        //sprintf(buffer3,"%d",total_points);
+        robot.lcd_print(buffer1,2); 
+        robot.lcd_goto_xy(0,1);
+        robot.lcd_print(buffer2,2); 
+        robot.lcd_goto_xy(5,0);
+        char *b = &dir;
+        robot.lcd_print(b,2); 
+        
+        robot.stop();
+        wait(1);
     }
 
     int curr_angle = 0;
@@ -461,91 +501,107 @@
 
 void back_track()
 {
-    //// 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
+    // 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;   // an index to the most recent node with unexplored paths
-    int pointer2;   // a pointer to the previous node in that path that must be visited to reach the node before
-    int new_dir;
-    int count = path_length; // to be added to path length at the end
+    //bool check = false;
+//    int pointer1;   // an index to the most recent node with unexplored paths
+//    int pointer2;   // a pointer to the previous node in that path that must be visited to reach the node before
+//    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
+//            
+//        while(curr_coords[0] != coords_x[pointer1] || curr_coords[1] != coords_y[pointer1]) { // starts at the previous node to current
+//            count--;
+//            // convert looped_path[j] into an index for that nodes coords
+//            pointer2 = path_to_point_index(looped_path[count]); 
+//            
+//            // 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; }  // north
+//                else { new_dir = 2; } // south
+//            } 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; } // east
+//                else { new_dir = 3; } // west
+//            }
+//            
+//            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
+//            
+//            int turn_ang = ((new_dir*90) - curr_angle + 360) % 360;
+//            
+//            if( turn_ang == 0 ) { turn_select('S'); }
+//            else if( turn_ang == 90 ) { turn_select('R'); }
+//            else if( turn_ang == 180 ) { 
+//                robot.reverse(speed); 
+//                wait(0.1); 
+//                turn_select('B');
+//            }
+//            else if( curr_angle == 270 ) { turn_select('L'); }
+//            
+//            while(junction_detect() == false) {
+//                follow_line();
+//            }
+//            
+//            robot.stop();
+//            wait(0.5);
+//            
+//            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];
+//        } 
+//    }
+
+    turn_select('B');
     
-    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;
-        }
+    leds = 0b1001;
+    
+    if (dir == 'S') { dir = 'N'; curr_coords[1] +=1; }
+    else if (dir == 'W') { dir = 'E'; curr_coords[0] +=1; }
+    else if (dir == 'N') { dir = 'S'; curr_coords[1] -=1; }
+    else if (dir == 'E') { dir = 'W'; curr_coords[0] -=1; }
+    
+    while(junction_detect() == false) {
+        follow_line();
     }
     
-    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
-            
-        while(curr_coords[0] != coords_x[pointer1] || curr_coords[1] != coords_y[pointer1]) { // starts at the previous node to current
-            count--;
-            // convert looped_path[j] into an index for that nodes coords
-            pointer2 = path_to_point_index(looped_path[count]); 
-            
-            // 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; }  // north
-                else { new_dir = 2; } // south
-            } 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; } // east
-                else { new_dir = 3; } // west
-            }
-            
-            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
-            
-            int turn_ang = ((new_dir*90) - curr_angle + 360) % 360;
-            
-            if( turn_ang == 0 ) { turn_select('S'); }
-            else if( turn_ang == 90 ) { turn_select('R'); }
-            else if( turn_ang == 180 ) { 
-                robot.reverse(speed); 
-                wait(0.1); 
-                turn_select('B');
-            }
-            else if( curr_angle == 270 ) { turn_select('L'); }
-            
-            while(junction_detect() == false) {
-                follow_line();
-            }
-            
-            robot.stop();
-            wait(0.5);
-            
-            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];
-        } 
-    } 
+    robot.stop();
+    wait(1);
 }
 
 void follow_line()