diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 31024c3ae8..4f1daaa83e 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -12,19 +12,32 @@ using namespace frc; using namespace frc::sim; FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant, - const DCMotor& gearbox, double gearing, + const DCMotor& gearbox, const std::array& measurementStdDevs) : LinearSystemSim<1, 1, 1>(plant, measurementStdDevs), m_gearbox(gearbox), - m_gearing(gearing) {} + // By theorem 6.10.1 of + // https://file.tavsys.net/control/controls-engineering-in-frc.pdf, the + // flywheel state-space model is: + // + // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u + // A = -G²Kₜ/(KᵥRJ) + // B = GKₜ/(RJ) + // + // Solve for G. + // + // A/B = -G/Kᵥ + // G = -KᵥA/B + // + // Solve for J. + // + // B = GKₜ/(RJ) + // J = GKₜ/(RB) + m_gearing(-gearbox.Kv.value() * m_plant.A(0, 0) / m_plant.B(0, 0)), + m_j(m_gearing * gearbox.Kt.value() / + (gearbox.R.value() * m_plant.B(0, 0))) {} -FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing, - units::kilogram_square_meter_t moi, - const std::array& measurementStdDevs) - : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing), - gearbox, gearing, measurementStdDevs) {} - -void FlywheelSim::SetState(units::radians_per_second_t velocity) { +void FlywheelSim::SetVelocity(units::radians_per_second_t velocity) { LinearSystemSim::SetState(Vectord<1>{velocity.value()}); } @@ -32,6 +45,16 @@ units::radians_per_second_t FlywheelSim::GetAngularVelocity() const { return units::radians_per_second_t{GetOutput(0)}; } +units::radians_per_second_squared_t FlywheelSim::GetAngularAcceleration() + const { + return units::radians_per_second_squared_t{ + (m_plant.A() * m_x + m_plant.B() * m_u)(0, 0)}; +} + +units::newton_meter_t FlywheelSim::GetTorque() const { + return units::newton_meter_t{GetAngularAcceleration().value() * m_j.value()}; +} + units::ampere_t FlywheelSim::GetCurrentDraw() const { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor @@ -41,6 +64,10 @@ units::ampere_t FlywheelSim::GetCurrentDraw() const { wpi::sgn(m_u(0)); } +units::volt_t FlywheelSim::GetInputVoltage() const { + return units::volt_t{GetInput(0)}; +} + void FlywheelSim::SetInputVoltage(units::volt_t voltage) { SetInput(Vectord<1>{voltage.value()}); ClampInput(frc::RobotController::GetBatteryVoltage().value()); diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h index 32c8aa3311..13ca5a4081 100644 --- a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h @@ -4,8 +4,10 @@ #pragma once +#include #include #include +#include #include "frc/simulation/LinearSystemSim.h" #include "frc/system/LinearSystem.h" @@ -22,54 +24,59 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { * * @param plant The linear system representing the flywheel. This * system can be created with - * LinearSystemId::FlywheelSystem(). + * LinearSystemId::FlywheelSystem() or + * LinearSystemId::IdentifyVelocitySystem(). * @param gearbox The type of and number of motors in the flywheel * gearbox. - * @param gearing The gearing of the flywheel (numbers greater than - * 1 represent reductions). * @param measurementStdDevs The standard deviation of the measurement noise. */ FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox, - double gearing, - const std::array& measurementStdDevs = {0.0}); - - /** - * Creates a simulated flywheel mechanism. - * - * @param gearbox The type of and number of motors in the flywheel - * gearbox. - * @param gearing The gearing of the flywheel (numbers greater than - * 1 represent reductions). - * @param moi The moment of inertia of the flywheel. - * @param measurementStdDevs The standard deviation of the measurement noise. - */ - FlywheelSim(const DCMotor& gearbox, double gearing, - units::kilogram_square_meter_t moi, const std::array& measurementStdDevs = {0.0}); using LinearSystemSim::SetState; /** - * Sets the flywheel's state. + * Sets the flywheel's angular velocity. * * @param velocity The new velocity */ - void SetState(units::radians_per_second_t velocity); + void SetVelocity(units::radians_per_second_t velocity); /** - * Returns the flywheel velocity. + * Returns the flywheel's velocity. * - * @return The flywheel velocity. + * @return The flywheel's velocity. */ units::radians_per_second_t GetAngularVelocity() const; /** - * Returns the flywheel current draw. + * Returns the flywheel's acceleration. * - * @return The flywheel current draw. + * @return The flywheel's acceleration + */ + units::radians_per_second_squared_t GetAngularAcceleration() const; + + /** + * Returns the flywheel's torque. + * + * @return The flywheel's torque + */ + units::newton_meter_t GetTorque() const; + + /** + * Returns the flywheel's current draw. + * + * @return The flywheel's current draw. */ units::ampere_t GetCurrentDraw() const; + /** + * Gets the input voltage for the flywheel. + * + * @return The flywheel input voltage. + */ + units::volt_t GetInputVoltage() const; + /** * Sets the input voltage for the flywheel. * @@ -77,8 +84,24 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { */ void SetInputVoltage(units::volt_t voltage); + /** + * Returns the gearbox. + */ + DCMotor Gearbox() const { return m_gearbox; } + + /** + * Returns the gearing; + */ + double Gearing() const { return m_gearing; } + + /** + * Returns the moment of inertia + */ + units::kilogram_square_meter_t J() const { return m_j; } + private: DCMotor m_gearbox; double m_gearing; + units::kilogram_square_meter_t m_j; }; } // namespace frc::sim diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index 6d15fc4592..fbcab20879 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -25,7 +25,7 @@ TEST(StateSpaceSimTest, FlywheelSim) { const frc::LinearSystem<1, 1, 1> plant = frc::LinearSystemId::IdentifyVelocitySystem( 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq); - frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0}; + frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2)}; frc::PIDController controller{0.2, 0.0, 0.0}; frc::SimpleMotorFeedforward feedforward{ 0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq}; diff --git a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp index 86807e60fe..047f01a1d1 100644 --- a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp @@ -92,8 +92,11 @@ class Robot : public frc::TimedRobot { static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia = 0.5 * 1.5_lb * 4_in * 4_in; - frc::sim::FlywheelSim m_flywheelSim{frc::DCMotor::NEO(1), kFlywheelGearing, - kFlywheelMomentOfInertia}; + frc::DCMotor m_gearbox = frc::DCMotor::NEO(1); + frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem( + m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)}; + + frc::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox}; frc::sim::EncoderSim m_encoderSim{m_encoder}; }; 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 eb3d8e6390..225543a2b6 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 @@ -4,12 +4,19 @@ package edu.wpi.first.wpilibj.simulation; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; + 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; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated flywheel mechanism. */ @@ -20,84 +27,168 @@ public class FlywheelSim extends LinearSystemSim { // The gearing from the motors to the output. private final double m_gearing; + // The moment of inertia for the flywheel mechanism. + private final double m_jKgMetersSquared; + + // The angular velocity of the system. + private final MutableMeasure> m_angularVelocity = + MutableMeasure.zero(RadiansPerSecond); + + // The angular acceleration of the system. + private final MutableMeasure>> m_angularAcceleration = + MutableMeasure.zero(RadiansPerSecondPerSecond); + /** * Creates a simulated flywheel mechanism. * - * @param plant The linear system that represents the flywheel. + * @param plant The linear system that represents the flywheel. Use either {@link + * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants + * or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system + * characterization. * @param gearbox The type of and number of motors in the flywheel gearbox. - * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for velocity. */ public FlywheelSim( - LinearSystem plant, - DCMotor gearbox, - double gearing, - double... measurementStdDevs) { + LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_gearing = gearing; + + // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf, + // the flywheel state-space model is: + // + // dx/dt = -G²Kₜ/(KᵥRJ)x + (GKₜ)/(RJ)u + // A = -G²Kₜ/(KᵥRJ) + // B = GKₜ/(RJ) + // + // Solve for G. + // + // A/B = -G/Kᵥ + // G = -KᵥA/B + // + // Solve for J. + // + // B = GKₜ/(RJ) + // J = GKₜ/(RB) + m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0); + m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0)); } /** - * Creates a simulated flywheel mechanism. - * - * @param gearbox The type of and number of motors in the flywheel gearbox. - * @param gearing The gearing of the flywheel (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the - * {@link #FlywheelSim(LinearSystem, DCMotor, double, double...)} constructor. - * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no - * noise is desired. If present must have 1 element for velocity. - */ - public FlywheelSim( - DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { - super( - LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing), - measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; - } - - /** - * Sets the flywheel's state. + * Sets the flywheel's angular velocity. * * @param velocityRadPerSec The new velocity in radians per second. */ - public void setState(double velocityRadPerSec) { + public void setAngularVelocity(double velocityRadPerSec) { setState(VecBuilder.fill(velocityRadPerSec)); } /** - * Returns the flywheel velocity. + * Returns the gear ratio of the flywheel. * - * @return The flywheel velocity. + * @return the flywheel's gear ratio. + */ + public double getGearing() { + return m_gearing; + } + + /** + * Returns the moment of inertia in kilograms meters squared. + * + * @return The flywheel's moment of inertia. + */ + public double getJKgMetersSquared() { + return m_jKgMetersSquared; + } + + /** + * Returns the gearbox for the flywheel. + * + * @return The flywheel's gearbox. + */ + public DCMotor getGearBox() { + return m_gearbox; + } + + /** + * Returns the flywheel's velocity in Radians Per Second. + * + * @return The flywheel's velocity in Radians Per Second. */ public double getAngularVelocityRadPerSec() { return getOutput(0); } /** - * Returns the flywheel velocity in RPM. + * Returns the flywheel's velocity in RPM. * - * @return The flywheel velocity in RPM. + * @return The flywheel's velocity in RPM. */ public double getAngularVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(getOutput(0)); + return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); } /** - * Returns the flywheel current draw. + * Returns the flywheel's velocity. * - * @return The flywheel current draw. + * @return The flywheel's velocity + */ + public Measure> getAngularVelocity() { + m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec()); + return m_angularVelocity; + } + + /** + * Returns the flywheel's acceleration in Radians Per Second Squared. + * + * @return The flywheel's acceleration in Radians Per Second Squared. + */ + public double getAngularAccelerationRadPerSecSq() { + var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u)); + return acceleration.get(0, 0); + } + + /** + * Returns the flywheel's acceleration. + * + * @return The flywheel's acceleration. + */ + public Measure>> getAngularAcceleration() { + m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq()); + return m_angularAcceleration; + } + + /** + * Returns the flywheel's torque in Newton-Meters. + * + * @return The flywheel's torque in Newton-Meters. + */ + public double getTorqueNewtonMeters() { + return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + } + + /** + * Returns the flywheel's current draw. + * + * @return The flywheel's current draw. */ public double getCurrentDrawAmps() { // I = V / R - omega / (Kv * R) // Reductions are output over input, so a reduction of 2:1 means the motor is spinning // 2x faster than the flywheel - return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) + return m_gearbox.getCurrent(m_x.get(0, 0) * m_gearing, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } + /** + * Gets the input voltage for the flywheel. + * + * @return The flywheel input voltage. + */ + public double getInputVoltage() { + return getInput(0); + } + /** * Sets the input voltage for the flywheel. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index a025b6ad65..3bd0542904 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -6,7 +6,10 @@ package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Joystick; @@ -54,8 +57,12 @@ public class Robot extends TimedRobot { private static final double kFlywheelMomentOfInertia = 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); - private final FlywheelSim m_flywheelSim = - new FlywheelSim(DCMotor.getNEO(1), kFlywheelGearing, kFlywheelMomentOfInertia); + private final DCMotor m_gearbox = DCMotor.getNEO(1); + + private final LinearSystem m_plant = + LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia); + + private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); @Override