[wpimath] Move controller from wpilibj to wpimath (#3439)

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

@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/HolonomicDriveController.h"
+
+#include <utility>
+
+#include "units/angular_velocity.h"
+
+using namespace frc;
+
+HolonomicDriveController::HolonomicDriveController(
+    frc2::PIDController xController, frc2::PIDController yController,
+    ProfiledPIDController<units::radian> thetaController)
+    : m_xController(std::move(xController)),
+      m_yController(std::move(yController)),
+      m_thetaController(std::move(thetaController)) {}
+
+bool HolonomicDriveController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_rotationError;
+  const auto& tolTranslate = m_poseTolerance.Translation();
+  const auto& tolRotate = m_poseTolerance.Rotation();
+  return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+         units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+         units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
+  m_poseTolerance = tolerance;
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Pose2d& poseRef,
+    units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+  // If this is the first run, then we need to reset the theta controller to the
+  // current pose's heading.
+  if (m_firstRun) {
+    m_thetaController.Reset(currentPose.Rotation().Radians());
+    m_firstRun = false;
+  }
+
+  // Calculate feedforward velocities (field-relative)
+  auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
+  auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
+  auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
+      currentPose.Rotation().Radians(), angleRef.Radians()));
+
+  m_poseError = poseRef.RelativeTo(currentPose);
+  m_rotationError = angleRef - currentPose.Rotation();
+
+  if (!m_enabled) {
+    return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
+                                                  currentPose.Rotation());
+  }
+
+  // Calculate feedback velocities (based on position error).
+  auto xFeedback = units::meters_per_second_t(m_xController.Calculate(
+      currentPose.X().to<double>(), poseRef.X().to<double>()));
+  auto yFeedback = units::meters_per_second_t(m_yController.Calculate(
+      currentPose.Y().to<double>(), poseRef.Y().to<double>()));
+
+  // Return next output.
+  return ChassisSpeeds::FromFieldRelativeSpeeds(
+      xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState,
+    const Rotation2d& angleRef) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   angleRef);
+}
+
+void HolonomicDriveController::SetEnabled(bool enabled) {
+  m_enabled = enabled;
+}

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

@@ -0,0 +1,175 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/PIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/MathUtil.h"
+#include "wpimath/MathShared.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd,
+                             units::second_t period)
+    : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  if (period <= 0_s) {
+    wpi::math::MathSharedStore::ReportError(
+        "Controller period must be a non-zero positive number, got {}!",
+        period.to<double>());
+    m_period = 20_ms;
+    wpi::math::MathSharedStore::ReportWarning(
+        "{}", "Controller period defaulted to 20ms.");
+  }
+  static int instances = 0;
+  instances++;
+
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kController_PIDController2, instances);
+  wpi::SendableRegistry::Add(this, "PIDController", instances);
+}
+
+void PIDController::SetPID(double Kp, double Ki, double Kd) {
+  m_Kp = Kp;
+  m_Ki = Ki;
+  m_Kd = Kd;
+}
+
+void PIDController::SetP(double Kp) {
+  m_Kp = Kp;
+}
+
+void PIDController::SetI(double Ki) {
+  m_Ki = Ki;
+}
+
+void PIDController::SetD(double Kd) {
+  m_Kd = Kd;
+}
+
+double PIDController::GetP() const {
+  return m_Kp;
+}
+
+double PIDController::GetI() const {
+  return m_Ki;
+}
+
+double PIDController::GetD() const {
+  return m_Kd;
+}
+
+units::second_t PIDController::GetPeriod() const {
+  return units::second_t(m_period);
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+  m_setpoint = setpoint;
+}
+
+double PIDController::GetSetpoint() const {
+  return m_setpoint;
+}
+
+bool PIDController::AtSetpoint() const {
+  double positionError;
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    positionError = m_setpoint - m_measurement;
+  }
+
+  double velocityError = (positionError - m_prevError) / m_period.to<double>();
+
+  return std::abs(positionError) < m_positionTolerance &&
+         std::abs(velocityError) < m_velocityTolerance;
+}
+
+void PIDController::EnableContinuousInput(double minimumInput,
+                                          double maximumInput) {
+  m_continuous = true;
+  m_minimumInput = minimumInput;
+  m_maximumInput = maximumInput;
+}
+
+void PIDController::DisableContinuousInput() {
+  m_continuous = false;
+}
+
+bool PIDController::IsContinuousInputEnabled() const {
+  return m_continuous;
+}
+
+void PIDController::SetIntegratorRange(double minimumIntegral,
+                                       double maximumIntegral) {
+  m_minimumIntegral = minimumIntegral;
+  m_maximumIntegral = maximumIntegral;
+}
+
+void PIDController::SetTolerance(double positionTolerance,
+                                 double velocityTolerance) {
+  m_positionTolerance = positionTolerance;
+  m_velocityTolerance = velocityTolerance;
+}
+
+double PIDController::GetPositionError() const {
+  return m_positionError;
+}
+
+double PIDController::GetVelocityError() const {
+  return m_velocityError;
+}
+
+double PIDController::Calculate(double measurement) {
+  m_measurement = measurement;
+  m_prevError = m_positionError;
+
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    m_positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    m_positionError = m_setpoint - measurement;
+  }
+
+  m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
+
+  if (m_Ki != 0) {
+    m_totalError =
+        std::clamp(m_totalError + m_positionError * m_period.to<double>(),
+                   m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+  }
+
+  return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+  // Set setpoint to provided value
+  SetSetpoint(setpoint);
+  return Calculate(measurement);
+}
+
+void PIDController::Reset() {
+  m_prevError = 0;
+  m_totalError = 0;
+}
+
+void PIDController::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PIDController");
+  builder.AddDoubleProperty(
+      "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+  builder.AddDoubleProperty(
+      "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+  builder.AddDoubleProperty(
+      "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+  builder.AddDoubleProperty(
+      "setpoint", [this] { return GetSetpoint(); },
+      [this](double value) { SetSetpoint(value); });
+}

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

