[wpilib] Add unit-aware constructor overloads to physics sim classes

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

@@ -13,8 +13,13 @@
 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.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Mass;
 import edu.wpi.first.wpilibj.RobotController;
 
+import static edu.wpi.first.units.Units.*;
+
 /** Represents a simulated elevator mechanism. */
 public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
   // Gearbox for the elevator.
@@ -127,6 +132,111 @@ public ElevatorSim(
         measurementStdDevs);
   }
 
+  /**
+   * 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 minHeight The min allowable height of the elevator.
+   * @param maxHeight 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. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  @SuppressWarnings("this-escape")
+  public ElevatorSim(
+          LinearSystem<N2, N1, N2> plant,
+          DCMotor gearbox,
+          Distance minHeight,
+          Distance maxHeight,
+          boolean simulateGravity,
+          Distance startingHeightMeters,
+          double... measurementStdDevs) {
+    this(
+            plant,
+            gearbox,
+            minHeight.in(Meters),
+            maxHeight.in(Meters),
+            simulateGravity,
+            startingHeightMeters.in(Meters),
+            measurementStdDevs
+    );
+  }
+
+  /**
+   * 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 minHeight The min allowable height of the elevator.
+   * @param maxHeight The max allowable 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 deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ElevatorSim(
+          double kV,
+          double kA,
+          DCMotor gearbox,
+          Distance minHeight,
+          Distance maxHeight,
+          boolean simulateGravity,
+          Distance startingHeight,
+          double... measurementStdDevs) {
+    this(
+        kV,
+        kA,
+        gearbox,
+        minHeight.in(Meters),
+        maxHeight.in(Meters),
+        simulateGravity,
+        startingHeight.in(Meters),
+        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 carriageMass The mass of the elevator carriage.
+   * @param drumRadius The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeight The min allowable height of the elevator.
+   * @param maxHeight The max allowable 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 deviations of the measurements. Can be omitted if no
+   *     noise is desired. If present must have 1 element for position.
+   */
+  public ElevatorSim(
+          DCMotor gearbox,
+          double gearing,
+          Mass carriageMass,
+          Distance drumRadius,
+          Distance minHeight,
+          Distance maxHeight,
+          boolean simulateGravity,
+          Distance startingHeight,
+          double... measurementStdDevs) {
+    this(
+        gearbox,
+        gearing,
+        carriageMass.in(Kilograms),
+        drumRadius.in(Meters),
+        minHeight.in(Meters),
+        maxHeight.in(Meters),
+        simulateGravity,
+        startingHeight.in(Meters),
+        measurementStdDevs
+    );
+  }
+
   /**
    * Sets the elevator's state. The new position will be limited between the minimum and maximum
    * allowed heights.
@@ -187,6 +297,15 @@ public double getPositionMeters() {
     return getOutput(0);
   }
 
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  public Distance getPosition() {
+    return Meters.of(getPositionMeters());
+  }
+
   /**
    * Returns the velocity of the elevator.
    *
@@ -196,6 +315,15 @@ public double getVelocityMetersPerSecond() {
     return getOutput(1);
   }
 
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  public LinearVelocity getVelocity() {
+    return MetersPerSecond.of(getVelocityMetersPerSecond());
+  }
+
   /**
    * Returns the elevator current draw.
    *

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

@@ -13,8 +13,14 @@
 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.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.MomentOfInertia;
 import edu.wpi.first.wpilibj.RobotController;
 
+import static edu.wpi.first.units.Units.*;
+
 /** Represents a simulated single jointed arm mechanism. */
 public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
   // The gearbox for the arm.
@@ -109,6 +115,82 @@ public SingleJointedArmSim(
         measurementStdDevs);
   }
 
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param plant The linear system that represents the arm. This system can be created with {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor,
+   *     double, double)}.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLength The length of the arm.
+   * @param minAngle  The minimum angle that the arm is capable of.
+   * @param maxAngle  The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngle  The initial position of the Arm simulation.
+   * @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 SingleJointedArmSim(
+          LinearSystem<N2, N1, N2> plant,
+          DCMotor gearbox,
+          double gearing,
+          Distance armLength,
+          Angle minAngle,
+          Angle maxAngle,
+          boolean simulateGravity,
+          Angle startingAngle,
+          double... measurementStdDevs) {
+    this(
+            plant,
+            gearbox,
+            gearing,
+            armLength.in(Meters),
+            minAngle.in(Radians),
+            maxAngle.in(Radians),
+            simulateGravity,
+            startingAngle.in(Radians),
+            measurementStdDevs
+    );
+  }
+
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param moi The moment of inertia of the arm; can be calculated from CAD software.
+   * @param armLength The length of the arm.
+   * @param minAngle  The minimum angle that the arm is capable of.
+   * @param maxAngle  The maximum angle that the arm is capable of.
+   * @param simulateGravity Whether gravity should be simulated or not.
+   * @param startingAngle  The initial position of the Arm simulation.
+   * @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 SingleJointedArmSim(
+          DCMotor gearbox,
+          double gearing,
+          MomentOfInertia moi,
+          Distance armLength,
+          Angle minAngle,
+          Angle maxAngle,
+          boolean simulateGravity,
+          Angle startingAngle,
+          double... measurementStdDevs) {
+    this(
+            gearbox,
+            gearing,
+            moi.in(KilogramSquareMeters),
+            armLength.in(Meters),
+            minAngle.in(Radians),
+            maxAngle.in(Radians),
+            simulateGravity,
+            startingAngle.in(Radians),
+            measurementStdDevs);
+  }
+
   /**
    * Sets the arm's state. The new angle will be limited between the minimum and maximum allowed
    * limits.
@@ -121,6 +203,17 @@ public final void setState(double angleRadians, double velocityRadPerSec) {
         VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec));
   }
 
+  /**
+   * Sets the arm's state. The new angle will be limited between the minimum and maximum allowed
+   * limits.
+   *
+   * @param angle The new angle.
+   * @param velocity The new angular velocity.
+   */
+  public final void setState(Angle angle, AngularVelocity velocity) {
+    setState(angle.in(Radians), velocity.in(RadiansPerSecond));
+  }
+
   /**
    * Returns whether the arm would hit the lower limit.
    *
@@ -189,6 +282,24 @@ public double getCurrentDrawAmps() {
     return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
   }
 
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle.
+   */
+  public Angle getAngle() {
+    return Radians.of(getAngleRads());
+  }
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity.
+   */
+  public AngularVelocity getVelocity() {
+    return RadiansPerSecond.of(getVelocityRadPerSec());
+  }
+
   /**
    * Sets the input voltage for the arm.
    *