ARES / Mbed 2 deprecated Timer

Time is good

Dependencies:   RoboClaw mbed

Fork of Robot2016_2-0 by ARES

Files at this revision

API Documentation at this revision

Revision 33:7f8c29ddee61, committed 2016-01-26

Comitter:
Near32
Date:
Tue Jan 26 17:14:54 2016 +0000
Parent:
32:068bd2b2e1f3
Child:
35:4e3d9ab1b94b
Commit message:
odometry::update_odo : sampling changes on the computation on dx/dy in theta(t-1)+dtheta(t)/2 instead of theta(t-1) only.

Changed in this revision

Odometry/Odometry.cpp Show annotated file Show diff for this revision Revisions of this file 
--- 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)