From 2c5f283a46b98e14ec116c5a4e81bd1c235d1ab6 Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Thu, 9 Jun 2022 22:44:26 +0200 Subject: [PATCH] Fix goToAngle with new odom --- server_v2/CMakeLists.txt | 3 +- server_v2/include/Robot.hpp | 4 +- server_v2/src/GoToController.cpp | 9 ++- server_v2/src/OdometryController.cpp | 96 ++++++++++++++-------------- server_v2/src/main.cpp | 21 +++--- 5 files changed, 71 insertions(+), 62 deletions(-) diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt index 90d4b2e..b9dd7c0 100644 --- a/server_v2/CMakeLists.txt +++ b/server_v2/CMakeLists.txt @@ -22,10 +22,11 @@ if (UNIX AND ${CMAKE_SYSTEM_PROCESSOR} STREQUAL armv7l) set(LIBRARIES "pigpio" "spdlog::spdlog") set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-psabi") - set(IS_RASPI) + set(IS_RASPI true) endif () if (NOT IS_RASPI) + message("NOT Running on RaspberryPi") set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -gdwarf-3") set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -Wall -Wextra -gdwarf-3") diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index 0ff17f0..499cd98 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -34,9 +34,9 @@ #define ROBOT_GO_TO_DISTANCE_RATE_HZ 200 #define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 -#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 4.0 +#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 10.0 #define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target -#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.025 +#define ROBOT_GO_TO_DISTANCE_MIN_VEL 0.075 #define ROBOT_GO_TO_ANGLE_RATE_HZ 200 #define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp index da32bb9..b8390d1 100644 --- a/server_v2/src/GoToController.cpp +++ b/server_v2/src/GoToController.cpp @@ -12,8 +12,8 @@ // Source: http://faculty.salina.k-state.edu/tim/robot_prog/MobileBot/Steering/unicycle.html#calculating-wheel-velocities // Source: https://www.roboticsbook.org/S52_diffdrive_actions.html#kinematics-in-code void GoToController::diff_drive_inverse_kinematics(double v_m_s, double w_rad_s) { - auto vr_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - auto vl_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + auto vr_rad_s = (2 * v_m_s + w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + auto vl_rad_s = (2 * v_m_s - w_rad_s * ROBOT_ARBOR_LENGTH_M) / (2.0 * ROBOT_WHEEL_RADIUS_M) * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s); ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s); @@ -100,6 +100,11 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) { } } + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 8067bea..4dfb48b 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -10,80 +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 - auto v = (distance_right + distance_left) / 2.0; - auto w = (distance_right - distance_left) / ROBOT_ARBOR_LENGTH_M; + // 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; - 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 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 = v * cos(w); - auto y = v * sin(w); + auto x = v * cos(w); + auto y = v * sin(w); - 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; + 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; - new_theta = mathUtils::wrap_angle_to_pi(new_theta); - current_odometry = Odometry(new_x, new_y, new_theta); + new_theta = mathUtils::wrap_angle_to_pi(new_theta); + current_odometry = Odometry(new_x, new_y, new_theta); -// 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, + v, w); } + } } diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 9948800..91c38f4 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -39,7 +39,7 @@ int main(int argc, char *argv[]) { // GoToGoalController::go_to_point(2, 0, 0.2); // GoToController::drive_distance(-4, 0.2); -// GoToController::turn_degrees(90, M_PI); +// GoToController::turn_degrees(180, M_PI); // while (true) { // this_thread::sleep_for(std::chrono::milliseconds(10)); // ClosedLoopMotorController::getInstance().set_power(0, 100); @@ -50,17 +50,20 @@ int main(int argc, char *argv[]) { // std::this_thread::sleep_for(std::chrono::seconds(2)); // GoToController::drive_distance(-2, 0.3); -// GoToController::drive_distance(2, 0.2); -// GoToController::turn_degrees(-90, M_PI_2); +// while (true) { +// GoToController::drive_distance(0.2, 0.4); +// GoToController::turn_degrees(90, M_PI_2); // -// GoToController::drive_distance(2, 0.2); -// GoToController::turn_degrees(-90, M_PI_2); +// GoToController::drive_distance(0.2, 0.4); +// GoToController::turn_degrees(90, M_PI_2); // -// GoToController::drive_distance(2, 0.2); -// GoToController::turn_degrees(-90, M_PI_2); +// GoToController::drive_distance(0.2, 0.4); +// GoToController::turn_degrees(90, M_PI_2); // -// GoToController::drive_distance(2, 0.2); -// GoToController::turn_degrees(-90, M_PI_2); +// GoToController::drive_distance(0.2, 0.4); +// GoToController::turn_degrees(90, M_PI_2); +// } + // ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(10, stoi(argv[1]));