From 4ee0b8045fb1e09bd0d48d8c893609cbae996820 Mon Sep 17 00:00:00 2001 From: Konstantin Lampalzer Date: Sun, 29 May 2022 15:20:11 +0200 Subject: [PATCH] Tune turning a bit --- server_v2/include/Robot.hpp | 10 +++++++--- server_v2/include/mathUtils.hpp | 17 +++++++++++++++++ server_v2/src/GoToController.cpp | 19 +++++++++++-------- server_v2/src/OdometryController.cpp | 8 ++++---- server_v2/src/main.cpp | 6 +++--- 5 files changed, 42 insertions(+), 18 deletions(-) diff --git a/server_v2/include/Robot.hpp b/server_v2/include/Robot.hpp index 7e94738..ed1fd88 100644 --- a/server_v2/include/Robot.hpp +++ b/server_v2/include/Robot.hpp @@ -23,17 +23,21 @@ #define ROBOT_WHEEL_RADIUS_MM 35.5 #define ROBOT_WHEEL_RADIUS_M (ROBOT_WHEEL_RADIUS_MM / 1000.0) -#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2 * ROBOT_WHEEL_RADIUS_MM * M_PI) +#define ROBOT_WHEEL_CIRCUMFERENCE_MM (2.0 * ROBOT_WHEEL_RADIUS_MM * M_PI) #define ROBOT_WHEEL_CIRCUMFERENCE_M (2 * ROBOT_WHEEL_RADIUS_M * M_PI) #define ROBOT_TICKS_PER_TURN (27.7 * 100.0) -#define ROBOT_ARBOR_LENGTH_MM 139.0 +#define ROBOT_ARBOR_LENGTH_MM 140.0 #define ROBOT_ARBOR_LENGTH_M (ROBOT_ARBOR_LENGTH_MM / 1000.0) #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 + +#define ROBOT_GO_TO_ANGLE_END_RAD (0.1 * (M_PI / 180.0)) // stop 0.1 deg before target +#define ROBOT_GO_TO_ANGLE_ALPHA_P 0.5 +#define ROBOT_GO_TO_ANGLE_MIN_VEL (M_PI / 3.0) #endif //COMPLIB_SERVER_ROBOT_HPP diff --git a/server_v2/include/mathUtils.hpp b/server_v2/include/mathUtils.hpp index 9e16e4b..897fb20 100644 --- a/server_v2/include/mathUtils.hpp +++ b/server_v2/include/mathUtils.hpp @@ -40,6 +40,23 @@ namespace mathUtils { inline T wrap_angle_to_pi(T angle) { return atan2(sin(angle), cos(angle)); } + + template + inline T limit_min(T val, T min) { + if (val < 0.0 && val > -min) { + return -min; + } else if (val > 0.0 && val < min) { + return min; + } else if (val == 0) { + return min; + } + return val; + } + + template + inline T limit_max(T val, T min, T max) { + return fmax(min, fmin(max, val)); + } } #endif // COMPLIB_SERVER_MATHUTILS_HPP \ No newline at end of file diff --git a/server_v2/src/GoToController.cpp b/server_v2/src/GoToController.cpp index 123c679..53c8e8d 100644 --- a/server_v2/src/GoToController.cpp +++ b/server_v2/src/GoToController.cpp @@ -105,12 +105,12 @@ void GoToController::drive_distance(double distance_m, double v_m_s) { } void GoToController::turn_degrees(double angle_deg, double v_rad_s) { - if (angle_deg < 0 && v_rad_s > 0) { + auto angle_rad = mathUtils::wrap_angle_to_pi(angle_deg * (M_PI / 180.0)); + + if (angle_rad < 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(); @@ -118,19 +118,22 @@ void GoToController::turn_degrees(double angle_deg, double v_rad_s) { 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) { + auto angle_turned = 0.0; + while (fabs(angle_to_target) > ROBOT_GO_TO_ANGLE_END_RAD && angle_turned < angle_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_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); + auto speed_mult = mathUtils::limit_max(fabs(angle_to_target) * ROBOT_GO_TO_ANGLE_ALPHA_P, 0.0, 1.0); + auto adjusted_speed = mathUtils::limit_min(v_rad_s * speed_mult, ROBOT_GO_TO_ANGLE_MIN_VEL); + diff_drive_inverse_kinematics(0.0, adjusted_speed); - spdlog::debug("{:03.4f}; {:03.4f}; {:03.4f}; {:03.4f}; {:03.4f};", + spdlog::debug("{: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(), - angle_turned, angle_to_target + angle_turned, angle_to_target, adjusted_speed ); } diff --git a/server_v2/src/OdometryController.cpp b/server_v2/src/OdometryController.cpp index 52c532e..38932cf 100644 --- a/server_v2/src/OdometryController.cpp +++ b/server_v2/src/OdometryController.cpp @@ -80,10 +80,10 @@ OdometryController::OdometryController() { } current_odometry = Odometry(new_x_position, new_y_position, new_angular_orientation); -// spdlog::info("{} {} {} {} {} {}", -// current_position_left, current_position_right, -// distance_left, distance_right, -// distance, theta); + spdlog::info("{} {} {} {} {} {}", + current_position_left, current_position_right, + distance_left, distance_right, + distance, theta); } } } diff --git a/server_v2/src/main.cpp b/server_v2/src/main.cpp index 0da449e..d64c710 100644 --- a/server_v2/src/main.cpp +++ b/server_v2/src/main.cpp @@ -38,7 +38,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_2); + GoToController::turn_degrees(180, M_PI); // GoToController::drive_distance(2, 0.2); // GoToController::turn_degrees(-90, M_PI_2); @@ -52,10 +52,10 @@ int main(int argc, char *argv[]) { // GoToController::drive_distance(2, 0.2); // GoToController::turn_degrees(-90, M_PI_2); - ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1])); +// ClosedLoopMotorController::getInstance().calibrate_wheel_ticks(5, stoi(argv[1])); // ClosedLoopMotorController::getInstance().generate_step_response_data(); // ClosedLoopMotorController::getInstance().generate_tuned_step_response_data(); -// std::this_thread::sleep_for(std::chrono::hours(12)); + std::this_thread::sleep_for(std::chrono::hours(12)); return 0; }