diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java index 4440a2d7787..53b19271751 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -20,6 +21,7 @@ import edu.wpi.first.units.measure.MutAngle; import edu.wpi.first.units.measure.MutAngularAcceleration; import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated DC motor mechanism. */ @@ -43,6 +45,9 @@ public class DCMotorSim extends LinearSystemSim { private final MutAngularAcceleration m_angularAcceleration = RadiansPerSecondPerSecond.mutable(0.0); + // Input voltage of the system + private final MutVoltage m_voltage = Volts.mutable(0); + /** * Creates a simulated DC motor mechanism. * @@ -243,6 +248,16 @@ public double getInputVoltage() { return getInput(0); } + /** + * Gets the input voltage for the DC motor. + * + * @return The DC motor's input voltage. + */ + public Voltage getInputVoltageMeasure() { + m_voltage.mut_setMagnitude(getInputVoltage()); + return m_voltage; + } + /** * Sets the input voltage for the DC motor. * @@ -252,4 +267,13 @@ public void setInputVoltage(double volts) { setInput(volts); clampInput(RobotController.getBatteryVoltage()); } + + /** + * Sets the input voltage for the DC motor. + * + * @param volts The input voltage. + */ + public void setInputVoltage(Voltage volts) { + setInputVoltage(volts.in(Volts)); + } }