[wpimath] Add feedforward constant constructor to ElevatorSim

wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp

@@ -13,17 +13,14 @@ using namespace frc;
 using namespace frc::sim;
 
 ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
-                         const DCMotor& gearbox, double gearing,
-                         units::meter_t drumRadius, units::meter_t minHeight,
+                         const DCMotor& gearbox, units::meter_t minHeight,
                          units::meter_t maxHeight, bool simulateGravity,
                          units::meter_t startingHeight,
                          const std::array<double, 1>& measurementStdDevs)
     : LinearSystemSim(plant, measurementStdDevs),
       m_gearbox(gearbox),
-      m_drumRadius(drumRadius),
       m_minHeight(minHeight),
       m_maxHeight(maxHeight),
-      m_gearing(gearing),
       m_simulateGravity(simulateGravity) {
   SetState(startingHeight, 0_mps);
 }
@@ -36,7 +33,20 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
                          const std::array<double, 1>& measurementStdDevs)
     : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
                                                  drumRadius, gearing),
-                  gearbox, gearing, drumRadius, minHeight, maxHeight,
+                  gearbox, minHeight, maxHeight,
+                  simulateGravity, startingHeight, measurementStdDevs) {}
+
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
+ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
+                          decltype(1_V / Acceleration_t<Distance>(1)) kA, const DCMotor& gearbox,
+                         units::meter_t minHeight,
+                         units::meter_t maxHeight, bool simulateGravity,
+                         units::meter_t startingHeight,
+                         const std::array<double, 1>& measurementStdDevs)
+    : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA),
+                  gearbox, minHeight, maxHeight,
                   simulateGravity, startingHeight, measurementStdDevs) {}
 
 void ElevatorSim::SetState(units::meter_t position,
@@ -75,9 +85,11 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
   // is spinning 10x faster than the output.
 
   // v = r w, so w = v / r
+  double kA = 1.0 / m_plant.B(1, 0);
+  double kV = kA * m_plant.A(1, 1);
   units::meters_per_second_t velocity{m_x(1)};
   units::radians_per_second_t motorVelocity =
-      velocity / m_drumRadius * m_gearing * 1_rad;
+      units::radians_per_second_t{velocity.value() * kV * m_gearbox.Kv.value()};
 
   // Perform calculation and return.
   return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
@@ -108,4 +120,4 @@ Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
     return Vectord<2>{m_maxHeight.value(), 0.0};
   }
   return updatedXhat;
-}
+}

wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h