@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/ProfiledPIDController.h"
+
+void frc::detail::ReportProfiledPIDController() {
+  static int instances = 0;
+  ++instances;
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+}

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

@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/RamseteController.h"
+
+#include <cmath>
+
+#include "units/math.h"
+
+using namespace frc;
+
+/**
+ * Returns sin(x) / x.
+ *
+ * @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;
+  } else {
+    return std::sin(x) / x;
+  }
+}
+
+RamseteController::RamseteController(double b, double zeta)
+    : m_b{b}, m_zeta{zeta} {}
+
+bool RamseteController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_poseError.Rotation();
+  const auto& tolTranslate = m_poseTolerance.Translation();
+  const auto& tolRotate = m_poseTolerance.Rotation();
+  return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+         units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+         units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
+  m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds RamseteController::Calculate(
+    const Pose2d& currentPose, const Pose2d& poseRef,
+    units::meters_per_second_t linearVelocityRef,
+    units::radians_per_second_t angularVelocityRef) {
+  if (!m_enabled) {
+    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+  }
+
+  m_poseError = poseRef.RelativeTo(currentPose);
+
+  // Aliases for equation readability
+  double eX = m_poseError.X().to<double>();
+  double eY = m_poseError.Y().to<double>();
+  double eTheta = m_poseError.Rotation().Radians().to<double>();
+  double vRef = linearVelocityRef.to<double>();
+  double omegaRef = angularVelocityRef.to<double>();
+
+  double k =
+      2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+
+  units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
+  units::radians_per_second_t omega{omegaRef + k * eTheta +
+                                    m_b * vRef * Sinc(eTheta) * eY};
+  return ChassisSpeeds{v, 0_mps, omega};
+}
+
+ChassisSpeeds RamseteController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   desiredState.velocity * desiredState.curvature);
+}
+
+void RamseteController::SetEnabled(bool enabled) {
+  m_enabled = enabled;
+}

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

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/controller/PIDController.h"
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angle.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * This holonomic drive controller can be used to follow trajectories using a
+ * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory
+ * following is a much simpler problem to solve compared to skid-steer style
+ * drivetrains because it is possible to individually control forward, sideways,
+ * and angular velocity.
+ *
+ * The holonomic drive controller takes in one PID controller for each
+ * direction, forward and sideways, and one profiled PID controller for the
+ * angular direction. Because the heading dynamics are decoupled from
+ * translations, users can specify a custom heading that the drivetrain should
+ * point toward. This heading reference is profiled for smoothness.
+ */
+class HolonomicDriveController {
+ public:
+  /**
+   * Constructs a holonomic drive controller.
+   *
+   * @param xController     A PID Controller to respond to error in the
+   * field-relative x direction.
+   * @param yController     A PID Controller to respond to error in the
+   * field-relative y direction.
+   * @param thetaController A profiled PID controller to respond to error in
+   * angle.
+   */
+  HolonomicDriveController(
+      frc2::PIDController xController, frc2::PIDController yController,
+      ProfiledPIDController<units::radian> thetaController);
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& tolerance);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angleRef           The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   * @param angleRef     The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes. When
+   * Calculate() is called on a disabled controller, only feedforward values
+   * are returned.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  Pose2d m_poseError;
+  Rotation2d m_rotationError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+
+  frc2::PIDController m_xController;
+  frc2::PIDController m_yController;
+  ProfiledPIDController<units::radian> m_thetaController;
+
+  bool m_firstRun = true;
+};
+}  // namespace frc

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

