turtlebot v 01
Dependencies: Servo mbed-rtos mbed PM hormone
Fork of TurtleBot_v01 by
Diff: main.cpp
- Revision:
- 4:ec7e68b84f2b
- Parent:
- 3:5e867483469e
- Child:
- 5:08334c6a42ca
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;