worasuchad haomachai / Mbed 2 deprecated TurtleBot_v01

20/6/18 experiment wt hormone

Dependencies:   Servo TurtleBot_V6 mbed-rtos mbed

Fork of TurtleBot_v00 by worasuchad haomachai

Files at this revision

API Documentation at this revision

Revision 4:ec7e68b84f2b, committed 2018-06-21

Comitter:
worasuchad
Date:
Thu Jun 21 08:12:44 2018 +0000
Parent:
3:5e867483469e
Commit message:
20/6/18

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 5e867483469e -r ec7e68b84f2b main.cpp
--- 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;