Romain Ame / Mbed 2 deprecated Robot2016_2-0_STATIC

Voili voilou

Dependencies:   RoboClaw StepperMotor mbed

Fork of Robot2016_2-0 by ARES

Diff: Odometry/Odometry.cpp

Revision:
34: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)