hidaka sato / MecanumController

High level mecanum robot controller library

Dependents:   WRS2021_mecanum_driver

Files at this revision

API Documentation at this revision

Revision 2:bc2e5034dda7, committed 2021-08-25

Comitter:
sgrsn
Date:
Wed Aug 25 12:11:24 2021 +0000
Parent:
1:26c16fb42252
Commit message:
Fix calculation of odometry

Changed in this revision

mecanum_controller.hpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 26c16fb42252 -r bc2e5034dda7 mecanum_controller.hpp
--- 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: