diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 98d0f8b3fa4..02817302147 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -13,17 +13,14 @@ using namespace frc; using namespace frc::sim; ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant, - const DCMotor& gearbox, double gearing, - units::meter_t drumRadius, units::meter_t minHeight, + const DCMotor& gearbox, units::meter_t minHeight, units::meter_t maxHeight, bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs) : LinearSystemSim(plant, measurementStdDevs), m_gearbox(gearbox), - m_drumRadius(drumRadius), m_minHeight(minHeight), m_maxHeight(maxHeight), - m_gearing(gearing), m_simulateGravity(simulateGravity) { SetState(startingHeight, 0_mps); } @@ -36,8 +33,21 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing, const std::array& measurementStdDevs) : ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass, drumRadius, gearing), - gearbox, gearing, drumRadius, minHeight, maxHeight, - simulateGravity, startingHeight, measurementStdDevs) {} + gearbox, minHeight, maxHeight, simulateGravity, + startingHeight, measurementStdDevs) {} + +template + requires std::same_as || + std::same_as +ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, + units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, + const std::array& measurementStdDevs) + : ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox, + minHeight, maxHeight, simulateGravity, startingHeight, + measurementStdDevs) {} void ElevatorSim::SetState(units::meter_t position, units::meters_per_second_t velocity) { @@ -74,10 +84,12 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor // is spinning 10x faster than the output. - // v = r w, so w = v / r + double kA = 1.0 / m_plant.B(1, 0); + using Kv_t = units::unit_t>>; + Kv_t Kv = Kv_t{kA * m_plant.A(1, 1)}; units::meters_per_second_t velocity{m_x(1)}; - units::radians_per_second_t motorVelocity = - velocity / m_drumRadius * m_gearing * 1_rad; + units::radians_per_second_t motorVelocity = velocity * Kv * m_gearbox.Kv; // Perform calculation and return. return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) * diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 705ddbcd251..3d5c43398e5 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -19,6 +19,15 @@ namespace frc::sim { */ class ElevatorSim : public LinearSystemSim<2, 1, 1> { public: + template + using Velocity_t = units::unit_t< + units::compound_unit>>; + + template + using Acceleration_t = units::unit_t>, + units::inverse>>; + /** * Constructs a simulated elevator mechanism. * @@ -27,10 +36,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * LinearSystemId::ElevatorSystem(). * @param gearbox The type of and number of motors in your * elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater - * than 1 represent reductions). - * @param drumRadius The radius of the drum that your cable is - * wrapped around. * @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. @@ -38,7 +43,6 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * @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, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); @@ -65,6 +69,28 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { bool simulateGravity, units::meter_t startingHeight, const std::array& measurementStdDevs = {0.0}); + /** + * Constructs a simulated elevator mechanism. + * + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in your + * elevator gearbox. + * @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. + */ + template + requires std::same_as || + std::same_as + ElevatorSim(decltype(1_V / Velocity_t(1)) kV, + decltype(1_V / Acceleration_t(1)) kA, + const DCMotor& gearbox, units::meter_t minHeight, + units::meter_t maxHeight, bool simulateGravity, + units::meter_t startingHeight, + const std::array& measurementStdDevs = {0.0}); using LinearSystemSim::SetState; /** @@ -146,10 +172,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { private: DCMotor m_gearbox; - units::meter_t m_drumRadius; units::meter_t m_minHeight; units::meter_t m_maxHeight; - double m_gearing; bool m_simulateGravity; }; } // namespace frc::sim 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 6d655f72b41..322d897479c 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 @@ -19,12 +19,6 @@ public class ElevatorSim extends LinearSystemSim { // Gearbox for the elevator. private final DCMotor m_gearbox; - // Gearing between the motors and the output. - private final double m_gearing; - - // The radius of the drum that the elevator spool is wrapped around. - private final double m_drumRadius; - // The min allowable height for the elevator. private final double m_minHeight; @@ -41,8 +35,6 @@ public class ElevatorSim extends LinearSystemSim { * {@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 gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @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. @@ -52,8 +44,6 @@ public class ElevatorSim extends LinearSystemSim { public ElevatorSim( LinearSystem plant, DCMotor gearbox, - double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -61,8 +51,6 @@ public ElevatorSim( Matrix measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_gearing = gearing; - m_drumRadius = drumRadiusMeters; m_minHeight = minHeightMeters; m_maxHeight = maxHeightMeters; m_simulateGravity = simulateGravity; @@ -77,8 +65,6 @@ public ElevatorSim( * {@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 gearing The gearing of the elevator (numbers greater than 1 represent reductions). - * @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. @@ -87,8 +73,6 @@ public ElevatorSim( public ElevatorSim( LinearSystem plant, DCMotor gearbox, - double gearing, - double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, boolean simulateGravity, @@ -96,8 +80,6 @@ public ElevatorSim( this( plant, gearbox, - gearing, - drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, @@ -105,6 +87,67 @@ public ElevatorSim( null); } + /** + * 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 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( + double kV, + double kA, + DCMotor gearbox, + double minHeightMeters, + double maxHeightMeters, + boolean simulateGravity, + double startingHeightMeters) { + this( + kV, + kA, + gearbox, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + null); + } + + /** + * 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 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( + double kV, + double kA, + DCMotor gearbox, + double minHeightMeters, + double maxHeightMeters, + boolean simulateGravity, + double startingHeightMeters, + Matrix measurementStdDevs) { + this( + LinearSystemId.identifyPositionSystem(kV, kA), + gearbox, + minHeightMeters, + maxHeightMeters, + simulateGravity, + startingHeightMeters, + measurementStdDevs); + } + /** * Creates a simulated elevator mechanism. * @@ -131,8 +174,6 @@ public ElevatorSim( this( LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), gearbox, - gearing, - drumRadiusMeters, minHeightMeters, maxHeightMeters, simulateGravity, @@ -253,7 +294,10 @@ public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output // v = r w, so w = v/r - double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing; + double kA = 1 / m_plant.getB().get(1, 0); + double kV = -m_plant.getA().get(1, 1) * kA; + double motorVelocityRadPerSec = + getVelocityMetersPerSecond() * kV * m_gearbox.KvRadPerSecPerVolt; var appliedVoltage = m_u.get(0, 0); return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage);