Diff: main/main.cpp

Revision:

65:2103a02d72bc

Parent:

64:a58d8538132a

Child:

66:3130be689176

--- a/main/main.cpp	Sat Apr 18 21:57:31 2020 +0000
+++ b/main/main.cpp	Fri May 08 15:14:17 2020 +0000
@@ -464,7 +464,7 @@
         }
     }
     
-    if( fully_explored == true ) { looped_goal(); }
+    if( fully_explored == true && first_g == true ) { first_g = false; looped_goal(); }
     
     else {
             // compare node coordinates to previous node coordinates
@@ -680,6 +680,48 @@
     // add inverted sub path to the real path and increment the path length by real_len -1
 }
 
+void shorten( int path[], int length ) 
+{
+    int temp_array[100];
+    int i = 0;
+    
+    while( i <= length ) {
+        int count = i;
+        for( int j = 0; j <= length; j++ ) {
+            if( path[i] == path[j] ){
+                count = j;
+            }
+        }
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = path[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= length; z++ ) {
+                temp_array[i+ind] = path[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= length; x++ ) { path[x] = NULL; }
+            
+            length -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= length; x++ ) { path[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= length; x++ ) { temp_array[x] = NULL; }
+        }
+        i++;
+    }
+    
+    temp_length = length;
+    
+    for( int x = 0; x <= 100; x++ ) { temp_path[x] = NULL; }
+    
+    for( int x = 0; x <= length; x++ ) { temp_path[x] = path[x]; }
+}
+
 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++ ) {
@@ -863,19 +905,19 @@
 //        robot.lcd_clear();
 //    }
     
-    robot.lcd_print("g path",10);
-    wait(1);
-    robot.lcd_clear();
-    
-    leds = 0b1111;
-    for(int m = 0; m <= goal_length1; m++) {
-        leds = ~leds;
-        char buffer[3];
-        sprintf(buffer,"%d", goal_path1[m]);
-        robot.lcd_print(buffer,3);
-        wait(1);
-        robot.lcd_clear();
-    }
+//    robot.lcd_print("g path",10);
+//    wait(1);
+//    robot.lcd_clear();
+//    
+//    leds = 0b1111;
+//    for(int m = 0; m <= goal_length1; m++) {
+//        leds = ~leds;
+//        char buffer[3];
+//        sprintf(buffer,"%d", goal_path1[m]);
+//        robot.lcd_print(buffer,3);
+//        wait(1);
+//        robot.lcd_clear();
+//    }
     
     // print these to check
     // add inverted sub path to the real path and increment the path length by real_len -1
@@ -1165,25 +1207,27 @@
     
     looped_goal_simplification();
     
-    _switch = ~_switch;
-    
     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);
-        
-        if( _switch ) {
+        if( _switch == false ) {
+            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);
             }
+        } else {
+            leds = 0b1001;
+            wait(0.2);
+            leds = 0b0110;
+            wait(0.2);
         }
-        
+                
         wait(2);
         
         t_restart = true;
@@ -1195,7 +1239,6 @@
             if ( t_restart ){                // only start the timer if it isnt already started
                 t_coord.start();
                 t_restart = false;
-                
             }
             
             follow_line();
@@ -1213,23 +1256,22 @@
                 if (dir == 'W'){ curr_coords[0] -= dist_est; }
                 
                 update_index();
-                if(point[curr_index] == 100) {break;}
+                
+                if(point[curr_index] == 100 && _switch == false) { _switch = true; break;}
+                if(point[curr_index] == 0 && _switch == true) { _switch = false; break;}
+                
                 choose_turn(); //looks at the point we are at, examines the type vs explored and makes the appropriate turn also updates the explored
             
             }
         }
-        dir = 'N';
-        curr_coords[0] = 0;
-        curr_coords[1] = 0;
-        update_index();
         
+        turn_select('B');
         robot.stop();
-        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'; }
         
     }  
 }
@@ -1238,7 +1280,7 @@
 {
     char dir_diff;
     
-    if ( inverse == false ) {
+    if ( inverse ) {
         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_path1[j-1]);