[wpimath] Fix units of RamseteController's b and zeta (#3757)

Fixes #3755.

wpimath/src/main/native/cpp/controller/RamseteController.cpp

@@ -6,6 +6,7 @@
 
 #include <cmath>
 
+#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_unit>{b},
+                        units::unit_t<zeta_unit>{zeta}) {}
+
+RamseteController::RamseteController(units::unit_t<b_unit> b,
+                                     units::unit_t<zeta_unit> zeta)
     : m_b{b}, m_zeta{zeta} {}
 
+RamseteController::RamseteController()
+    : RamseteController(units::unit_t<b_unit>{2.0},
+                        units::unit_t<zeta_unit>{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 +

wpimath/src/main/native/include/frc/controller/RamseteController.h

@@ -5,11 +5,13 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/deprecated.h>
 
 #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::squared<units::radians>,
+                           units::inverse<units::squared<units::meters>>>;
+  using zeta_unit = units::inverse<units::radians>;
+
   /**
    * 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_unit> b, units::unit_t<zeta_unit> 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<b_unit> m_b;
+  units::unit_t<zeta_unit> m_zeta;
 
   Pose2d m_poseError;
   Pose2d m_poseTolerance;