diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 43b72db2d2..9cbd414e3a 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -16,6 +16,7 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox, double gearing, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, const std::array& measurementStdDevs) : LinearSystemSim(plant, measurementStdDevs), m_gearbox(gearbox), @@ -23,22 +24,21 @@ ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, m_minHeight(minHeight), m_maxHeight(maxHeight), m_gearing(gearing), - m_simulateGravity(simulateGravity) {} + m_simulateGravity(simulateGravity) { + SetState( + frc::Vectord<2>{std::clamp(startingHeight, minHeight, maxHeight), 0.0}); +} ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, units::kilogram_t carriageMass, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, const std::array& measurementStdDevs) - : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, - drumRadius, gearing), - measurementStdDevs), - m_gearbox(gearbox), - m_drumRadius(drumRadius), - m_minHeight(minHeight), - m_maxHeight(maxHeight), - m_gearing(gearing), - m_simulateGravity(simulateGravity) {} + : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, + drumRadius, gearing), + gearbox, gearing, drumRadius, minHeight, maxHeight, + simulateGravity, startingHeight, measurementStdDevs) {} bool ElevatorSim::WouldHitLowerLimit(units::meter_t elevatorHeight) const { return elevatorHeight <= m_minHeight; diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index 58392d6709..67dfa3fde3 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -19,6 +19,7 @@ SingleJointedArmSim::SingleJointedArmSim( const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, + units::radian_t startingAngle, const std::array& measurementStdDevs) : LinearSystemSim<2, 1, 1>(system, measurementStdDevs), m_armLen(armLength), @@ -26,17 +27,20 @@ SingleJointedArmSim::SingleJointedArmSim( m_maxAngle(maxAngle), m_gearbox(gearbox), m_gearing(gearing), - m_simulateGravity(simulateGravity) {} + m_simulateGravity(simulateGravity) { + SetState(frc::Vectord<2>{startingAngle, 0.0}); +} SingleJointedArmSim::SingleJointedArmSim( const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, + units::radian_t startingAngle, const std::array& measurementStdDevs) : SingleJointedArmSim( LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing), gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity, - measurementStdDevs) {} + startingAngle, measurementStdDevs) {} bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const { return armAngle <= m_minAngle; diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index cf408107c3..5bc3083e98 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -32,12 +32,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed 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 deviation of the measurements. */ ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox, double gearing, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, - bool simulateGravity, + bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); /** @@ -53,12 +54,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed 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 deviation of the measurements. */ ElevatorSim(const DCMotor& gearbox, double gearing, units::kilogram_t carriageMass, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, - bool simulateGravity, + bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); /** diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index bf0de42bdf..06621aaeba 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -31,12 +31,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @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. * @param measurementStdDevs The standard deviations of the measurements. */ SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, + units::radian_t startingAngle, const std::array& measurementStdDevs = {0.0}); /** * Creates a simulated arm mechanism. @@ -50,12 +52,14 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * @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. * @param measurementStdDevs The standard deviation of the measurement noise. */ SingleJointedArmSim(const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi, units::meter_t armLength, units::radian_t minAngle, units::radian_t maxAngle, bool simulateGravity, + units::radian_t startingAngle, const std::array& measurementStdDevs = {0.0}); /** diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 8afcf6f654..4e7e652458 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -21,7 +21,7 @@ TEST(ElevatorSimTest, StateSpaceSim) { frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in, - 0_m, 3_m, true, {0.01}); + 0_m, 3_m, true, 0_m, {0.01}); frc2::PIDController controller(10, 0.0, 0.0); frc::PWMVictorSPX motor(0); @@ -46,7 +46,7 @@ TEST(ElevatorSimTest, StateSpaceSim) { TEST(ElevatorSimTest, MinMax) { frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, 0.75_in, - 0_m, 1_m, true, {0.01}); + 0_m, 1_m, true, 0_m, {0.01}); for (size_t i = 0; i < 100; ++i) { sim.SetInput(frc::Vectord<1>{0.0}); sim.Update(20_ms); @@ -66,7 +66,7 @@ TEST(ElevatorSimTest, MinMax) { TEST(ElevatorSimTest, Stability) { frc::sim::ElevatorSim sim{ - frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false}; + frc::DCMotor::Vex775Pro(4), 100, 4_kg, 0.5_in, 0_m, 10_m, false, 0_m}; sim.SetState(frc::Vectord<2>{0.0, 0.0}); sim.SetInput(frc::Vectord<1>{12.0}); diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 44264308da..07a1e1c409 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -9,7 +9,7 @@ TEST(SingleJointedArmTest, Disabled) { frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 300, 3_kg_sq_m, - 30_in, -180_deg, 0_deg, true); + 30_in, -180_deg, 0_deg, true, 90_deg); sim.SetState(frc::Vectord<2>{0.0, 0.0}); for (size_t i = 0; i < 12 / 0.02; ++i) { diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h index 811c645b26..929cbd458b 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/include/subsystems/Arm.h @@ -53,6 +53,7 @@ class Arm { kMinAngle, kMaxAngle, true, + 0_deg, {kArmEncoderDistPerPulse}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h index 0f1313a951..4b07189442 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/include/subsystems/Elevator.h @@ -56,6 +56,7 @@ class Elevator { Constants::kMinElevatorHeight, Constants::kMaxElevatorHeight, true, + 0_m, {0.01}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; diff --git a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp index 92b1affe7d..8173aab917 100644 --- a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp @@ -38,7 +38,8 @@ class PotentiometerPIDTest : public testing::Test { kElevatorDrumRadius, 0.0_m, Robot::kFullHeight, - true}; + true, + 0.0_m}; frc::sim::PWMSim m_motorSim{Robot::kMotorChannel}; frc::sim::AnalogInputSim m_analogSim{Robot::kPotChannel}; frc::sim::JoystickSim m_joystickSim{Robot::kJoystickChannel}; 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 21486092e3..f1ccdf2c01 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 @@ -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; @@ -43,6 +44,8 @@ public class ElevatorSim extends LinearSystemSim { * @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. */ public ElevatorSim( LinearSystem plant, @@ -51,16 +54,19 @@ public class ElevatorSim extends LinearSystemSim { double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, - boolean simulateGravity) { - this( - plant, - gearbox, - gearing, - drumRadiusMeters, - minHeightMeters, - maxHeightMeters, - simulateGravity, - null); + boolean simulateGravity, + double startingHeightMeters, + Matrix measurementStdDevs) { + super(plant, measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_drumRadius = drumRadiusMeters; + m_minHeight = minHeightMeters; + m_maxHeight = maxHeightMeters; + m_simulateGravity = simulateGravity; + + setState( + VecBuilder.fill(MathUtil.clamp(startingHeightMeters, minHeightMeters, maxHeightMeters), 0)); } /** @@ -72,8 +78,8 @@ public class ElevatorSim extends LinearSystemSim { * @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 startingHeightMeters The starting height of the elevator. * @param simulateGravity Whether gravity should be simulated or not. - * @param measurementStdDevs The standard deviations of the measurements. */ public ElevatorSim( LinearSystem plant, @@ -83,43 +89,16 @@ public class ElevatorSim extends LinearSystemSim { double minHeightMeters, double maxHeightMeters, boolean simulateGravity, - Matrix measurementStdDevs) { - super(plant, measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; - m_drumRadius = drumRadiusMeters; - m_minHeight = minHeightMeters; - m_maxHeight = maxHeightMeters; - m_simulateGravity = simulateGravity; - } - - /** - * 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. - */ - public ElevatorSim( - DCMotor gearbox, - double gearing, - double carriageMassKg, - double drumRadiusMeters, - double minHeightMeters, - double maxHeightMeters, - boolean simulateGravity) { + double startingHeightMeters) { this( + plant, gearbox, gearing, - carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, + startingHeightMeters, null); } @@ -133,6 +112,7 @@ public class ElevatorSim extends LinearSystemSim { * @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. */ public ElevatorSim( @@ -143,16 +123,51 @@ public class ElevatorSim extends LinearSystemSim { double minHeightMeters, double maxHeightMeters, boolean simulateGravity, + double startingHeightMeters, Matrix measurementStdDevs) { - super( + this( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), + gearbox, + gearing, + drumRadiusMeters, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; - m_drumRadius = drumRadiusMeters; - m_minHeight = minHeightMeters; - m_maxHeight = maxHeightMeters; - m_simulateGravity = simulateGravity; + } + + /** + * 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); } /** 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 26a41bd479..72d1be4e62 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 @@ -43,6 +43,8 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngleRads The initial position of the Arm simulation in radians. + * @param measurementStdDevs The standard deviations of the measurements. */ public SingleJointedArmSim( LinearSystem plant, @@ -51,16 +53,18 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - boolean simulateGravity) { - this( - plant, - gearbox, - gearing, - armLengthMeters, - minAngleRads, - maxAngleRads, - simulateGravity, - null); + boolean simulateGravity, + double startingAngleRads, + Matrix measurementStdDevs) { + super(plant, measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_armLenMeters = armLengthMeters; + m_minAngle = minAngleRads; + m_maxAngle = maxAngleRads; + m_simulateGravity = simulateGravity; + + setState(VecBuilder.fill(startingAngleRads, 0)); } /** @@ -73,7 +77,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. - * @param measurementStdDevs The standard deviations of the measurements. + * @param startingAngleRads The initial position of the Arm simulation in radians. */ public SingleJointedArmSim( LinearSystem plant, @@ -83,14 +87,17 @@ public class SingleJointedArmSim extends LinearSystemSim { double minAngleRads, double maxAngleRads, boolean simulateGravity, - Matrix measurementStdDevs) { - super(plant, measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; - m_armLenMeters = armLengthMeters; - m_minAngle = minAngleRads; - m_maxAngle = maxAngleRads; - m_simulateGravity = simulateGravity; + double startingAngleRads) { + this( + plant, + gearbox, + gearing, + armLengthMeters, + minAngleRads, + maxAngleRads, + simulateGravity, + startingAngleRads, + null); } /** @@ -103,6 +110,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngleRads The initial position of the Arm simulation in radians. */ public SingleJointedArmSim( DCMotor gearbox, @@ -111,7 +119,8 @@ public class SingleJointedArmSim extends LinearSystemSim { double armLengthMeters, double minAngleRads, double maxAngleRads, - boolean simulateGravity) { + boolean simulateGravity, + double startingAngleRads) { this( gearbox, gearing, @@ -120,6 +129,7 @@ public class SingleJointedArmSim extends LinearSystemSim { minAngleRads, maxAngleRads, simulateGravity, + startingAngleRads, null); } @@ -133,6 +143,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * @param minAngleRads The minimum angle that the arm is capable of. * @param maxAngleRads The maximum angle that the arm is capable of. * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngleRads The initial position of the Arm simulation in radians. * @param measurementStdDevs The standard deviations of the measurements. */ public SingleJointedArmSim( @@ -143,16 +154,18 @@ public class SingleJointedArmSim extends LinearSystemSim { double minAngleRads, double maxAngleRads, boolean simulateGravity, + double startingAngleRads, Matrix measurementStdDevs) { - super( + this( LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), + gearbox, + gearing, + armLengthMeters, + minAngleRads, + maxAngleRads, + simulateGravity, + startingAngleRads, measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; - m_armLenMeters = armLengthMeters; - m_minAngle = minAngleRads; - m_maxAngle = maxAngleRads; - m_simulateGravity = simulateGravity; } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index b31769b5fd..336489a334 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -33,6 +33,7 @@ class ElevatorSimTest { 0.0, 3.0, true, + 0.0, VecBuilder.fill(0.01)); try (var motor = new PWMVictorSPX(0); @@ -71,6 +72,7 @@ class ElevatorSimTest { 0.0, 1.0, true, + 0.0, VecBuilder.fill(0.01)); for (int i = 0; i < 100; i++) { @@ -91,7 +93,8 @@ class ElevatorSimTest { @Test void testStability() { var sim = - new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false); + new ElevatorSim( + DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10, false, 0.0); sim.setState(VecBuilder.fill(0, 0)); sim.setInput(12); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index f14bcd177f..928fd3edc8 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java @@ -14,10 +14,18 @@ import org.junit.jupiter.api.Test; class SingleJointedArmSimTest { SingleJointedArmSim m_sim = new SingleJointedArmSim( - DCMotor.getVex775Pro(2), 300, 3.0, Units.inchesToMeters(30.0), -Math.PI, 0.0, true); + DCMotor.getVex775Pro(2), + 300, + 3.0, + Units.inchesToMeters(30.0), + -Math.PI, + 0.0, + true, + Math.PI / 2.0); @Test void testArmDisabled() { + // Reset Arm angle to 0 m_sim.setState(VecBuilder.fill(0.0, 0.0)); for (int i = 0; i < 12 / 0.02; i++) { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java index de464e8310..3cce47d58c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/subsystems/Arm.java @@ -50,6 +50,7 @@ public class Arm implements AutoCloseable { Constants.kMinAngleRads, Constants.kMaxAngleRads, true, + 0, VecBuilder.fill(Constants.kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick ); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java index 7ac9fd2b09..1c9d876764 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/subsystems/Elevator.java @@ -54,6 +54,7 @@ public class Elevator implements AutoCloseable { Constants.kMinElevatorHeightMeters, Constants.kMaxElevatorHeightMeters, true, + 0, VecBuilder.fill(0.01)); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); private final PWMSim m_motorSim = new PWMSim(m_motor); diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java index c2b5311141..0da5e3d930 100644 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/potentiometerpid/PotentiometerPIDTest.java @@ -55,6 +55,7 @@ class PotentiometerPIDTest { 0.0, Robot.kFullHeightMeters, true, + 0, null); m_analogSim = new AnalogInputSim(Robot.kPotChannel); m_motorSim = new PWMSim(Robot.kMotorChannel);