Diff: main/main.cpp

Revision:

58:cb32aa0f4116

Parent:

57:9f9067e3f99e

Child:

59:11e31ce4d675

diff -r 9f9067e3f99e -r cb32aa0f4116 main/main.cpp
--- a/main/main.cpp	Sat Apr 18 00:10:25 2020 +0000
+++ b/main/main.cpp	Sat Apr 18 17:33:31 2020 +0000
@@ -444,9 +444,9 @@
 //    if( turn_angle != 180 ) { robot.forward(speed); wait(0.05); } // maybe remove
     
     if( turn_angle == 0)  { robot.forward(speed); wait(0.1); turn_select('S'); } 
-    else if( turn_angle == 90)  { robot.forward(speed); wait(0.02); turn_select('R'); } 
+    else if( turn_angle == 90)  { robot.forward(speed); wait(0.03); turn_select('R'); } 
     else if( turn_angle == 180)  { turn_select('B'); } 
-    else if( turn_angle == 270)  { robot.forward(speed); wait(0.02); turn_select('L'); } 
+    else if( turn_angle == 270)  { robot.forward(speed); wait(0.03); turn_select('L'); } 
 }
 
 void back_track()
@@ -459,7 +459,7 @@
     int d_node;   // an index to the most recent (desired) node with unexplored paths
     
     for(int i = total_points; i >= 0; i--) {    // start from the most recently discovered node
-        if( explored[i] != type[i] ) {
+        if( explored[i] != type[i] ) {          
             complete = true;
             d_node = i;
             break;
@@ -689,6 +689,94 @@
     // add inverted sub path to the real path and increment the path length by real_len -1
 }
 
+void looped_goal_simplification()   // change into dead_end_removal and have it take two indexes and a path and return an array
+{   
+    for( int i = 0; i <= path_length; i++ ) {
+        goal_path1[i] = looped_path[i];
+        if( looped_path[i] == 100 ) { goal_length1 = i; break; }
+    }
+    
+    for( int i = path_length; i >= 0; i-- ) {
+        goal_path2[path_length-i] = looped_path[i];
+        if( looped_path[i] == 100 ) { goal_length2 = path_length-i; break; }
+    }
+    
+    int temp_array[100];
+    
+    int i = 0;
+    while( i <= goal_length1 ) {
+        int count = i;
+        for( int j = 0; j <= goal_length1; j++ ) {
+            if( goal_path1[i] == goal_path1[j] ){
+                count = j;
+            }
+        }
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = goal_path1[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= goal_length1; z++ ) {
+                temp_array[i+ind] = goal_path1[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL; }
+            
+            goal_length1 -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= goal_length1; x++ ) { temp_array[x] = NULL; }
+        }
+        i++;
+    }
+    
+    i = 0;
+    while( i <= goal_length2 ) {
+        int count = i;
+        for( int j = 0; j <= goal_length2; j++ ) {
+            if( goal_path2[i] == goal_path2[j] ){
+                count = j;
+            }
+        }
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = goal_path2[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= goal_length2; z++ ) {
+                temp_array[i+ind] = goal_path2[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= goal_length2; x++ ) { goal_path2[x] = NULL; }
+            
+            goal_length2 -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= goal_length2; x++ ) { goal_path2[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= goal_length2; x++ ) { temp_array[x] = NULL; }
+        }
+        i++;
+    }
+    
+    if( goal_length2 < goal_length1 ) {
+        for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL;  }
+        for( int x = 0; x <= goal_length2; x++ ) { 
+            goal_path1[x] = goal_path2[x];
+        }
+        goal_length1 = goal_length2;
+    }
+    
+    // print these to check
+    // add inverted sub path to the real path and increment the path length by real_len -1
+}
+
 void follow_line() 
 {
     robot.scan();
@@ -807,7 +895,7 @@
     while (sensor[0] > SENS_THRESH) { robot.scan(); }
     
     robot.spin_left(TURN_SPEED);
-    wait(0.2);
+    wait(0.25);
     
     while (sensor[1] < SENS_THRESH) { robot.scan(); }
     
@@ -821,7 +909,7 @@
     while (sensor[4] > SENS_THRESH) { robot.scan(); }
     
     robot.spin_right(TURN_SPEED);
-    wait(0.2);
+    wait(0.25);
     
     while (sensor[3] < SENS_THRESH) { robot.scan(); }
     
@@ -956,30 +1044,151 @@
             }
         }
     }
