Diff: main/main.cpp

Revision:

64:a58d8538132a

Parent:

63:0358f24b6181

Child:

65:2103a02d72bc

--- a/main/main.cpp	Sat Apr 18 21:19:56 2020 +0000
+++ b/main/main.cpp	Sat Apr 18 21:57:31 2020 +0000
@@ -687,13 +687,23 @@
         if( looped_path[i] == 100 ) { goal_length1 = i; break; }
     }
     
+    for( int i = goal_length1; i >= 0; i-- ) {
+        goal_path3[goal_length1-i] = goal_path1[i];
+    }
+    int goal_length3 = goal_length1;
+    
     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; }
     }
     
+    for( int i = goal_length2; i >= 0; i-- ) {
+        goal_path4[goal_length2-i] = goal_path2[i];
+    }
+    int goal_length4 = goal_length2;
+    
     int temp_array[100];
-    
+    // path 1
     int i = 0;
     while( i <= goal_length1 ) {
         int count = i;
@@ -724,7 +734,7 @@
         }
         i++;
     }
-    
+    // path 2
     i = 0;
     while( i <= goal_length2 ) {
         int count = i;
@@ -755,28 +765,103 @@
         }
         i++;
     }
+    // path 3
+    i = 0;
+    while( i <= goal_length3 ) {
+        int count = i;
+        for( int j = 0; j <= goal_length3; j++ ) {
+            if( goal_path3[i] == goal_path3[j] ){
+                count = j;
+            }
+        }
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = goal_path3[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= goal_length3; z++ ) {
+                temp_array[i+ind] = goal_path3[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= goal_length3; x++ ) { goal_path3[x] = NULL; }
+            
+            goal_length3 -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= goal_length3; x++ ) { goal_path3[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= goal_length3; x++ ) { temp_array[x] = NULL; }
+        }
+        i++;
+    }
+    i = 0;
+    while( i <= goal_length4 ) {
+        int count = i;
+        for( int j = 0; j <= goal_length4; j++ ) {
+            if( goal_path4[i] == goal_path4[j] ){
+                count = j;
+            }
+        }
+        if( count != i ) { 
+            
+            for( int k = 0; k <= i; k++ ) {
+                temp_array[k] = goal_path4[k];
+            }
+            int ind = 1;
+            for( int z = count+1; z <= goal_length4; z++ ) {
+                temp_array[i+ind] = goal_path4[z];
+                ind++;
+            }
+            // clear the array
+            for( int x = 0; x <= goal_length4; x++ ) { goal_path4[x] = NULL; }
+            
+            goal_length4 -= (count-i);
+            i = -1;
+            
+            for( int x = 0; x <= goal_length4; x++ ) { goal_path4[x] = temp_array[x]; }
+            
+            for( int x = 0; x <= goal_length4; x++ ) { temp_array[x] = NULL; }
+        }
+        i++;
+    }
     
-    if( goal_length2 < goal_length1 ) {
+    if( goal_length2 <= goal_length1 && goal_length2 <= goal_length3 && goal_length2 <= goal_length4 ) {
         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;
     } 
-    
-    robot.lcd_print("path",10);
-    wait(1);
-    robot.lcd_clear();
+    else if( goal_length3 <= goal_length1 && goal_length3 <= goal_length2 && goal_length3 <= goal_length4 ) {
+        for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL;  }
+        for( int x = goal_length3; x >= 0; x-- ) { 
+            goal_path1[goal_length3 - x] = goal_path3[x];
+        }
+        goal_length1 = goal_length3;
+    } 
+    else if( goal_length4 <= goal_length1 && goal_length4 <= goal_length2 && goal_length4 <= goal_length3 ) {
+        for( int x = 0; x <= goal_length1; x++ ) { goal_path1[x] = NULL;  }
+        for( int x = goal_length4; x >= 0; x-- ) { 
+            goal_path1[goal_length4 - x] = goal_path4[x];
+        }
+        goal_length1 = goal_length4;
+    } 
     
-    leds = 0b1111;
-    for(int m = 0; m <= path_length; m++) {
-        leds = ~leds;
-        char buffer[3];
-        sprintf(buffer,"%d", looped_path[m]);
-        robot.lcd_print(buffer,3);
-        wait(1);
-        robot.lcd_clear();
-    }
+    //robot.lcd_print("path",10);
+//    wait(1);
+//    robot.lcd_clear();
+//    
+//    leds = 0b1111;
+//    for(int m = 0; m <= path_length; m++) {
+//        leds = ~leds;
+//        char buffer[3];
+//        sprintf(buffer,"%d", looped_path[m]);
+//        robot.lcd_print(buffer,3);
+//        wait(1);
+//        robot.lcd_clear();
+//    }
     
     robot.lcd_print("g path",10);
     wait(1);
@@ -1128,13 +1213,24 @@
                 if (dir == 'W'){ curr_coords[0] -= dist_est; }
                 
                 update_index();
-                
+                if(point[curr_index] == 100) {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();
         
-        //_switch = ~ _switch;
+        robot.stop();
+        while( button_enter.read() == 1 ) {
+            leds = 0b1001;
+            wait(0.2);
+            leds = 0b0110;
+            wait(0.2);
+        }
+        
     }  
 }