Diff: main.cpp

Revision:

23:accd07ca2da7

Parent:

21:41997651337a

Child:

28:b7ce1e3bf08b

--- a/main.cpp	Tue May 15 11:19:15 2018 +0000
+++ b/main.cpp	Tue May 15 13:35:50 2018 +0000
@@ -54,7 +54,10 @@
     const int MOVE = 1;
     const int LEFT = 2;
     const int RIGHT = 3;
-    const int SPEED = 4;
+    const int HALF_MOVE = 4;
+    const int TURN_LEFT = 5;
+    const int TURN_RIGHT = 6;
+    const int EMPTY = 7;
     
     //Sensor tresholds [mm]
     const float thresholdL = 80;
@@ -312,7 +315,31 @@
             if (motion.finish() == 1) {
             
                 Ziel = 1;  
-                r = 0;  
+                r = 1;  
+                //Convert rotations to smooth turns
+                while (route[r] != 0) {
+                    //Convert pattern MOVE - ROTATE - MOVE
+                    if (route[r-1] == MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
+                        
+                        route[r-1] = HALF_MOVE;
+                        route[r+1] = HALF_MOVE;
+                        
+                        if (route[r] == LEFT) route[r] = TURN_LEFT;
+                        else route[r] = TURN_RIGHT;
+                    //Convert consequent ROTATION to a smooth TURN 
+                    }else if (route[r-1] == HALF_MOVE && (route[r] == LEFT || route[r] == RIGHT) && route[r+1] == MOVE) {
+                    
+                        route[r-1] = EMPTY;
+                        route[r+1] = HALF_MOVE;
+                    
+                        if (route[r] == LEFT) route[r] = TURN_LEFT;
+                        else route[r] = TURN_RIGHT;   
+                       
+                    }
+                    
+                    r++;
+                }
+                r = 0;
                 start = 0;
                 controller.counterReset();
                 myled = 1;