Diff: odometry/odom.cpp

Revision:

10:3b47050b1652

Parent:

8:80708bacb5b5

--- a/odometry/odom.cpp	Sun Jan 06 04:23:42 2019 +0000
+++ b/odometry/odom.cpp	Fri Mar 22 12:29:18 2019 +0000
@@ -29,9 +29,9 @@
     velocity.x((enc0_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
     velocity.y((enc1_->get_encoder_pulse() * ENCODER_COEFFICIENT) / ((0.02f)));
     
-    world_velocity.x(((velocity.x() * cosf(-odom_.angle())) - (velocity.y() * sinf(-odom_.angle()))));
-    world_velocity.y(((velocity.x() * sinf(-odom_.angle())) + (velocity.y() * cosf(-odom_.angle()))));
-      
+    world_velocity.x(((velocity.x() * cosf(odom_.angle() + initialpose_.angle() - H_PI)) - (velocity.y() * sinf(odom_.angle() + initialpose_.angle() - H_PI))));
+    world_velocity.y(((velocity.x() * sinf(odom_.angle() + initialpose_.angle() - H_PI)) + (velocity.y() * cosf(odom_.angle() + initialpose_.angle() - H_PI))));
+        
     odom_.x(odom_.x() + world_velocity.x() * 0.02f);
     odom_.y(odom_.y() + world_velocity.y() * 0.02f);   
 }