-}
+}   
 
 void looped_goal() 
 {
     // return to start (adding nodes to path)
     // simplify path, check path before reaching goal and path after reaching goal, use the shorter one
     // loop between start and goal
+    turn_select('B');
     robot.stop();
-    bool na = dead_end_removal( point[0], point[total_points] );
-    leds = 0b1001;
-        wait(0.2);
-        leds = 0b0110;
-        wait(0.2);
-    na = dead_end_removal( point[2], point[1] );
-    leds = 0b1001;
-        wait(0.2);
-        leds = 0b0110;
-        wait(0.2);
-    na = dead_end_removal( point[4], 100 );
-    while( button_enter.read() == 1 ) {
-        leds = 0b1001;
-        wait(0.2);
-        leds = 0b0110;
-        wait(0.2);
+    
+    if( dir == 'N' ) { dir = 'S'; }
+    else if( dir == 'S' ) { dir = 'N'; }
+    else if( dir == 'E' ) { dir = 'W'; }
+    else if( dir == 'W' ) { dir = 'E'; }
+    
+    looped_goal_simplification();
+    
+    bool _switch = true;
+    
+    while(1) {
+        robot.lcd_clear();
+        robot.lcd_goto_xy(0,0);
+        robot.lcd_print("  ENTER   ", 10);
+        robot.lcd_goto_xy(0,1);
+        robot.lcd_print("=restart", 10);
+        
+        while( button_enter.read() == 1 ) {
+            leds = 0b1001;
+            wait(0.2);
+            leds = 0b0110;
+            wait(0.2);
+        }
+        
+        wait(2);
+        
+        t_restart = true;
+        
+        goal_path_explored(_switch);
+        
+        _switch = ~ _switch;
+        
+        while(1) {
+        
+            if ( t_restart ){                // only start the timer if it isnt already started
+                t_coord.start();
+                t_restart = false;
+                
+            }
+            
+            follow_line();
+            
+            if( junction_detect() ) {
+                float time = t_coord.read();
+                int dist_est = ceil(time*2); // scaled so that a longer straight will have a different time to a shorter straight
+                t_coord.stop();
+                t_coord.reset();
+                t_restart = true; //restart the timer next loop
+                
+                if (dir == 'N'){ curr_coords[1] += dist_est; }  // y coord
+                if (dir == 'E'){ curr_coords[0] += dist_est; }  // x coord
+                if (dir == 'S'){ curr_coords[1] -= dist_est; }
+                if (dir == 'W'){ curr_coords[0] -= dist_est; }
+                
+                update_index();
+                choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
+            
+            }
+            
+            if( _switch == true && point[curr_index] == 100 )  { break; }
+            else if( _switch == false && point[curr_index] == 0 ) { break; }
+        }
+        
+        robot.stop();
+    }  
+}
+
+void goal_path_explored( bool inverse )
+{
+    char dir_diff;
+    
+    if ( inverse == false ) {
+        for(int j = goal_length1; j >= 0; j--) {
+            int curr_node = path_to_point_index(goal_path1[j]);
+            int next_node = path_to_point_index(goal_path2[j-1]);
+            if(coords_x[next_node] != coords_x[curr_node]) {
+                if(coords_x[next_node] - coords_x[curr_node] > 0){
+                    dir_diff = 'E';
+                } else {
+                    dir_diff = 'W';
+                }
+            } else if( coords_y[next_node] != coords_y[curr_node] ) {
+                if(coords_y[next_node] - coords_y[curr_node] > 0){
+                    dir_diff = 'N';
+                } else {
+                    dir_diff = 'S';
+                }
+            }
+            
+            if( dir_diff == 'N' ) { 
+                explored[curr_node] &= 0b1011;
+            }
+            else if( dir_diff == 'E' ) { 
+                explored[curr_node] &= 0b1101; 
+            }
+            else if( dir_diff == 'S' ) { 
+                explored[curr_node] &= 0b1110;
+            }
+            else if( dir_diff == 'W' ) { 
+                explored[curr_node] &= 0b0111;
+            }
+        }
+    }
+    
+    else{
+        
+        for(int j = 0; j <= goal_length1; j++) {
+            int curr_node = path_to_point_index(goal_path1[j]);
+            int next_node = path_to_point_index(goal_path2[j+1]);
+            if(coords_x[next_node] != coords_x[curr_node]) {
+                if(coords_x[next_node] - coords_x[curr_node] > 0){
+                    dir_diff = 'E';
+                } else {
+                    dir_diff = 'W';
+                }
+            } else if( coords_y[next_node] != coords_y[curr_node] ) {
+                if(coords_y[next_node] - coords_y[curr_node] > 0){
+                    dir_diff = 'N';
+                } else {
+                    dir_diff = 'S';
+                }
+            }
+            
+            if( dir_diff == 'N' ) { 
+                explored[curr_node] &= 0b1011;
+            }
+            else if( dir_diff == 'E' ) { 
+                explored[curr_node] &= 0b1101; 
+            }
+            else if( dir_diff == 'S' ) { 
+                explored[curr_node] &= 0b1110;
+            }
+            else if( dir_diff == 'W' ) { 
+                explored[curr_node] &= 0b0111;
+            }
+        }
     }
 }