Romain Ame / Mbed 2 deprecated Timer71pt

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   RoboClaw mbed

Fork of Timer by ARES

Diff: Odometry/Odometry.cpp

Revision:
33:7f8c29ddee61
Parent:
32:068bd2b2e1f3
Child:
35:4e3d9ab1b94b
--- a/Odometry/Odometry.cpp	Tue Jan 26 17:02:41 2016 +0000
+++ b/Odometry/Odometry.cpp	Tue Jan 26 17:14:54 2016 +0000
@@ -73,15 +73,18 @@
         x = xO + R*sin(theta);
         y = yO - R*cos(theta);
     }*/
-
-    double dx = deltaS*cos(theta);
-    double dy = deltaS*sin(theta);
+    theta += deltaTheta;
+    while(theta > PI) theta -= 2*PI;
+    while(theta <= -PI) theta += 2*PI;
+    
+    
+    double dx = deltaS*cos(theta-deltaTheta/2);
+    double dy = deltaS*sin(theta-deltaTheta/2);
     x += dx;
     y += dy;
-    theta += deltaTheta;
+    
 
-    while(theta > PI) theta -= 2*PI;
-    while(theta <= -PI) theta += 2*PI;
+    
 }
 
 void Odometry::GotoXY(double x_goal, double y_goal)