Diff: main/main.cpp

Revision:

48:76cf4521d342

Parent:

43:ec047ba15db1

Child:

49:89f0f54aa46b

--- a/main/main.cpp	Wed Apr 15 17:37:23 2020 +0000
+++ b/main/main.cpp	Fri Apr 17 13:20:19 2020 +0000
@@ -441,17 +441,9 @@
 //    
 //    print_data("After Turn");
     
-    if( turn_angle == 0)  { 
-        robot.forward(speed);
-        wait(0.1);
-        turn_select('S'); 
-    } 
+    if( turn_angle == 0)  { turn_select('S'); } 
     else if( turn_angle == 90)  { turn_select('R'); } 
-    else if( turn_angle == 180)  { 
-//        robot.reverse(speed);
-//        wait(0.1);
-        turn_select('B');
-    } 
+    else if( turn_angle == 180)  { turn_select('B'); } 
     else if( turn_angle == 270)  { turn_select('L'); } 
 }
 
@@ -461,12 +453,13 @@
     // also if no nodes have unexplored paths set complete to true
     
     bool complete = false;
-    int pointer;   // an index to the most recent node with unexplored paths
+    
+    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] ) {
             complete = true;
-            pointer = i;
+            d_node = i;
             break;
         }
     }
@@ -480,78 +473,180 @@
             // 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
+        
+        // check if the current node exists before the discovery of the desired node
+        // if it does, check the number of nodes between before and after
+        // which ever is shorter, set the 0 of those nodes
+        
+        int curr_node = curr_index;
+        bool before = false;
+        bool after = false;
+        int before_index;
+        int after_index;
+        
+        bool desired_discovered = false;
+        
+        for(int k = 0; k <= path_length; k++) {
+            if(looped_path[k] == point[d_node]) { desired_discovered = true; }  // maybe set a variable
+            if(desired_discovered == false && looped_path[k] == point[curr_node]) { before = true; before_index = k; }
+            if(desired_discovered == true && looped_path[k] == point[curr_node]) { after_index = k; break; }
+        }
+        
+        
+        if( before_index < after_index ){
+            before = true;
+            after = false;
+        }
+        else{
+            before = false;
+            after = true;
+        }
+        
         char dir_diff;
-          
-        for(int j = path_length; j >= 0; j--) {
-            int curr_node = path_to_point_index(looped_path[j]);
-            // check if curr node is repeated, if it is skip to the earliest one thats infront of the unxplored node
-            int correct_ind = node_repeat_check(curr_node, point[pointer]);
-            while(j != correct_ind) { j--; }
-            
-            int prev_node = path_to_point_index(looped_path[j-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(after == true) {
+             
+            for(int j = after_index; j >= 0; j--) {
+                curr_node = path_to_point_index(looped_path[j]);
+                
+                int prev_node = path_to_point_index(looped_path[j-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; 
+                
+                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;
+                }
+                if(point[prev_node] == point[d_node]) { break; }
             }
-            else if( dir_diff == 'S' ) { 
-                explored[curr_node] &= 0b1110; 
-                //explored[prev_node] &= 0b1011;
+        }
+        
+        else if( before == true ) {
+            for(int j = before_index; j <= path_length; j++) {
+                curr_node = path_to_point_index(looped_path[j]);
+                
+                int next_node = path_to_point_index(looped_path[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;
+                }
+                if(point[next_node] == point[d_node]) { break; }
             }
-            else if( dir_diff == 'W' ) { 
-                explored[curr_node] &= 0b0111;  
-                //explored[prev_node] &= 0b1101;
-            }
-//            print_data("after bt");
-            if(point[prev_node] == point[pointer]) { break; }
         }
     }
 }
 
-int node_repeat_check(int curr_node, int desired_node)  // will skip deadends in bt
-{
-    int repeats = 0;
-    int index[10];   // array of all the indexes that contain the curr_node
-    int earliest_d;  // index of desired node
-    int result;
+void dead_end_removal( int index1, int index2)   // change into dead_end_removal and have it take two indexes and a path and return an array
+{   
+    // dead end removal between start and final node
+    // add the inverse of the result to end of the looped_path
+    // then separate into before and after, simplify both and compare
+    
+    int temp_array[100];
+    
+    for( int x = 0; x <= path_length; x++ ) { shortest[x] = NULL; }
+    
+    for( int i = index1; i <= index2; i++ ) { shortest[i] = looped_path[i]; }
+    
+    int length = index2 - index1;
+    
+    char buffer[3];
+    sprintf(buffer,"%d", length);
+    robot.lcd_print(buffer,3);
+    wait(3);
+    robot.lcd_clear();
+    
+    // for every node in path, check if repeated
+    // if rpeated, move everyting before the first occurence to the final occurence
+    // save new path and repeat
+    // when no node is repeated, end
+    // or end when iterated through the entire array
+    
+    int i = 0;
     
-    for( int i = path_length; i >= 0; i-- ) {
-        if(looped_path[i] == point[curr_node]) { 
-            index[repeats] = i;
-            repeats++;
+    while( i <= length ) {
+        int count = i;
+        for( int j = 0; j <= length; j++ ) {
+            if( shortest[i] == shortest[j] ){
+                count = j;
+            }
         }
-        if(looped_path[i] == point[desired_node]) { 
-            earliest_d = i;
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = shortest[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= path_length; z++ ) {
+                temp_array[i+ind] = shortest[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= length; x++ ) { shortest[x] = NULL; }
+            
+            length -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= length; x++ ) { shortest[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= length; x++ ) { temp_array[x] = NULL; }
         }
+        i++;
     }
     
-    // return the index of the earliest 'c' that comes after a 'd'
-    
-    for( int j = repeats - 1; j >= 0; j-- ) {
-        if( earliest_d < index[j] ) {
-            result = index[j];
-            break;
-        }
+    leds = 0b1111;
+    for(int m = 0; m <= length; m++) {
+        leds = ~leds;
+        char buffer[3];
+        sprintf(buffer,"%d", shortest[m]);
+        robot.lcd_print(buffer,3);
+        wait(3);
+        robot.lcd_clear();
     }
     
-    return result;    // returns the correct path index of the node
+    // print these to check
+    // add inverted sub path to the real path and increment the path length by real_len -1
 }
 
 void follow_line() 
@@ -825,9 +920,14 @@
 
 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
     robot.stop();
+    dead_end_removal( 0, path_length );
+//    simplify_looped();
     
-    while(1) {
+    while( button_enter.read() == 1 ) {
         leds = 0b1001;
         wait(0.2);
         leds = 0b0110;