diff --git a/server_v2/CMakeLists.txt b/server_v2/CMakeLists.txt index f26d0d7..186acf1 100644 --- a/server_v2/CMakeLists.txt +++ b/server_v2/CMakeLists.txt @@ -50,7 +50,7 @@ set(SRC_FILES src/communication/MessageBuilder.cpp src/communication/UnixSocketServer.cpp src/communication/TCPSocketServer.cpp - src/GoToGoalController.cpp) + src/GoToController.cpp) set(HDR_FILES include/spi.hpp @@ -69,7 +69,7 @@ set(HDR_FILES include/communication/MessageBuilder.hpp include/communication/UnixSocketServer.hpp include/communication/TCPSocketServer.hpp - include/GoToGoalController.hpp) + include/GoToController.hpp) include_directories(third_party/asio) diff --git a/server_v2/include/GoToController.hpp b/server_v2/include/GoToController.hpp new file mode 100644 index 0000000..4eb7236 --- /dev/null +++ b/server_v2/include/GoToController.hpp @@ -0,0 +1,33 @@ +// +// Created by KonstantinViesure on 26.05.22. +// + +#ifndef COMPLIB_SERVER_GOTOCONTROLLER_HPP +#define COMPLIB_SERVER_GOTOCONTROLLER_HPP + + +class GoToController { +public: + static GoToController &getInstance() { + static GoToController instance; + return instance; + } + + GoToController(GoToController const &) = delete; + + void operator=(GoToController const &) = delete; + + static void drive_distance(double distance_m, double v_ms); + + static void turn_degrees(double angle_deg, double v_rad_s); + + static void go_to_point(double goal_x, double goal_y, double v_ms); + + static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s); + +private: + GoToController() = default; +}; + + +#endif //COMPLIB_SERVER_GOTOCONTROLLER_HPP diff --git a/server_v2/include/GoToGoalController.hpp b/server_v2/include/GoToGoalController.hpp deleted file mode 100644 index 33ec68d..0000000 --- a/server_v2/include/GoToGoalController.hpp +++ /dev/null @@ -1,29 +0,0 @@ -// -// Created by KonstantinViesure on 26.05.22. -// - -#ifndef COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP -#define COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP - - -class GoToGoalController { -public: - static GoToGoalController &getInstance() { - static GoToGoalController instance; - return instance; - } - - GoToGoalController(GoToGoalController const &) = delete; - - void operator=(GoToGoalController const &) = delete; - - static void go_to_point(double goal_x, double goal_y, double v_ms); - - static void diff_drive_inverse_kinematics(double v_m_s, double w_rad_s); - -private: - GoToGoalController() = default; -}; - - -#endif //COMPLIB_SERVER_GOTOGOALCONTROLLER_HPP diff --git a/server_v2/include/OdometryController.hpp b/server_v2/include/OdometryController.hpp index 570a603..f588cae 100644 --- a/server_v2/include/OdometryController.hpp +++ b/server_v2/include/OdometryController.hpp @@ -3,6 +3,7 @@ #include +#include #include #include "include/Odometry.hpp" @@ -34,6 +35,7 @@ private: OdometryController(); std::thread odometry_thread; + std::recursive_mutex odometry_mutex; [[noreturn]] void odometry_loop(); diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index 11f8f6e..7e94738 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -7,9 +7,9 @@ #define ROBOT_IR_RATE_HZ 250 #define ROBOT_MOTOR_COUNT 4 -#define ROBOT_MOTOR_SPEED_CONTROL_KP 2.5 -#define ROBOT_MOTOR_SPEED_CONTROL_KI 19.0 -#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.07 +#define ROBOT_MOTOR_SPEED_CONTROL_KP 8.5 +#define ROBOT_MOTOR_SPEED_CONTROL_KI 31.0 +#define ROBOT_MOTOR_SPEED_CONTROL_KD 0.085 #define ROBOT_MOTOR_SPEED_CONTROL_RAMP 4 * M_PI #define ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ 250 @@ -31,5 +31,9 @@ #define ROBOT_TICKS_PER_METER (1000.0 / ROBOT_WHEEL_CIRCUMFERENCE_MM * ROBOT_TICKS_PER_TURN) #define ROBOT_GO_TO_GOAL_K 0.99 // Between 0.5 and 1 +#define ROBOT_GO_TO_DISTANCE_ALPHA_P 2.0 +#define ROBOT_GO_TO_DISTANCE_VELOCITY_P 5.0 +#define ROBOT_GO_TO_DISTANCE_END_M 0.01 // stop approx 1 cm before target +#define ROBOT_GO_TO_ANGLE_END_RAD (0.5 * (M_PI / 180.0)) // stop 0.5 deg before target #endif //COMPLIB_SERVER_ROBOT_HPP diff --git a/server_v2/src/ClosedLoopMotorController.cpp b/server_v2/src/ClosedLoopMotorController.cpp index cc2ed36..a2ef426 100644 --- a/server_v2/src/ClosedLoopMotorController.cpp +++ b/server_v2/src/ClosedLoopMotorController.cpp @@ -54,84 +54,98 @@ void ClosedLoopMotorController::generate_step_response_data() { auto console = spdlog::stdout_color_mt("generate_step_response_data"); console->set_pattern("%v"); - auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT; - auto input = 10.0; + auto input = 25.0; - speed_targets.at(port) = input; - pids.at(port) = PID( + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID( 1.0, 0.0, 0.0, 1000.0, 100.0); - control_modes.at(port) = ControlMode::SPEED; + pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID( + 1.0, 0.0, 0.0, + 1000.0, 100.0); + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED; - std::this_thread::sleep_for(std::chrono::seconds(2)); + std::this_thread::sleep_for(std::chrono::seconds(1)); auto start_time = std::chrono::steady_clock::now(); - - for (int i = 0; i < 500; ++i) { + for (int i = 0; i < 150; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(port); + auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } - input = 20.0; - speed_targets.at(port) = input; - for (int i = 0; i < 500; ++i) { + input = 35.0; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + for (int i = 0; i < 150; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(port); + auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } - speed_targets.at(port) = 0.0; - control_modes.at(port) = ControlMode::NONE; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE; } void ClosedLoopMotorController::generate_tuned_step_response_data() { auto console = spdlog::stdout_color_mt("generate_tuned_step_response_data"); console->set_pattern("%v"); - auto port = ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT; auto input = 2 * M_PI; - speed_targets.at(port) = input; - pids.at(port) = PID( - 2.5, 19.0, 0.07, + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + pids.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = PID( + 8.5, 31.0, 0.085, 4 * M_PI, 100.0); - control_modes.at(port) = ControlMode::SPEED; + pids.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = PID( + 8.5, 31.0, 0.085, + 4 * M_PI, 100.0); + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::SPEED; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::SPEED; std::this_thread::sleep_for(std::chrono::seconds(2)); auto start_time = std::chrono::steady_clock::now(); - for (int i = 0; i < 500; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(port); + auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } input = 4 * M_PI; - speed_targets.at(port) = input; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; for (int i = 0; i < 500; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(port); + auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); } input = M_PI; - speed_targets.at(port) = input; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; for (int i = 0; i < 500; ++i) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); double delta_seconds = std::chrono::duration_cast>(std::chrono::steady_clock::now() - start_time).count() / US_IN_S; - auto output = Encoders::getInstance().get_velocities_rad_s().at(port); + auto output = Encoders::getInstance().get_velocities_rad_s().at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT); console->debug("{:03.4f}; {:03.4f}; {:03.4f}", delta_seconds, input, output); input += 0.01; - speed_targets.at(port) = input; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_LEFT_MULT; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = input * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; } - speed_targets.at(port) = 0.0; - control_modes.at(port) = ControlMode::NONE; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = 0; + speed_targets.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = 0; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT) = ControlMode::NONE; + control_modes.at(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT) = ControlMode::NONE; } diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp new file mode 100644 index 0000000..123c679 --- /dev/null +++ b/server_v2/src/GoToController.cpp @@ -0,0 +1,143 @@ +#include + +#include "include/GoToController.hpp" +#include "include/Robot.hpp" +#include "include/ClosedLoopMotorController.hpp" +#include "include/OdometryController.hpp" +#include "include/mathUtils.hpp" + +#define MS_IN_S 1000 +#define US_IN_S (1000 * MS_IN_S) + +// 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; + + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s); + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s); +} + +void GoToController::go_to_point(double goal_x, double goal_y, double v_ms) { + auto was_enabled = OdometryController::getInstance().is_enabled(); + OdometryController::getInstance().enable(); + auto start_pos = OdometryController::getInstance().get(); + + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ); + + auto abs_distance = fabs(sqrt( + pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2) + )); + while (abs_distance > 0.05) { + auto current_pos = OdometryController::getInstance().get(); + auto alpha = mathUtils::wrap_angle_to_pi( + (atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2)) + ); + + auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha)); + auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha)); + auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; + auto vl_rad_s = vl_ms / 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); + + abs_distance = fabs(sqrt( + pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2) + )); + spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}", + current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(), + goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(), + current_pos.get_angular_orientation() + M_PI_2, atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()), alpha, + vr_rad_s, vl_rad_s); + + std::this_thread::sleep_for(sleep_duration); + } + + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); + ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); + + if (!was_enabled) { + OdometryController::getInstance().disable(); + } +} + +void GoToController::drive_distance(double distance_m, double v_m_s) { + if (distance_m < 0 && v_m_s > 0) { + distance_m = fabs(distance_m); + v_m_s *= -1; + } + + auto was_enabled = OdometryController::getInstance().is_enabled(); + OdometryController::getInstance().enable(); + auto start_pos = OdometryController::getInstance().get(); + + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ); + + auto abs_distance_to_target = distance_m; + while (abs_distance_to_target > ROBOT_GO_TO_DISTANCE_END_M) { + std::this_thread::sleep_for(sleep_duration); + + auto current_pos = OdometryController::getInstance().get(); + + auto distance_driven = fabs(current_pos.get_x_position() - start_pos.get_x_position()); + abs_distance_to_target = fabs(distance_m - distance_driven); + + auto alpha = -mathUtils::wrap_angle_to_pi(current_pos.get_angular_orientation()); + + auto vel_mult = fmax(0, fmin(1, abs_distance_to_target * ROBOT_GO_TO_DISTANCE_VELOCITY_P)); + diff_drive_inverse_kinematics(v_m_s * vel_mult, alpha * ROBOT_GO_TO_DISTANCE_ALPHA_P); + + spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}", + current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(), + distance_driven, abs_distance_to_target, + alpha, vel_mult + ); + } + + ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); + ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); + + if (!was_enabled) { + OdometryController::getInstance().disable(); + } +} + +void GoToController::turn_degrees(double angle_deg, double v_rad_s) { + if (angle_deg < 0 && v_rad_s > 0) { + v_rad_s *= -1; + } + + auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0)); + + auto was_enabled = OdometryController::getInstance().is_enabled(); + OdometryController::getInstance().enable(); + auto start_pos = OdometryController::getInstance().get(); + + auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ); + + auto angle_to_target = angle_rad; + while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD) { + std::this_thread::sleep_for(sleep_duration); + + auto current_pos = OdometryController::getInstance().get(); + + auto angle_turned = current_pos.get_angular_orientation() - start_pos.get_angular_orientation(); + angle_to_target = mathUtils::wrap_angle_to_pi(angle_rad - angle_turned); + + diff_drive_inverse_kinematics(0.0, v_rad_s); + + spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};", + current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(), + angle_turned, angle_to_target + ); + } + + ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); + ClosedLoopMotorController::getInstance().set_power(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); + + if (!was_enabled) { + OdometryController::getInstance().disable(); + } +} diff --git a/server_v2/src/GoToGoalController.cpp b/server_v2/src/GoToGoalController.cpp deleted file mode 100644 index 3d6aafb..0000000 --- a/server_v2/src/GoToGoalController.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include - -#include "include/GoToGoalController.hpp" -#include "include/Robot.hpp" -#include "include/ClosedLoopMotorController.hpp" -#include "include/OdometryController.hpp" -#include "include/mathUtils.hpp" - -#define MS_IN_S 1000 -#define US_IN_S (1000 * MS_IN_S) - -// 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 GoToGoalController::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; - - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, vr_rad_s); - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, vl_rad_s); -} - -void GoToGoalController::go_to_point(double goal_x, double goal_y, double v_ms) { - auto was_enabled = OdometryController::getInstance().is_enabled(); - OdometryController::getInstance().enable(); - auto start_pos = OdometryController::getInstance().get(); - - auto sleep_duration = std::chrono::microseconds(US_IN_S / ROBOT_MOTOR_SPEED_CONTROL_RATE_HZ); - - auto abs_distance = fabs(sqrt( - pow(goal_x - start_pos.get_x_position(), 2) + pow(goal_y - start_pos.get_y_position(), 2) - )); - while (abs_distance > 0.05) { - auto current_pos = OdometryController::getInstance().get(); - auto alpha = mathUtils::wrap_angle_to_pi( - (atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()) - (current_pos.get_angular_orientation() + M_PI_2)) - ); - - auto vr_ms = v_ms * (cos(alpha) - ROBOT_GO_TO_GOAL_K * sin(alpha)); - auto vl_ms = v_ms * (cos(alpha) + ROBOT_GO_TO_GOAL_K * sin(alpha)); - auto vr_rad_s = vr_ms / ROBOT_WHEEL_RADIUS_M * ROBOT_ODOMETRY_CONTROLLER_RIGHT_MULT; - auto vl_rad_s = vl_ms / 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); - - abs_distance = fabs(sqrt( - pow(goal_x - current_pos.get_x_position(), 2) + pow(goal_y - current_pos.get_y_position(), 2) - )); - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}", - current_pos.get_x_position(), current_pos.get_y_position(), current_pos.get_angular_orientation(), - goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position(), atan2(goal_x - current_pos.get_x_position(), goal_y - current_pos.get_y_position()), - current_pos.get_angular_orientation() + M_PI_2, alpha, - vr_rad_s, vl_rad_s); - - std::this_thread::sleep_for(sleep_duration); - } - - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_RIGHT_PORT, 0); - ClosedLoopMotorController::getInstance().set_speed(ROBOT_ODOMETRY_CONTROLLER_LEFT_PORT, 0); - - if (!was_enabled) { - OdometryController::getInstance().disable(); - } -} diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 379401c..52c532e 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -19,14 +19,14 @@ void OdometryController::enable() { } void OdometryController::disable() { + spdlog::debug("OdometryController::disable()"); enabled = false; OdometryController::reset(); } void OdometryController::reset() { + std::lock_guard lock(odometry_mutex); current_odometry = Odometry(); - last_position_left = 0; - last_position_right = 0; last_run = clock::now(); } @@ -44,7 +44,7 @@ OdometryController::OdometryController() { 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(); diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 104e8d4..7eeca54 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -6,7 +6,7 @@ #include "include/ClosedLoopMotorController.hpp" #include "include/communication/UnixSocketServer.hpp" #include "include/communication/TCPSocketServer.hpp" -#include "include/GoToGoalController.hpp" +#include "include/GoToController.hpp" using namespace std; @@ -36,7 +36,21 @@ int main() { // spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation()); // } - GoToGoalController::go_to_point(2, 0, 0.2); +// GoToGoalController::go_to_point(2, 0, 0.2); +// GoToController::drive_distance(-4, 0.2); +// GoToController::turn_degrees(-90, M_PI_2); + + GoToController::drive_distance(2, 0.2); + GoToController::turn_degrees(-90, M_PI_2); + + GoToController::drive_distance(2, 0.2); + GoToController::turn_degrees(-90, M_PI_2); + + GoToController::drive_distance(2, 0.2); + GoToController::turn_degrees(-90, M_PI_2); + + GoToController::drive_distance(2, 0.2); + GoToController::turn_degrees(-90, M_PI_2); // ClosedLoopMotorController::getInstance().generate_step_response_data(); // ClosedLoopMotorController::getInstance().generate_tuned_step_response_data();