Diff: main/main.cpp

Revision:

34:63f7c61ee4da

Parent:

33:9fa9e09f2e8f

Child:

35:2fd4ee9ac889

--- a/main/main.cpp	Mon Apr 13 15:31:30 2020 +0000
+++ b/main/main.cpp	Tue Apr 14 00:36:29 2020 +0000
@@ -166,11 +166,11 @@
         curr_coords[1] = 0;
         dir = 'N';
         total_points = 0;
-        point[total_points] = total_points;
+        point[total_points] = total_points;       // first point is 0
         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] = 0b1000;          // not sure if i set this as explored already or whether i do that at the next junction
+        type[total_points] = 0b0100;              // start is always 1 exit type in the north direction
+        explored[total_points] = 0b0100;          
         looped_path[total_points] = 0;            // start node is '0'
     }
     
@@ -214,20 +214,22 @@
         
         char buffer1[2];
         char buffer2[2];
-        //char buffer3[2];
+        char buffer3[2];
         robot.lcd_clear();
-        sprintf(buffer1,"%x",type[curr_index]);
-        sprintf(buffer2,"%x",explored[curr_index]);
+//        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);
+        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.lcd_goto_xy(5,1);
+        robot.lcd_print(buffer3,2);
         
         robot.stop();
         wait(2);
@@ -341,8 +343,8 @@
         }
     }
     
-//    robot.stop(); 
-//    wait(1);
+    robot.stop(); 
+    wait(1);
     
 //    if(goal) {
 //        if( dir == 'N' ) { south = true;       _explored |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
@@ -367,9 +369,11 @@
         int angle = 0;
         int reset_ang = 0;
         
-        if (dir == 'E') { angle = 90; reset_ang = 90; }
-        else if (dir == 'S') { angle = 180; reset_ang = 180; }
-        else if (dir == 'W') { angle = 270; reset_ang = 270; }
+        if (dir == 'E') { angle = 90; }
+        else if (dir == 'S') { angle = 180; }
+        else if (dir == 'W') { angle = 270; }
+        
+        reset_ang = angle;
         
         if (left) { 
             angle += 270;
@@ -399,7 +403,7 @@
         }
         
         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 == 'E' ) { west = true;  _explored |= 0b1000; }  // this is acc done in choose turn so might not be needed here
         else if ( dir == 'S' ) { north = true; _explored |= 0b0100; }
         else if ( dir == 'W' ) { east = true;  _explored |= 0b0010; }
         
@@ -421,47 +425,15 @@
     // sets the explored of the current node to 1 in whatever path is chosen
     // also update dir
     
+    if( dir == 'N' ) {       explored[curr_index] |= 0b0001; }  // sets the direction opposite to entry direction as an explored path 
+    else if ( dir == 'E' ) { explored[curr_index] |= 0b1000; }
+    else if ( dir == 'S' ) { explored[curr_index] |= 0b0100; }
+    else if ( dir == 'W' ) { explored[curr_index] |= 0b0010; }
+    
     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();
-        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);
+        back_track();
     }
 
     int curr_angle = 0;
@@ -504,104 +476,65 @@
     // 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
-//    
-//    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');
+    bool check = false;
+    int pointer;   // an index to the most recent node with unexplored paths
     
-    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();
+    for(int i = total_points; i >= 0; i--) {    // start from the most recently discovered node
+        if( explored[i] != type[i] ) {
+            check = true;
+            pointer = i;
+            break;
+        }
     }
     
-    robot.stop();
-    wait(1);
+    if( check == false ) { complete = true; }
+    
+    else {
+            // compare node coordinates to previous node coordinates
+            // determine which direction the previous node is compared to current
+            // set the current nodes direction path to 0 and the opposite direction path to 0
+            // decrement the count (setting the previous as current node and the next previous as previous)
+            // when previous node == point[pointer1] break
+            // choose turn should then do all those turns and arrive at correct node
+        char dir_diff;
+          
+        for(int j = path_length; j >= 0; j--) {
+            int curr_node = path_to_point_index(looped_path[path_length]);
+            int prev_node = path_to_point_index(looped_path[path_length-1]);
+            if(coords_x[prev_node] != coords_x[curr_node]) {
+                if(coords_x[prev_node] - coords_x[curr_node] > 0){
+                    dir_diff = 'E';
+                } else {
+                    dir_diff = 'W';
+                }
+            } else if( coords_y[prev_node] != coords_y[curr_node] ) {
+                if(coords_y[prev_node] - coords_y[curr_node] > 0){
+                    dir_diff = 'N';
+                } else {
+                    dir_diff = 'S';
+                }
+            }
+            
+            if( dir_diff == 'N' ) { 
+                explored[curr_node] &= 0b1011; 
+                explored[prev_node] &= 0b1110; 
+            }
+            else if( dir_diff == 'E' ) { 
+                explored[curr_node] &= 0b1101; 
+                explored[prev_node] &= 0b0111; 
+            }
+            else if( dir_diff == 'S' ) { 
+                explored[curr_node] &= 0b1110; 
+                explored[prev_node] &= 0b1011;
+            }
+            else if( dir_diff == 'W' ) { 
+                explored[curr_node] &= 0b0111;  
+                explored[prev_node] &= 0b1101;
+            }
+            
+            if(point[prev_node] == point[pointer]) { break; }
+        }
+    }
 }
 
 void follow_line() 
@@ -871,6 +804,8 @@
 
 void looped_goal() 
 {
+    robot.stop();
+    
     while(1) {
         leds = 0b1001;
         wait(0.2);