LinearSystemSim Constructor and method cleanup

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

@@ -48,4 +48,5 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const {
 
 void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
   SetInput(Vectord<1>{voltage.value()});
+  ClampInput(frc::RobotController::GetBatteryVoltage().value());
 }

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

@@ -98,6 +98,7 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
 
 void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
   SetInput(Vectord<1>{voltage.value()});
+  ClampInput(frc::RobotController::GetBatteryVoltage().value());
 }
 
 Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,

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

@@ -43,4 +43,5 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const {
 
 void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
   SetInput(Vectord<1>{voltage.value()});
+  ClampInput(frc::RobotController::GetBatteryVoltage().value());
 }

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

@@ -81,6 +81,7 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
 
 void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
   SetInput(Vectord<1>{voltage.value()});
+  ClampInput(frc::RobotController::GetBatteryVoltage().value());
 }
 
 Vectord<2> SingleJointedArmSim::UpdateX(const Vectord<2>& currentXhat,

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

@@ -78,7 +78,7 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> {
    *
    * @return The DC motor current draw.
    */
-  units::ampere_t GetCurrentDraw() const override;
+  units::ampere_t GetCurrentDraw() const;
 
   /**
    * Sets the input voltage for the DC motor.

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

@@ -150,7 +150,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> {
    *
    * @return The elevator current draw.
    */
-  units::ampere_t GetCurrentDraw() const override;
+  units::ampere_t GetCurrentDraw() const;
 
   /**
    * Sets the input voltage for the elevator.

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

@@ -68,7 +68,7 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
    *
    * @return The flywheel current draw.
    */
-  units::ampere_t GetCurrentDraw() const override;
+  units::ampere_t GetCurrentDraw() const;
 
   /**
    * Sets the input voltage for the flywheel.

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

@@ -86,18 +86,15 @@ class LinearSystemSim {
    *
    * @param u The system inputs.
    */
-  void SetInput(const Vectord<Inputs>& u) { m_u = ClampInput(u); }
+  void SetInput(const Vectord<Inputs>& u) { m_u = u; }
 
   /**
    * Sets the system inputs.
    *
    * @param row   The row in the input matrix to set.
    * @param value The value to set the row to.
    */
-  void SetInput(int row, double value) {
-    m_u(row, 0) = value;
-    ClampInput(m_u);
-  }
+  void SetInput(int row, double value) { m_u(row, 0) = value; }
 
   /**
    * Returns the current input of the plant.
@@ -121,14 +118,6 @@ class LinearSystemSim {
    */
   void SetState(const Vectord<States>& state) { m_x = state; }
 
-  /**
-   * Returns the current drawn by this simulated system. Override this method to
-   * add a custom current calculation.
-   *
-   * @return The current drawn by this simulated mechanism.
-   */
-  virtual units::ampere_t GetCurrentDraw() const { return 0_A; }
-
  protected:
   /**
    * Updates the state estimate of the system.
@@ -147,12 +136,10 @@ class LinearSystemSim {
    * Clamp the input vector such that no element exceeds the given voltage. If
    * any does, the relative magnitudes of the input will be maintained.
    *
-   * @param u          The input vector.
-   * @return The normalized input.
+   * @param maxInput The maximum magnitude of the input vector after clamping.
    */
-  Vectord<Inputs> ClampInput(Vectord<Inputs> u) {
-    return frc::DesaturateInputVector<Inputs>(
-        u, frc::RobotController::GetInputVoltage());
+  void ClampInput(double maxInput) {
+    m_u = frc::DesaturateInputVector<Inputs>(m_u, maxInput);
   }
 
   /// The plant that represents the linear system.

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

@@ -126,7 +126,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
    *
    * @return The arm current draw.
    */
-  units::ampere_t GetCurrentDraw() const override;
+  units::ampere_t GetCurrentDraw() const;
 
   /**
    * Sets the input voltage for the arm.

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

@@ -4,14 +4,14 @@
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.system.LinearSystem;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
 
 /** Represents a simulated DC motor mechanism. */
 public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
@@ -25,30 +25,17 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
    * Creates a simulated DC motor mechanism.
    *
    * @param plant The linear system representing the DC motor. This system can be created with
-   *     {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double,
-   *     double)}.
    * @param gearbox The type of and number of motors in the DC motor gearbox.
    * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
-   */
-  public DCMotorSim(LinearSystem<N2, N1, N2> plant, DCMotor gearbox, double gearing) {
-    super(plant);
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-  }
-
-  /**
-   * Creates a simulated DC motor mechanism.
-   *
-   * @param plant The linear system representing the DC motor. This system can be created with
-   * @param gearbox The type of and number of motors in the DC motor gearbox.
-   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
-   * @param measurementStdDevs The standard deviations of the measurements.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 2 elements. The first element is for position. The
+   *     second element is for velocity.
    */
   public DCMotorSim(
       LinearSystem<N2, N1, N2> plant,
       DCMotor gearbox,
       double gearing,
-      Matrix<N2, N1> measurementStdDevs) {
+      double... measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -60,25 +47,13 @@ public DCMotorSim(
    * @param gearbox The type of and number of motors in the DC motor gearbox.
    * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
    * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
-   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
-   */
-  public DCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
-    super(LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing));
-    m_gearbox = gearbox;
-    m_gearing = gearing;
-  }
-
-  /**
-   * Creates a simulated DC motor mechanism.
-   *
-   * @param gearbox The type of and number of motors in the DC motor gearbox.
-   * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions).
-   * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the
-   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, Matrix)} constructor.
-   * @param measurementStdDevs The standard deviations of the measurements.
+   *     {@link #DCMotorSim(LinearSystem, DCMotor, double, double...)} constructor.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 2 elements. The first element is for position. The
+   *     second element is for velocity.
    */
   public DCMotorSim(
-      DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N2, N1> measurementStdDevs) {
+      DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) {
     super(
         LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs);
     m_gearbox = gearbox;
@@ -136,7 +111,6 @@ public double getAngularVelocityRPM() {
    *
    * @return The DC motor current draw.
    */
-  @Override
   public double getCurrentDrawAmps() {
     // I = V / R - omega / (Kv * R)
     // Reductions are output over input, so a reduction of 2:1 means the motor is spinning
@@ -152,5 +126,6 @@ public double getCurrentDrawAmps() {
    */
   public void setInputVoltage(double volts) {
     setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
   }
 }

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

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.system.NumericalIntegration;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.RobotController;
 
 /** Represents a simulated elevator mechanism. */
 public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
@@ -39,7 +40,8 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
    * @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.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
    */
   @SuppressWarnings("this-escape")
   public ElevatorSim(
@@ -49,7 +51,7 @@ public ElevatorSim(
       double maxHeightMeters,
       boolean simulateGravity,
       double startingHeightMeters,
-      Matrix<N2, N1> measurementStdDevs) {
+      double... measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
     m_minHeight = minHeightMeters;
@@ -59,35 +61,6 @@ public ElevatorSim(
     setState(startingHeightMeters, 0);
   }
 
-  /**
-   * Creates a simulated elevator mechanism.
-   *
-   * @param plant The linear system that represents the elevator. This system can be created with
-   *     {@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 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.
-   * @param simulateGravity Whether gravity should be simulated or not.
-   */
-  public ElevatorSim(
-      LinearSystem<N2, N1, N2> plant,
-      DCMotor gearbox,
-      double minHeightMeters,
-      double maxHeightMeters,
-      boolean simulateGravity,
-      double startingHeightMeters) {
-    this(
-        plant,
-        gearbox,
-        minHeightMeters,
-        maxHeightMeters,
-        simulateGravity,
-        startingHeightMeters,
-        null);
-  }
-
   /**
    * Creates a simulated elevator mechanism.
    *
@@ -98,37 +71,8 @@ public ElevatorSim(
    * @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.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
    */
   public ElevatorSim(
       double kV,
@@ -138,7 +82,7 @@ public ElevatorSim(
       double maxHeightMeters,
       boolean simulateGravity,
       double startingHeightMeters,
-      Matrix<N2, N1> measurementStdDevs) {
+      double... measurementStdDevs) {
     this(
         LinearSystemId.identifyPositionSystem(kV, kA),
         gearbox,
@@ -160,7 +104,8 @@ public ElevatorSim(
    * @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.
+   * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
    */
   public ElevatorSim(
       DCMotor gearbox,
@@ -171,7 +116,7 @@ public ElevatorSim(
       double maxHeightMeters,
       boolean simulateGravity,
       double startingHeightMeters,
-      Matrix<N2, N1> measurementStdDevs) {
+      double... measurementStdDevs) {
     this(
         LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
         gearbox,
@@ -182,39 +127,6 @@ public ElevatorSim(
         measurementStdDevs);
   }
 
-  /**
-   * Creates a simulated elevator mechanism.
-   *
-   * @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 carriageMassKg The mass of the elevator carriage.
-   * @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.
-   * @param startingHeightMeters The starting height of the elevator.
-   */
-  public ElevatorSim(
-      DCMotor gearbox,
-      double gearing,
-      double carriageMassKg,
-      double drumRadiusMeters,
-      double minHeightMeters,
-      double maxHeightMeters,
-      boolean simulateGravity,
-      double startingHeightMeters) {
-    this(
-        gearbox,
-        gearing,
-        carriageMassKg,
-        drumRadiusMeters,
-        minHeightMeters,
-        maxHeightMeters,
-        simulateGravity,
-        startingHeightMeters,
-        null);
-  }
-
   /**
    * Sets the elevator's state. The new position will be limited between the minimum and maximum
    * allowed heights.
@@ -289,7 +201,6 @@ public double getVelocityMetersPerSecond() {
    *
    * @return The elevator current draw.
    */
-  @Override
   public double getCurrentDrawAmps() {
     // I = V / R - omega / (Kv * R)
     // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
@@ -310,6 +221,7 @@ public double getCurrentDrawAmps() {
    */
   public void setInputVoltage(double volts) {
     setInput(volts);
+    clampInput(RobotController.getBatteryVoltage());
   }
 
   /**