[wpimath] Make controllers and some trajectory classes constexpr (#7343)

wpilibsuite · Nov 7, 2024 · a66fa33 · a66fa33
1 parent 44a45d4
commit a66fa33
Show file tree

Hide file tree

Showing 71 changed files with 1,516 additions and 1,904 deletions.
diff --git a/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
@@ -6,7 +6,6 @@
 
 #include <memory>
 #include <stdexcept>
-#include <utility>
 
 #include <hal/simulation/AddressableLEDData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/AnalogGyroSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/AnalogGyroData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/AnalogInputSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/AnalogInData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/AnalogOutputSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/AnalogOutData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
@@ -6,7 +6,6 @@
 
 #include <memory>
 #include <stdexcept>
-#include <utility>
 
 #include <hal/simulation/AnalogTriggerData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/BuiltInAccelerometerSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/AccelerometerData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp b/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/CTREPCMSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/CTREPCMData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
@@ -6,7 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
-#include "frc/system/plant/LinearSystemId.h"
+#include "frc/RobotController.h"
 
 using namespace frc;
 using namespace frc::sim;

diff --git a/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/DIOSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/DIOData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -11,6 +11,7 @@
 #include <wpi/MathExtras.h>
 
 #include "frc/RobotController.h"
+#include "frc/StateSpaceUtil.h"
 #include "frc/system/NumericalIntegration.h"
 
 using namespace frc;

diff --git a/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
@@ -6,7 +6,6 @@
 
 #include <memory>
 #include <stdexcept>
-#include <utility>
 
 #include <hal/simulation/DigitalPWMData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/DriverStationSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/DriverStation.h>
 #include <hal/simulation/DriverStationData.h>

diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
@@ -6,7 +6,6 @@
 
 #include <memory>
 #include <stdexcept>
-#include <utility>
 
 #include <hal/simulation/DutyCycleData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -6,6 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
+#include "frc/RobotController.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 

diff --git a/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
@@ -6,7 +6,6 @@
 
 #include <memory>
 #include <stdexcept>
-#include <utility>
 
 #include <hal/simulation/EncoderData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
@@ -6,7 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
-#include "frc/system/plant/LinearSystemId.h"
+#include "frc/RobotController.h"
 
 using namespace frc;
 using namespace frc::sim;

diff --git a/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/PWMSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/PWMData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp b/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/PowerDistributionSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/PowerDistributionData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/REVPHSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/REVPHData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/RelaySim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/RelayData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
@@ -5,7 +5,6 @@
 #include "frc/simulation/SPIAccelerometerSim.h"
 
 #include <memory>
-#include <utility>
 
 #include <hal/simulation/SPIAccelerometerData.h>
 

diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -9,6 +9,7 @@
 #include <units/voltage.h>
 #include <wpi/MathExtras.h>
 
+#include "frc/RobotController.h"
 #include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 

diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -10,7 +10,6 @@
 #include <units/time.h>
 
 #include "frc/EigenCore.h"
-#include "frc/RobotController.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/LinearSystem.h"
 

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,