From 80ed5e5bf9206e3ac239058a7c7c27050c0cc301 Mon Sep 17 00:00:00 2001 From: Alexander Lampalzer Date: Thu, 9 Jun 2022 10:25:11 +0200 Subject: [PATCH] Improvements in odometry kinematics --- server_v2/src/OdometryController.cpp | 100 +++++++++++++-------------- 1 file changed, 48 insertions(+), 52 deletions(-) diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 281e3bb..8067bea 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -10,84 +10,80 @@ #define US_IN_S (1000 * MS_IN_S) bool OdometryController::is_enabled() const { - return enabled; + return enabled; } void OdometryController::enable() { - last_run = clock::now(); - enabled = true; + last_run = clock::now(); + enabled = true; } void OdometryController::disable() { - spdlog::debug("OdometryController::disable()"); - enabled = false; - OdometryController::reset(); + spdlog::debug("OdometryController::disable()"); + enabled = false; + OdometryController::reset(); } void OdometryController::reset() { - std::lock_guard lock(odometry_mutex); - current_odometry = Odometry(); - last_run = clock::now(); + std::lock_guard lock(odometry_mutex); + current_odometry = Odometry(); + last_run = clock::now(); } Odometry OdometryController::get() { - return current_odometry; + return current_odometry; } OdometryController::OdometryController() { - odometry_thread = std::thread(&OdometryController::odometry_loop, this); - odometry_thread.detach(); + odometry_thread = std::thread(&OdometryController::odometry_loop, this); + odometry_thread.detach(); } [[noreturn]] void OdometryController::odometry_loop() { - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ); - while (true) { - std::this_thread::sleep_for(sleep_duration); - std::lock_guard lock(odometry_mutex); - if (enabled) { - last_run = clock::now(); - auto encoder_positions = Encoders::getInstance().get_positions(); + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_ODOMETRY_CONTROLLER_RATE_HZ); + while (true) { + std::this_thread::sleep_for(sleep_duration); + std::lock_guard lock(odometry_mutex); + if (enabled) { + last_run = clock::now(); + auto encoder_positions = Encoders::getInstance().get_positions(); - auto current_position_left = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; - auto current_position_right = encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER; - auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER; - last_position_left = current_position_left; - last_position_right = current_position_right; + auto current_position_left = + encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + auto current_position_right = + encoder_positions.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + auto distance_left = (current_position_left - last_position_left) / ROBOT_TICKS_PER_METER; + auto distance_right = (current_position_right - last_position_right) / ROBOT_TICKS_PER_METER; + last_position_left = current_position_left; + last_position_right = current_position_right; - // The section below implements differential drive kinematics. - // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin - // or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart - // and Illah R. Nourbakhsh + // The section below implements differential drive kinematics. + // Refer to Computational Principles of Mobile Robotics, Dudek and Jenkin + // or Chapter "Mobile Robot Kinematics" Introduction to Autonomous Mobile Robots, Roland Siegwart + // and Illah R. Nourbakhsh + auto v = (distance_right + distance_left) / 2.0; + auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; - // 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 curr_x = current_odometry.get_x_position(); + auto curr_y = current_odometry.get_y_position(); + auto curr_theta = current_odometry.get_angular_orientation(); - auto x = dist_forward * cos(dist_rot); - auto y = dist_forward * sin(dist_rot); - auto theta = dist_rot; + auto x = v * cos(w); + auto y = v * sin(w); - auto new_x_position = current_odometry.get_x_position(); - auto new_y_position = current_odometry.get_y_position(); - auto new_angular_orientation = current_odometry.get_angular_orientation(); + auto new_x = curr_x + (cos(curr_theta) * x - sin(curr_theta) * y); + auto new_y = curr_y + (sin(curr_theta) * x + cos(curr_theta) * y); + auto new_theta = curr_theta + w; - if (dist_forward != 0) { - new_x_position = cos(theta) * x - sin(theta) * y + current_odometry.get_x_position(); - new_y_position = sin(theta) * x + cos(theta) * y + current_odometry.get_y_position(); - } + new_theta = mathUtils::wrap_angle_to_pi(new_theta); + current_odometry = Odometry(new_x, new_y, new_theta); - if (theta != 0) { - new_angular_orientation = mathUtils::wrap_angle_to_pi(current_odometry.get_angular_orientation() + theta); - } - current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation); - - spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", - current_position_left, current_position_right, - distance_left, distance_right, - dist_forward, dist_rot); +// spdlog::info("{:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f} {:03.4f}", +// current_position_left, current_position_right, +// distance_left, distance_right, +// dist_forward, dist_rot); + } } - } }