diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h index cdd3a43b1f..174a028a00 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -62,8 +63,8 @@ constexpr auto kMaxAcceleration = 3_mps_sq; // Reasonable baseline values for a RAMSETE follower in units of meters and // seconds -constexpr double kRamseteB = 2; -constexpr double kRamseteZeta = 0.7; +constexpr auto kRamseteB = 2.0 * 1_rad * 1_rad / (1_m * 1_m); +constexpr auto kRamseteZeta = 0.7 / 1_rad; } // namespace AutoConstants namespace OIConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h index 307d40846d..b12eef1bcb 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h @@ -74,8 +74,8 @@ constexpr auto kMaxAcceleration = 3_mps_sq; // Reasonable baseline values for a RAMSETE follower in units of meters and // seconds -constexpr double kRamseteB = 2; -constexpr double kRamseteZeta = 0.7; +constexpr auto kRamseteB = 2 * 1_rad * 1_rad / (1_m * 1_m); +constexpr auto kRamseteZeta = 0.7 / 1_rad; } // namespace AutoConstants namespace OIConstants { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java index 0be592e4a7..a1d6237d6d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java @@ -46,10 +46,10 @@ public class RamseteController { /** * Construct a Ramsete unicycle controller. * - * @param b Tuning parameter (b > 0) for which larger values make convergence more aggressive - * like a proportional term. - * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide more damping - * in response. + * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make convergence more + * aggressive like a proportional term. + * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger values provide + * more damping in response. */ @SuppressWarnings("ParameterName") public RamseteController(double b, double zeta) { @@ -58,8 +58,8 @@ public class RamseteController { } /** - * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7 - * have been well-tested to produce desirable results. + * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m² + * and 0.7 rad⁻¹ have been well-tested to produce desirable results. */ public RamseteController() { this(2.0, 0.7); diff --git a/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/wpimath/src/main/native/cpp/controller/RamseteController.cpp index 8aa16d8311..55c4815fe1 100644 --- a/wpimath/src/main/native/cpp/controller/RamseteController.cpp +++ b/wpimath/src/main/native/cpp/controller/RamseteController.cpp @@ -6,6 +6,7 @@ #include +#include "units/angle.h" #include "units/math.h" using namespace frc; @@ -15,17 +16,26 @@ using namespace frc; * * @param x Value of which to take sinc(x). */ -static double Sinc(double x) { - if (std::abs(x) < 1e-9) { - return 1.0 - 1.0 / 6.0 * x * x; +static decltype(1 / 1_rad) Sinc(units::radian_t x) { + if (units::math::abs(x) < 1e-9_rad) { + return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()}; } else { - return std::sin(x) / x; + return units::math::sin(x) / x; } } RamseteController::RamseteController(double b, double zeta) + : RamseteController(units::unit_t{b}, + units::unit_t{zeta}) {} + +RamseteController::RamseteController(units::unit_t b, + units::unit_t zeta) : m_b{b}, m_zeta{zeta} {} +RamseteController::RamseteController() + : RamseteController(units::unit_t{2.0}, + units::unit_t{0.7}) {} + bool RamseteController::AtReference() const { const auto& eTranslate = m_poseError.Translation(); const auto& eRotate = m_poseError.Rotation(); @@ -51,14 +61,15 @@ ChassisSpeeds RamseteController::Calculate( m_poseError = poseRef.RelativeTo(currentPose); // Aliases for equation readability - double eX = m_poseError.X().value(); - double eY = m_poseError.Y().value(); - double eTheta = m_poseError.Rotation().Radians().value(); - double vRef = linearVelocityRef.value(); - double omegaRef = angularVelocityRef.value(); + const auto& eX = m_poseError.X(); + const auto& eY = m_poseError.Y(); + const auto& eTheta = m_poseError.Rotation().Radians(); + const auto& vRef = linearVelocityRef; + const auto& omegaRef = angularVelocityRef; - double k = - 2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2)); + auto k = 2.0 * m_zeta * + units::math::sqrt(units::math::pow<2>(omegaRef) + + m_b * units::math::pow<2>(vRef)); units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX}; units::radians_per_second_t omega{omegaRef + k * eTheta + diff --git a/wpimath/src/main/native/include/frc/controller/RamseteController.h b/wpimath/src/main/native/include/frc/controller/RamseteController.h index 022fff9fe7..bb23f46f9b 100644 --- a/wpimath/src/main/native/include/frc/controller/RamseteController.h +++ b/wpimath/src/main/native/include/frc/controller/RamseteController.h @@ -5,11 +5,13 @@ #pragma once #include +#include #include "frc/geometry/Pose2d.h" #include "frc/kinematics/ChassisSpeeds.h" #include "frc/trajectory/Trajectory.h" #include "units/angular_velocity.h" +#include "units/length.h" #include "units/velocity.h" namespace frc { @@ -43,22 +45,38 @@ namespace frc { */ class WPILIB_DLLEXPORT RamseteController { public: + using b_unit = + units::compound_unit, + units::inverse>>; + using zeta_unit = units::inverse; + /** * Construct a Ramsete unicycle controller. * - * @param b Tuning parameter (b > 0) for which larger values make + * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make * convergence more aggressive like a proportional term. - * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide - * more damping in response. + * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger + * values provide more damping in response. */ + WPI_DEPRECATED("Use unit-safe constructor instead") RamseteController(double b, double zeta); /** - * Construct a Ramsete unicycle controller. The default arguments for - * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable - * results. + * Construct a Ramsete unicycle controller. + * + * @param b Tuning parameter (b > 0 rad²/m²) for which larger values make + * convergence more aggressive like a proportional term. + * @param zeta Tuning parameter (0 rad⁻¹ < zeta < 1 rad⁻¹) for which larger + * values provide more damping in response. */ - RamseteController() : RamseteController(2.0, 0.7) {} + RamseteController(units::unit_t b, units::unit_t zeta); + + /** + * Construct a Ramsete unicycle controller. The default arguments for + * b and zeta of 2.0 rad²/m² and 0.7 rad⁻¹ have been well-tested to produce + * desirable results. + */ + RamseteController(); /** * Returns true if the pose error is within tolerance of the reference. @@ -109,8 +127,8 @@ class WPILIB_DLLEXPORT RamseteController { void SetEnabled(bool enabled); private: - double m_b; - double m_zeta; + units::unit_t m_b; + units::unit_t m_zeta; Pose2d m_poseError; Pose2d m_poseTolerance; diff --git a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp index 5e297f4294..8829deb5c3 100644 --- a/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -16,7 +16,8 @@ static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi / 180.0}; TEST(RamseteControllerTest, ReachesReference) { - frc::RamseteController controller{2.0, 0.7}; + frc::RamseteController controller{2.0 * 1_rad * 1_rad / (1_m * 1_m), + 0.7 / 1_rad}; frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}}; auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},