Diff: main/main.cpp

Revision:

4:38c29dbc5953

Parent:

3:a5e06482462e

Child:

5:ae417756235a

--- a/main/main.cpp	Thu Feb 13 16:34:40 2020 +0000
+++ b/main/main.cpp	Thu Feb 13 17:14:39 2020 +0000
@@ -36,9 +36,10 @@
 
 const float A = 0.5;         // 20
 const float B = 1/10000;     // 10000
-const float C = 1.5;    // 2/3
+const float C = 1.5;         // 2/3
 
-const int sensor_threshold = 500;
+const int sensor_threshold = 500;    // replace the hard coded bits
+const int turn_speed = 0.2;
 
 // Global Variables
 
@@ -106,7 +107,12 @@
             //goal(simple_path);  // make this an infinite loop that 
         }
         
-        robot.display_data();
+        simplify();
+        
+        robot.lcd_clear();
+        robot.lcd_print(path,100);
+    
+        //robot.display_data();
         
         wait(dt);
     }
@@ -231,8 +237,24 @@
     // if it was, iterate over the last three turns and check the total angle change
     // replace the three turns with the new single turn
     
-    if( path[path_length-1] == 'B' ) {
+    if( path[path_length-2] == 'B' && path_length > 3) {
         int angle_change;
         
+        for (int i = 0; i < 3; i++) {
+            if (path[i] == 'L') { angle_change += 270; }
+            else if (path[i] == 'R') { angle_change += 90; }
+            else if (path[i] == 'B') { angle_change += 180; }
+        }
+        
+        angle_change = angle_change % 360;
+        
+        if (angle_change == 0) { path[path_length - 3] = 'S'; }
+        else if (angle_change == 90) { path[path_length - 3] = 'R'; }
+        else if (angle_change == 180) { path[path_length - 3] = 'B'; }
+        else if (angle_change == 270) { path[path_length - 3] = 'L'; }
+        
+        for (int i = 1; i <= 2; i++) { path[path_length - i] = NULL; }   // clear the other turns
+        
+        path_length -= 2;        
     }
 }  
\ No newline at end of file