High level mecanum robot controller library
Dependents: WRS2021_mecanum_driver
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: