From b3eb01ad6ec8b9ffbc71770e16aa4f4381f30ef1 Mon Sep 17 00:00:00 2001 From: Alexander Lampalzer Date: Wed, 8 Jun 2022 23:42:43 +0200 Subject: [PATCH] Improvements in odometry kinematics --- server_v2/src/OdometryController.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 2ee64fe..281e3bb 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -63,10 +63,10 @@ OdometryController::OdometryController() { // Forward and Rotational velocity have already been integrated. auto dist_forward = (distance_right + distance_left) / 2.0; - auto dist_rot = (distance_right + distance_left) / ROBOT_ARBOR_LENGTH_M; + auto dist_rot = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; - auto x = dist_forward * -sin(dist_rot); - auto y = dist_forward * cos(dist_rot); + auto x = dist_forward * cos(dist_rot); + auto y = dist_forward * sin(dist_rot); auto theta = dist_rot; auto new_x_position = current_odometry.get_x_position();