@@ -19,6 +19,15 @@ namespace frc::sim {
  */
 class ElevatorSim : public LinearSystemSim<2, 1, 1> {
  public:
+    template <typename Distance>
+  using Velocity_t = units::unit_t<
+      units::compound_unit<Distance, units::inverse<units::seconds>>>;
+
+  template <typename Distance>
+  using Acceleration_t = units::unit_t<units::compound_unit<
+      units::compound_unit<Distance, units::inverse<units::seconds>>,
+      units::inverse<units::seconds>>>;
+
   /**
    * Constructs a simulated elevator mechanism.
    *
@@ -38,7 +47,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
    * @param measurementStdDevs The standard deviation of the measurements.
    */
   ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
-              double gearing, units::meter_t drumRadius,
               units::meter_t minHeight, units::meter_t maxHeight,
               bool simulateGravity, units::meter_t startingHeight,
               const std::array<double, 1>& measurementStdDevs = {0.0});
@@ -65,6 +73,27 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
               bool simulateGravity, units::meter_t startingHeight,
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param kV                 The velocity gain.
+   * @param kA                 The acceleration gain.
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param startingHeight     The starting height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  template <typename Distance>
+    requires std::same_as<units::meter, Distance> ||
+             std::same_as<units::radian, Distance>
+  ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
+              decltype(1_V / Acceleration_t<Distance>(1)) kA, const DCMotor& gearbox,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              bool simulateGravity, units::meter_t startingHeight,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
   using LinearSystemSim::SetState;
 
   /**
@@ -146,10 +175,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
 
  private:
   DCMotor m_gearbox;
-  units::meter_t m_drumRadius;
   units::meter_t m_minHeight;
   units::meter_t m_maxHeight;
-  double m_gearing;
   bool m_simulateGravity;
 };
-}  // namespace frc::sim
+}  // namespace frc::sim

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java

@@ -19,12 +19,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   // Gearbox for the elevator.
   private final DCMotor m_gearbox;
 
-  // Gearing between the motors and the output.
-  private final double m_gearing;
-
-  // The radius of the drum that the elevator spool is wrapped around.
-  private final double m_drumRadius;
-
   // The min allowable height for the elevator.
   private final double m_minHeight;
 
@@ -41,8 +35,6 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
    *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
    *     double, double)}.
    * @param gearbox The type of and number of motors in the elevator gearbox.
-   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
    * @param simulateGravity Whether gravity should be simulated or not.
@@ -52,17 +44,13 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
       DCMotor gearbox,
-      double gearing,
-      double drumRadiusMeters,
       double minHeightMeters,
       double maxHeightMeters,
       boolean simulateGravity,
       double startingHeightMeters,
       Matrix<N1, N1> measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
-    m_gearing = gearing;
-    m_drumRadius = drumRadiusMeters;
     m_minHeight = minHeightMeters;
     m_maxHeight = maxHeightMeters;
     m_simulateGravity = simulateGravity;
@@ -77,8 +65,6 @@ public ElevatorSim(
    *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
    *     double, double)}.
    * @param gearbox The type of and number of motors in the elevator gearbox.
-   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
    * @param minHeightMeters The min allowable height of the elevator.
    * @param maxHeightMeters The max allowable height of the elevator.
    * @param startingHeightMeters The starting height of the elevator.
@@ -87,24 +73,81 @@ public ElevatorSim(
   public ElevatorSim(
       LinearSystem<N2, N1, N1> plant,
       DCMotor gearbox,
-      double gearing,
-      double drumRadiusMeters,
       double minHeightMeters,
       double maxHeightMeters,
       boolean simulateGravity,
       double startingHeightMeters) {
     this(
         plant,
         gearbox,
-        gearing,
-        drumRadiusMeters,
         minHeightMeters,
         maxHeightMeters,
         simulateGravity,
         startingHeightMeters,
         null);
   }
 
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param kV The velocity gain.
+   * @param kA The acceleration gain.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingHeightMeters The starting height of the elevator.
+   */
+  public ElevatorSim(
+      double kV,
+      double kA,
+      DCMotor gearbox,
+      double minHeightMeters,
+      double maxHeightMeters,
+      boolean simulateGravity,
+      double startingHeightMeters) {
+    this(
+        kV,
+        kA,
+        gearbox,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        startingHeightMeters,
+        null);
+  }
+
+  /**
+   * Creates a simulated elevator mechanism.
+   *
+   * @param kV The velocity gain.
+   * @param kA The acceleration gain.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingHeightMeters The starting height of the elevator.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  public ElevatorSim(
+      double kV,
+      double kA,
+      DCMotor gearbox,
+      double minHeightMeters,
+      double maxHeightMeters,
+      boolean simulateGravity,
+      double startingHeightMeters,
+      Matrix<N1, N1> measurementStdDevs) {
+    this(
+        LinearSystemId.identifyPositionSystem(kV, kA),
+        gearbox,
+        minHeightMeters,
+        maxHeightMeters,
+        simulateGravity,
+        startingHeightMeters,
+        measurementStdDevs);
+  }
+
   /**
    * Creates a simulated elevator mechanism.
    *
@@ -131,8 +174,6 @@ public ElevatorSim(
     this(
         LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
         gearbox,
-        gearing,
-        drumRadiusMeters,
         minHeightMeters,
         maxHeightMeters,
         simulateGravity,
@@ -253,7 +294,10 @@ public double getCurrentDrawAmps() {
     // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
     // spinning 10x faster than the output
     // v = r w, so w = v/r
-    double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
+    double kA = 1 / m_plant.getB().get(1, 0);
+    double kV = -m_plant.getA().get(1, 1) * kA;
+    double motorVelocityRadPerSec =
+        getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt;
     var appliedVoltage = m_u.get(0, 0);
     return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
         * Math.signum(appliedVoltage);