@@ -0,0 +1,247 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "units/time.h"
+
+namespace frc2 {
+
+/**
+ * Implements a PID control loop.
+ */
+class PIDController : public wpi::Sendable,
+                      public wpi::SendableHelper<PIDController> {
+ public:
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient.
+   * @param Ki     The integral coefficient.
+   * @param Kd     The derivative coefficient.
+   * @param period The period between controller updates in seconds. The
+   *               default is 20 milliseconds. Must be non-zero and positive.
+   */
+  PIDController(double Kp, double Ki, double Kd,
+                units::second_t period = 20_ms);
+
+  ~PIDController() override = default;
+
+  PIDController(const PIDController&) = default;
+  PIDController& operator=(const PIDController&) = default;
+  PIDController(PIDController&&) = default;
+  PIDController& operator=(PIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  void SetPID(double Kp, double Ki, double Kd);
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  void SetP(double Kp);
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  void SetI(double Ki);
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  void SetD(double Kd);
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const;
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const;
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const;
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  double GetSetpoint() const;
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtSetpoint() const;
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(double minimumInput, double maximumInput);
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinuousInput();
+
+  /**
+   * Returns true if continuous input is enabled.
+   */
+  bool IsContinuousInputEnabled() const;
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * When the cap is reached, the integrator value is added to the controller
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+
+  /**
+   * Sets the error which is considered tolerable for use with AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velociytTolerance Velocity error which is tolerable.
+   */
+  void SetTolerance(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   */
+  double GetPositionError() const;
+
+  /**
+   * Returns the velocity error.
+   */
+  double GetVelocityError() const;
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double Calculate(double measurement);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   */
+  double Calculate(double measurement, double setpoint);
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset();
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  // Factor for "proportional" control
+  double m_Kp;
+
+  // Factor for "integral" control
+  double m_Ki;
+
+  // Factor for "derivative" control
+  double m_Kd;
+
+  // The period (in seconds) of the control loop running this controller
+  units::second_t m_period;
+
+  double m_maximumIntegral = 1.0;
+
+  double m_minimumIntegral = -1.0;
+
+  double m_maximumInput = 0;
+
+  double m_minimumInput = 0;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  bool m_continuous = false;
+
+  // The error at the time of the most recent call to Calculate()
+  double m_positionError = 0;
+  double m_velocityError = 0;
+
+  // The error at the time of the second-most-recent call to Calculate() (used
+  // to compute velocity)
+  double m_prevError = 0;
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  // The error that is considered at setpoint.
+  double m_positionTolerance = 0.05;
+  double m_velocityTolerance = std::numeric_limits<double>::infinity();
+
+  double m_setpoint = 0;
+  double m_measurement = 0;
+};
+
+}  // namespace frc2
+
+namespace frc {
+
+using frc2::PIDController;
+
+}  // namespace frc

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

@@ -0,0 +1,365 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <limits>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/PIDController.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+#include "units/time.h"
+
+namespace frc {
+namespace detail {
+void ReportProfiledPIDController();
+}  // namespace detail
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+template <class Distance>
+class ProfiledPIDController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<ProfiledPIDController<Distance>> {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+  using State = typename TrapezoidProfile<Distance>::State;
+  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
+   *
+   * @param Kp          The proportional coefficient.
+   * @param Ki          The integral coefficient.
+   * @param Kd          The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 20 milliseconds.
+   */
+  ProfiledPIDController(double Kp, double Ki, double Kd,
+                        Constraints constraints, units::second_t period = 20_ms)
+      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+    detail::ReportProfiledPIDController();
+  }
+
+  ~ProfiledPIDController() override = default;
+
+  ProfiledPIDController(const ProfiledPIDController&) = default;
+  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
+  ProfiledPIDController(ProfiledPIDController&&) = default;
+  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  void SetPID(double Kp, double Ki, double Kd) {
+    m_controller.SetPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  void SetP(double Kp) { m_controller.SetP(Kp); }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  void SetI(double Ki) { m_controller.SetI(Ki); }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  void SetD(double Kd) { m_controller.SetD(Kd); }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const { return m_controller.GetP(); }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const { return m_controller.GetI(); }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const { return m_controller.GetD(); }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(State goal) { m_goal = goal; }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  State GetGoal() const { return m_goal; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  State GetSetpoint() const { return m_setpoint; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+    m_controller.EnableContinuousInput(minimumInput.template to<double>(),
+                                       maximumInput.template to<double>());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * When the cap is reached, the integrator value is added to the controller
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  void SetTolerance(
+      Distance_t positionTolerance,
+      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance.template to<double>(),
+                              velocityTolerance.template to<double>());
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  Distance_t GetPositionError() const {
+    return Distance_t(m_controller.GetPositionError());
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  Velocity_t GetVelocityError() const {
+    return Velocity_t(m_controller.GetVelocityError());
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double Calculate(Distance_t measurement) {
+    if (m_controller.IsContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      auto goalMinDistance = frc::InputModulus<Distance_t>(
+          m_goal.position - measurement, -errorBound, errorBound);
+      auto setpointMinDistance = frc::InputModulus<Distance_t>(
+          m_setpoint.position - measurement, -errorBound, errorBound);
+
+      // Recompute the profile goal with the smallest error, thus giving the
+      // shortest path. The goal may be outside the input range after this
+      // operation, but that's OK because the controller will still go there and
+      // report an error of zero. In other words, the setpoint only needs to be
+      // offset from the measurement by the input range modulus; they don't need
+      // to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
+    frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
+    m_setpoint = profile.Calculate(GetPeriod());
+    return m_controller.Calculate(measurement.template to<double>(),
+                                  m_setpoint.position.template to<double>());
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(Distance_t measurement, State goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(Distance_t measurement, Distance_t goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  double Calculate(
+      Distance_t measurement, Distance_t goal,
+      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+    SetConstraints(constraints);
+    return Calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t(0));
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("ProfiledPIDController");
+    builder.AddDoubleProperty(
+        "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+    builder.AddDoubleProperty(
+        "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+    builder.AddDoubleProperty(
+        "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "goal", [this] { return GetGoal().position.template to<double>(); },
+        [this](double value) { SetGoal(Distance_t{value}); });
+  }
+
+ private:
+  frc2::PIDController m_controller;
+  Distance_t m_minimumInput{0};
+  Distance_t m_maximumInput{0};
+  typename frc::TrapezoidProfile<Distance>::State m_goal;
+  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,118 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models
+ * that drives the model to a desired pose along a two-dimensional trajectory.
+ * Why would we need a nonlinear control law in addition to the linear ones we
+ * have used so far like PID? If we use the original approach with PID
+ * controllers for left and right position and velocity states, the controllers
+ * only deal with the local pose. If the robot deviates from the path, there is
+ * no way for the controllers to correct and the robot may not reach the desired
+ * global pose. This is due to multiple endpoints existing for the robot which
+ * have the same encoder path arc lengths.
+ *
+ * Instead of using wheel path arc lengths (which are in the robot's local
+ * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
+ * global pose. The controller uses this extra information to guide a linear
+ * reference tracker like the PID controllers back in by adjusting the
+ * references of the PID controllers.
+ *
+ * The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
+ * describes a nonlinear controller for a wheeled vehicle with unicycle-like
+ * kinematics; a global pose consisting of x, y, and theta; and a desired pose
+ * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
+ * acronym for the title of the book it came from in Italian ("Robotica
+ * Articolata e Mobile per i SErvizi e le TEcnologie").
+ *
+ * See <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
+ * on Ramsete unicycle controller for a derivation and analysis.
+ */
+class RamseteController {
+ public:
+  /**
+   * 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.
+   */
+  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.
+   */
+  RamseteController() : RamseteController(2.0, 0.7) {}
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& poseTolerance);
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angularVelocityRef The desired angular velocity.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          units::radians_per_second_t angularVelocityRef);
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  double m_b;
+  double m_zeta;
+
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+};
+
+}  // namespace frc

wpimath/src/main/native/include/wpimath/MathShared.h

@@ -18,7 +18,9 @@ enum class MathUsageId {
   kFilter_Linear,
   kOdometry_DifferentialDrive,
   kOdometry_SwerveDrive,
-  kOdometry_MecanumDrive
+  kOdometry_MecanumDrive,
+  kController_PIDController2,
+  kController_ProfiledPIDController,
 };
 
 class MathShared {