[wpimath] Make controllers and some trajectory classes constexpr

Merge after wpilibsuite#7341, wpilibsuite#7342.
calcmogul · Nov 5, 2024 · 53b1d5e · 53b1d5e
1 parent debb521
commit 53b1d5e
Show file tree

Hide file tree

Showing 44 changed files with 1,418 additions and 1,758 deletions.
diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
@@ -15,20 +15,6 @@
 
 using namespace frc;
 
-units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
-                                        units::unit_t<Velocity> currentVelocity,
-                                        units::unit_t<Velocity> nextVelocity,
-                                        units::second_t dt) const {
-  return Calculate(currentAngle, currentVelocity, nextVelocity);
-}
-
-units::volt_t ArmFeedforward::Calculate(
-    units::unit_t<Angle> currentAngle,
-    units::unit_t<Velocity> currentVelocity) const {
-  return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) +
-         kV * currentVelocity;
-}
-
 units::volt_t ArmFeedforward::Calculate(
     units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
     units::unit_t<Velocity> nextVelocity) const {

diff --git a/wpimath/src/main/native/cpp/controller/BangBangController.cpp b/wpimath/src/main/native/cpp/controller/BangBangController.cpp
@@ -6,52 +6,8 @@
 
 #include <wpi/sendable/SendableBuilder.h>
 
-#include "wpimath/MathShared.h"
-
 using namespace frc;
 
-BangBangController::BangBangController(double tolerance)
-    : m_tolerance(tolerance) {}
-
-void BangBangController::SetSetpoint(double setpoint) {
-  m_setpoint = setpoint;
-}
-
-double BangBangController::GetSetpoint() const {
-  return m_setpoint;
-}
-
-bool BangBangController::AtSetpoint() const {
-  return std::abs(m_setpoint - m_measurement) < m_tolerance;
-}
-
-void BangBangController::SetTolerance(double tolerance) {
-  m_tolerance = tolerance;
-}
-
-double BangBangController::GetTolerance() const {
-  return m_tolerance;
-}
-
-double BangBangController::GetMeasurement() const {
-  return m_measurement;
-}
-
-double BangBangController::GetError() const {
-  return m_setpoint - m_measurement;
-}
-
-double BangBangController::Calculate(double measurement, double setpoint) {
-  m_measurement = measurement;
-  m_setpoint = setpoint;
-
-  return measurement < setpoint ? 1 : 0;
-}
-
-double BangBangController::Calculate(double measurement) {
-  return Calculate(measurement, m_setpoint);
-}
-
 void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("BangBangController");
   builder.AddDoubleProperty(

diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
@@ -4,35 +4,10 @@
 
 #include "frc/controller/DifferentialDriveAccelerationLimiter.h"
 
-#include <utility>
-
 #include <Eigen/QR>
 
 using namespace frc;
 
-DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
-    LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
-    units::meters_per_second_squared_t maxLinearAccel,
-    units::radians_per_second_squared_t maxAngularAccel)
-    : DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel,
-                                           maxLinearAccel, maxAngularAccel) {}
-
-DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
-    LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
-    units::meters_per_second_squared_t minLinearAccel,
-    units::meters_per_second_squared_t maxLinearAccel,
-    units::radians_per_second_squared_t maxAngularAccel)
-    : m_system{std::move(system)},
-      m_trackwidth{trackwidth},
-      m_minLinearAccel{minLinearAccel},
-      m_maxLinearAccel{maxLinearAccel},
-      m_maxAngularAccel{maxAngularAccel} {
-  if (minLinearAccel > maxLinearAccel) {
-    throw std::invalid_argument(
-        "maxLinearAccel must be greater than minLinearAccel");
-  }
-}
-
 DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
     units::meters_per_second_t leftVelocity,
     units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,

diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveFeedforward.cpp
@@ -7,31 +7,9 @@
 #include <Eigen/Core>
 
 #include "frc/controller/LinearPlantInversionFeedforward.h"
-#include "frc/system/plant/LinearSystemId.h"
 
 using namespace frc;
 
-DifferentialDriveFeedforward::DifferentialDriveFeedforward(
-    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
-    decltype(1_V / 1_rad_per_s) kVAngular,
-    decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
-    // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
-    // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
-    // 1_rad_per_s_sq))
-    : DifferentialDriveFeedforward{kVLinear, kALinear,
-                                   kVAngular * 2.0 / trackwidth * 1_rad,
-                                   kAAngular * 2.0 / trackwidth * 1_rad} {}
-
-DifferentialDriveFeedforward::DifferentialDriveFeedforward(
-    decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
-    decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
-    : m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
-          kVLinear, kALinear, kVAngular, kAAngular)},
-      m_kVLinear{kVLinear},
-      m_kALinear{kALinear},
-      m_kVAngular{kVAngular},
-      m_kAAngular{kAAngular} {}
-
 DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
     units::meters_per_second_t currentLeftVelocity,
     units::meters_per_second_t nextLeftVelocity,

diff --git a/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp