diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg index d9bc7b127e..7547fd5be6 100644 --- a/msg/RoverMecanumStatus.msg +++ b/msg/RoverMecanumStatus.msg @@ -1,13 +1,17 @@ uint64 timestamp # time since system start (microseconds) -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output of the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 measured_yaw # [rad] Measured yaw -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller -float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller +float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller +float32 measured_yaw # [rad] Measured yaw +float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller +float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller +float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller +float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller # TOPICS rover_mecanum_status diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 7837d436dc..7c57a1138f 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -66,7 +66,7 @@ static constexpr float YAW_RATE_THRESHOLD = static constexpr float SPEED_THRESHOLD = 0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still static constexpr float STICK_DEADZONE = - 0.3f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value + 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value using namespace time_literals; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index ecb0ea61c2..eb536e2ea4 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -61,9 +61,19 @@ void RoverMecanumControl::updateParams() _pid_lateral_throttle.setOutputLimit(1.f); _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F; _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); _pid_yaw.setIntegralLimit(_max_yaw_rate); _pid_yaw.setOutputLimit(_max_yaw_rate); + + // Update slew rates + if (_max_yaw_rate > FLT_EPSILON) { + _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); + } + + if (_max_yaw_accel > FLT_EPSILON) { + _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); + } } void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -79,98 +89,191 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl // Closed loop yaw control if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { + _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt); + _rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); _pid_yaw.setSetpoint( - matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw)); // error as setpoint to take care of wrapping + matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - + vehicle_yaw)); // error as setpoint to take care of wrapping _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); + _rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; } else { _pid_yaw.resetIntegral(); + _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); } // Yaw rate control float speed_diff_normalized{0.f}; - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward - const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get(); - speed_diff_normalized = math::interpolate(speed_diff, -_param_rm_max_thr_yaw_r.get(), - _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); - } - - _pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint); - speed_diff_normalized = math::constrain(speed_diff_normalized + - _pid_yaw_rate.update(vehicle_yaw_rate, dt), - -1.f, 1.f); // Feedback + if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, false); + _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); } else { // Use normalized setpoint - speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f; + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, + vehicle_yaw_rate, + _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, true); } // Speed control - float forward_throttle{0.f}; - float lateral_throttle{0.f}; + float forward_speed_normalized{0.f}; + float lateral_speed_normalized{0.f}; if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control - // Closed loop forward speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_throttle = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - } + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); + _rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); + _rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState(); - _pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint); - forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt); - // Closed loop lateral speed control - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - lateral_throttle = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - } + } else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) + && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized, + vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); + lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, + vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, + _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - _pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint); - lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt); - - } else { // Use normalized setpoint - forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; - lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain( - _rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f; } // Publish rover mecanum status (logging) - rover_mecanum_status_s rover_mecanum_status{}; - rover_mecanum_status.timestamp = _timestamp; - rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; - rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; - rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; - rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; - rover_mecanum_status.measured_yaw = vehicle_yaw; - rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); - rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); - rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); - _rover_mecanum_status_pub.publish(rover_mecanum_status); + _rover_mecanum_status.timestamp = _timestamp; + _rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; + _rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; + _rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; + _rover_mecanum_status.measured_yaw = vehicle_yaw; + _rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); + _rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); + _rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); + _rover_mecanum_status_pub.publish(_rover_mecanum_status); // Publish to motors actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_throttle, lateral_throttle, + computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); } -matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle, +float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, + const float max_thr_yaw_r, + const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, + PID &pid_yaw_rate, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; + } + + if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); + yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); + + } else { + yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (normalized) { + speed_diff_normalized = yaw_rate_with_accel_limit.getState(); + + } else { + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + _pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); + speed_diff_normalized = math::constrain(speed_diff_normalized + + _pid_yaw_rate.update(vehicle_yaw_rate, dt), + -1.f, 1.f); // Feedback + + + } + + return math::constrain(speed_diff_normalized, -1.f, 1.f); + +} + +float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint, + const float vehicle_speed, const float max_thr_spd, SlewRate &speed_setpoint_with_accel_limit, + PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) { + if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); + + } else { + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + + } + + } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); + speed_setpoint_with_accel_limit.update(speed_setpoint, dt); + + } else { + speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + if (normalized) { + forward_speed_normalized = speed_setpoint_with_accel_limit.getState(); + + } else { // Closed loop speed control + + if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward + forward_speed_normalized = math::interpolate(speed_setpoint_with_accel_limit.getState(), + -max_thr_spd, max_thr_spd, + -1.f, 1.f); + } + + pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState()); + forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback + + } + + return math::constrain(forward_speed_normalized, -1.f, 1.f); + +} + +matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized, + float lateral_speed_normalized, float speed_diff) { // Prioritize ratio between forward and lateral speed over either magnitude - float combined_speed = fabsf(forward_throttle) + fabsf(lateral_throttle); + float combined_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized); if (combined_speed > 1.f) { - forward_throttle /= combined_speed; - lateral_throttle /= combined_speed; + forward_speed_normalized /= combined_speed; + lateral_speed_normalized /= combined_speed; combined_speed = 1.f; } @@ -179,20 +282,21 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr if (total_speed > 1.f) { const float excess_velocity = fabsf(total_speed - 1.f); - const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity; - const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity; + const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity; + const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity; - if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) { - forward_throttle = forward_throttle_temp; - lateral_throttle = lateral_throttle_temp; + if (sign(forward_throttle_temp) == sign(forward_speed_normalized) + && sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) { + forward_speed_normalized = forward_throttle_temp; + lateral_speed_normalized = lateral_throttle_temp; } else { - forward_throttle = lateral_throttle = 0.f; + forward_speed_normalized = lateral_speed_normalized = 0.f; } } // Calculate motor commands - const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff}; + const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff}; const Matrix input(input_data); const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; const Matrix m(m_data); diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp index 42cec627a0..53ae266383 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp @@ -43,7 +43,8 @@ #include #include #include - +#include +#include // Standard libraries #include @@ -89,6 +90,39 @@ protected: void updateParams() override; private: + /** + * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. + * @param vehicle_yaw_rate Measured yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. + * @param wheel_track Wheel track [m]. + * @param dt Time since last update [s]. + * @param yaw_rate_with_accel_limit Yaw rate slew rate. + * @param pid_yaw_rate Yaw rate PID + * @param normalized Indicates if the forward speed setpoint is already normalized. + * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); + /** + * @brief Compute normalized speed setpoint and apply slew rates. + * to the speed setpoint and doing closed loop speed control if enabled. + * @param speed_setpoint Speed setpoint [m/s]. + * @param vehicle_speed Actual speed [m/s]. + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. + * @param speed_setpoint_with_accel_limit Speed slew rate. + * @param pid_throttle Throttle PID + * @param max_accel Maximum acceleration [m/s^2] + * @param max_decel Maximum deceleration [m/s^2] + * @param dt Time since last update [s]. + * @param normalized Indicates if the speed setpoint is already normalized. + * @return Normalized speed setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd, + SlewRate &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, + float dt, bool normalized); + /** * @brief Turn normalized speed setpoints into normalized motor commands. * @@ -105,30 +139,39 @@ private: // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; + rover_mecanum_status_s _rover_mecanum_status{}; // Variables rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; // Controllers PID _pid_forward_throttle; // PID for the closed loop forward speed control PID _pid_lateral_throttle; // PID for the closed loop lateral speed control PID _pid_yaw; // PID for the closed loop yaw control PID _pid_yaw_rate; // PID for the closed loop yaw rate control + SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; + SlewRate _lateral_speed_setpoint_with_accel_limit{0.f}; + SlewRate _yaw_rate_with_accel_limit{0.f}; + SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_spd, + (ParamFloat) _param_rm_max_accel, + (ParamFloat) _param_rm_max_decel, (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, - (ParamInt) _param_r_rev + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_rm_max_yaw_accel, + (ParamFloat) _param_rm_yaw_rate_p, + (ParamFloat) _param_rm_yaw_rate_i, + (ParamFloat) _param_rm_p_gain_speed, + (ParamFloat) _param_rm_i_gain_speed, + (ParamFloat) _param_rm_p_gain_yaw, + (ParamFloat) _param_rm_i_gain_yaw, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index fa1bd01e4b..bccbe3e31f 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -29,6 +29,22 @@ parameters: decimal: 2 default: 90 + RM_MAX_YAW_ACCEL: + description: + short: Maximum allowed yaw acceleration for the rover + long: | + This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints + to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. + This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 + RM_MAX_THR_YAW_R: description: short: Yaw rate turning left/right wheels at max speed in opposite directions @@ -160,6 +176,21 @@ parameters: decimal: 2 default: 0.5 + RM_MAX_DECEL: + description: + short: Maximum deceleration + long: | + Maximum decelaration is used to limit the deceleration of the rover. + Set to -1 to disable, causing the rover to decelerate as fast as possible. + Caution: This disables the slow down effect in auto modes. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + RM_MISS_VEL_GAIN: description: short: Tuning parameter for the velocity reduction during waypoint transition