Diff: main.cpp

Revision:

4:ec7e68b84f2b

Parent:

3:5e867483469e

Child:

5:08334c6a42ca

--- a/main.cpp	Thu Jun 21 06:05:00 2018 +0000
+++ b/main.cpp	Thu Jun 21 08:12:44 2018 +0000
@@ -154,14 +154,23 @@
                 
                 if(directionRobot(MeanOfYaw, SDOfYaw))    // only one time for comming 
                 {
-                    directOfRobot = MeanOfYaw;
+                    directOfRobot = MeanOfYaw;            // direction of robot 
                     thread2.start(servo);                 // Servo Thread
                     pc.printf("::::::::::::::::::::::::::: Init Direction OK :::::::::::::::::::::::::::\n\r");
                 }
+                pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
                 
-                pc.printf("directOfRobot: %.3f\n\r", directOfRobot);
+                if(directOfRobot > SDOfYaw)
+                {
+                    pc.printf("Turn Left: %.3f\n\r", (directOfRobot - MeanOfYaw));
+                }
+                else
+                {
+                    pc.printf("Turn Right: %.3f\n\r", (MeanOfYaw - directOfRobot));
+                }
+                
                 // func find diff beween directOfRobot and MeanOfYaw
-                pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
+                //pc.printf("Diff from direction: %.3f\n\r", abs(directOfRobot - MeanOfYaw));
                 
                 // reset iteration
                 i = 0;