hidaka sato / MecanumController

High level mecanum robot controller library

Dependents:   WRS2021_mecanum_driver

Diff: mecanum_controller.hpp

Revision:
2:bc2e5034dda7
Parent:
1:26c16fb42252
--- a/mecanum_controller.hpp	Wed Aug 25 08:52:40 2021 +0000
+++ b/mecanum_controller.hpp	Wed Aug 25 12:11:24 2021 +0000
@@ -255,8 +255,8 @@
         UpdateTheta();
         double dx = (-wheel_rad_[0] - wheel_rad_[1] + wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius;
         double dy = ( wheel_rad_[0] - wheel_rad_[1] - wheel_rad_[2] + wheel_rad_[3]) / 4 * param_.wheel_radius;
-        pose_.x += dx;
-        pose_.y += dy;
+        pose_.x += dx * cos(pose_.theta) + dy * sin(-pose_.theta);
+        pose_.y += dy * cos(pose_.theta) + dx * sin(pose_.theta);
     }
     
     private: