From d0aab458e5616d83c1693662a847ba5db4297f19 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 13 Mar 2024 13:58:37 +0100 Subject: [PATCH] Differential Drive: Added parameter & logic to set the minium turning speed in the turning state --- .../DifferentialDriveGuidance.cpp | 30 ++++++++++++------- .../DifferentialDriveGuidance.hpp | 5 +++- src/modules/differential_drive/module.yaml | 11 +++++++ 3 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp index df453ff8b5..02c9a2127a 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp @@ -64,7 +64,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), current_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); + _heading_error = matrix::wrap_pi(desired_heading - yaw); if (_current_waypoint != current_waypoint) { _currentState = GuidanceState::TURNING; @@ -78,21 +78,29 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit float desired_speed = 0.f; switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; + case GuidanceState::TURNING: { + desired_speed = 0.f; - if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; + if (fabsf(_heading_error) < 0.05f) { + _currentState = GuidanceState::DRIVING; + } + + float minTurningSpeed = _param_rdd_min_turning_speed.get(); + + // Make sure we do not get stuck while turning as the error gets too small + if (fabsf(_heading_error) < minTurningSpeed) { + _heading_error = (_heading_error > 0) ? minTurningSpeed : -minTurningSpeed; + } + + break; } - break; - case GuidanceState::DRIVING: { const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); _forwards_velocity_smoothing.updateDurations(max_velocity); _forwards_velocity_smoothing.updateTraj(dt); - desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, + desired_speed = math::interpolate(abs(_heading_error), 0.1f, 0.8f, _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); break; } @@ -100,15 +108,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit case GuidanceState::GOAL_REACHED: // temporary till I find a better way to stop the vehicle desired_speed = 0.f; - heading_error = 0.f; + _heading_error = 0.f; angular_velocity = 0.f; _desired_angular_velocity = 0.f; break; } // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. - float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, - dt) + heading_error; + float angular_velocity_pid = pid_calculate(&_heading_p_controller, _heading_error, angular_velocity, 0, + dt) + _heading_error; differential_drive_setpoint_s output{}; output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp index 0a5a29bbca..5a4b8b2483 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp @@ -124,6 +124,8 @@ private: float _max_speed; ///< The maximum speed. float _max_angular_velocity; ///< The maximum angular velocity. + float _heading_error{0.0f}; + matrix::Vector2d _current_waypoint; ///< The current waypoint. VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. @@ -135,6 +137,7 @@ private: (ParamFloat) _param_rdd_p_gain_heading, (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel + (ParamFloat) _param_rdd_max_accel, + (ParamFloat) _param_rdd_min_turning_speed ) }; diff --git a/src/modules/differential_drive/module.yaml b/src/modules/differential_drive/module.yaml index 50ab9bb837..b6baeb8322 100644 --- a/src/modules/differential_drive/module.yaml +++ b/src/modules/differential_drive/module.yaml @@ -120,3 +120,14 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 + RDD_MIN_TRN_VEL: + description: + short: Minimun turning speed + long: Minimum turning speed required to ensure vehicle turns properly, preventing no-turn situations caused by minor heading errors or motor limitations at low speeds. + type: float + unit: rad/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.2