ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Diff: Odometry/Odometry.cpp

Revision:
36:2d7357a385bc
Parent:
35:4e3d9ab1b94b
Child:
37:da3a2c781672
diff -r 4e3d9ab1b94b -r 2d7357a385bc Odometry/Odometry.cpp
--- a/Odometry/Odometry.cpp	Tue Jan 26 17:21:11 2016 +0000
+++ b/Odometry/Odometry.cpp	Sun Jan 31 16:11:32 2016 +0000
@@ -73,18 +73,18 @@
         x = xO + R*sin(theta);
         y = yO - R*cos(theta);
     }*/
+    
+    
+    
+    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;
     
-    
-    double dx = deltaS*cos(theta-deltaTheta/2);
-    double dy = deltaS*sin(theta-deltaTheta/2);
-    x += dx;
-    y += dy;
-    
-
-    
 }
 
 void Odometry::GotoXY(double x_goal, double y_goal)