diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index bda202095c..c0448ead59 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -24,6 +24,11 @@ DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing, : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox, gearing, measurementStdDevs) {} +void DCMotorSim::SetState(units::radian_t angularPosition, + units::radians_per_second_t angularVelocity) { + SetState(Vectord<2>{angularPosition, angularVelocity}); +} + units::radian_t DCMotorSim::GetAngularPosition() const { return units::radian_t{GetOutput(0)}; } diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 9cbd414e3a..98d0f8b3fa 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -25,8 +25,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, m_maxHeight(maxHeight), m_gearing(gearing), m_simulateGravity(simulateGravity) { - SetState( - frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0}); + SetState(startingHeight, 0_mps); } ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, @@ -40,6 +39,12 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, gearbox, gearing, drumRadius, minHeight, maxHeight, simulateGravity, startingHeight, measurementStdDevs) {} +void ElevatorSim::SetState(units::meter_t position, + units::meters_per_second_t velocity) { + SetState( + Vectord<2>{std::clamp(position, m_minHeight, m_maxHeight), velocity}); +} + bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const { return elevatorHeight <= m_minHeight; } diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 95dcb9e5ed..f4b9a81556 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -24,6 +24,10 @@ FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing, : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing), gearbox, gearing, measurementStdDevs) {} +void FlywheelSim::SetState(units::radians_per_second_t velocity) { + LinearSystemSim::SetState(Vectord<1>{velocity.value()}); +} + units::radians_per_second_t FlywheelSim::GetAngularVelocity() const { return units::radians_per_second_t{GetOutput(0)}; } diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 67dfa3fde3..d9969ca15a 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -28,7 +28,7 @@ SingleJointedArmSim::SingleJointedArmSim( m_gearbox(gearbox), m_gearing(gearing), m_simulateGravity(simulateGravity) { - SetState(frc::Vectord<2>{startingAngle, 0.0}); + SetState(startingAngle, 0_rad_per_s); } SingleJointedArmSim::SingleJointedArmSim( @@ -42,6 +42,11 @@ SingleJointedArmSim::SingleJointedArmSim( gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity, startingAngle, measurementStdDevs) {} +void SingleJointedArmSim::SetState(units::radian_t angle, + units::radians_per_second_t velocity) { + SetState(Vectord<2>{std::clamp(angle, m_minAngle, m_maxAngle), velocity}); +} + bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const { return armAngle <= m_minAngle; } diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h index 9a90cf7ff9..be3a3259c5 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h @@ -48,6 +48,17 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { units::kilogram_square_meter_t moi, const std::array& measurementStdDevs = {0.0, 0.0}); + using LinearSystemSim::SetState; + + /** + * Sets the state of the DC motor. + * + * @param angularPosition The new position + * @param angularVelocity The new velocity + */ + void SetState(units::radian_t angularPosition, + units::radians_per_second_t angularVelocity); + /** * Returns the DC motor position. * diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index a21e69346a..705ddbcd25 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -65,6 +65,16 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); + using LinearSystemSim::SetState; + + /** + * Sets the elevator's state. The new position will be limited between the + * minimum and maximum allowed heights. + * @param position The new position + * @param velocity The new velocity + */ + void SetState(units::meter_t position, units::meters_per_second_t velocity); + /** * Returns whether the elevator would hit the lower limit. * diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h index a9a8101fe3..cc0eca9ae0 100644 --- a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h @@ -47,6 +47,15 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { units::kilogram_square_meter_t moi, const std::array& measurementStdDevs = {0.0}); + using LinearSystemSim::SetState; + + /** + * Sets the flywheel's state. + * + * @param velocity The new velocity + */ + void SetState(units::radians_per_second_t velocity); + /** * Returns the flywheel velocity. * diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 811c007cf8..12a719b55e 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -64,6 +64,17 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::radian_t startingAngle, const std::array& measurementStdDevs = {0.0}); + using LinearSystemSim::SetState; + + /** + * 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. + */ + void SetState(units::radian_t angle, units::radians_per_second_t velocity); + /** * Returns whether the arm would hit the lower limit. * 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 57ae7939de..f737fafad7 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 @@ -5,6 +5,7 @@ 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; @@ -84,6 +85,16 @@ public class DCMotorSim extends LinearSystemSim { m_gearing = gearing; } + /** + * Sets the state of the DC motor. + * + * @param position The new position + * @param velocity The new velocity + */ + void setState(double angularPositionRad, double angularVelocityRadPerSec) { + setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec)); + } + /** * Returns the DC motor position. * 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 28db3bfc36..6d655f72b4 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 @@ -67,8 +67,7 @@ public class ElevatorSim extends LinearSystemSim { m_maxHeight = maxHeightMeters; m_simulateGravity = simulateGravity; - setState( - VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0)); + setState(startingHeightMeters, 0); } /** @@ -174,6 +173,19 @@ public class ElevatorSim extends LinearSystemSim { null); } + /** + * Sets the elevator's state. The new position will be limited between the minimum and maximum + * allowed heights. + * + * @param positionMeters The new position in meters. + * @param velocityMetersPerSecond New velocity in meters per second. + */ + public void setState(double positionMeters, double velocityMetersPerSecond) { + setState( + VecBuilder.fill( + MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond)); + } + /** * Returns whether the elevator would hit the lower limit. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java index 9f1c76ee25..9c224880fd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -5,6 +5,7 @@ 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.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; @@ -84,6 +85,15 @@ public class FlywheelSim extends LinearSystemSim { m_gearing = gearing; } + /** + * Sets the flywheel's state. + * + * @param velocityRadPerSec The new velocity in radians per second. + */ + public void setState(double velocityRadPerSec) { + setState(VecBuilder.fill(velocityRadPerSec)); + } + /** * Returns the flywheel velocity. * 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 30c39a5b5c..f6921296d3 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 @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.simulation; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.numbers.N1; @@ -66,7 +67,7 @@ public class SingleJointedArmSim extends LinearSystemSim { m_maxAngle = maxAngleRads; m_simulateGravity = simulateGravity; - setState(VecBuilder.fill(startingAngleRads, 0)); + setState(startingAngleRads, 0.0); } /** @@ -172,6 +173,18 @@ public class SingleJointedArmSim extends LinearSystemSim { measurementStdDevs); } + /** + * Sets the arm's state. The new angle will be limited between the minimum and maximum allowed + * limits. + * + * @param angleRadians The new angle. + * @param velocityRadPerSec The new angular velocity. + */ + void setState(double angleRadians, double velocityRadPerSec) { + setState( + VecBuilder.fill(MathUtil.clamp(angleRadians, m_minAngle, m_maxAngle), velocityRadPerSec)); + } + /** * Returns whether the arm would hit the lower limit. *