diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index 2f7ce3e768d..f89f122044f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/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 { // 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 plant, + DCMotor gearbox, + Distance minHeight, + Distance maxHeight, + boolean simulateGravity, + Distance startingHeight, + 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. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index 7d7ae78284d..cc6c2a86e55 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/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 { // 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 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. *