diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp index f14564449a..dcceec9701 100644 --- a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -12,17 +12,30 @@ using namespace frc; using namespace frc::sim; DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant, - const DCMotor& gearbox, double gearing, + const DCMotor& gearbox, const std::array& measurementStdDevs) : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs), m_gearbox(gearbox), - m_gearing(gearing) {} - -DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing, - units::kilogram_square_meter_t moi, - const std::array& measurementStdDevs) - : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox, - gearing, measurementStdDevs) {} + // 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(1, 1) / m_plant.B(1, 0)), + m_j(m_gearing * gearbox.Kt.value() / + (gearbox.R.value() * m_plant.B(1, 0))) {} void DCMotorSim::SetState(units::radian_t angularPosition, units::radians_per_second_t angularVelocity) { @@ -37,6 +50,15 @@ units::radians_per_second_t DCMotorSim::GetAngularVelocity() const { return units::radians_per_second_t{GetOutput(1)}; } +units::radians_per_second_squared_t DCMotorSim::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 DCMotorSim::GetTorque() const { + return units::newton_meter_t{GetAngularAcceleration().value() * m_j.value()}; +} + units::ampere_t DCMotorSim::GetCurrentDraw() const { // I = V / R - omega / (Kv * R) // Reductions are greater than 1, so a reduction of 10:1 would mean the motor @@ -46,6 +68,10 @@ units::ampere_t DCMotorSim::GetCurrentDraw() const { wpi::sgn(m_u(0)); } +units::volt_t DCMotorSim::GetInputVoltage() const { + return units::volt_t{GetInput(0)}; +} + void DCMotorSim::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/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h index f054596dcc..c5c761845a 100644 --- a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h @@ -5,8 +5,10 @@ #pragma once #include +#include #include #include +#include #include "frc/simulation/LinearSystemSim.h" #include "frc/system/LinearSystem.h" @@ -27,26 +29,9 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { * radians. * @param gearbox The type of and number of motors in the DC motor * gearbox. - * @param gearing The gearing of the DC motor (numbers greater than - * 1 represent reductions). * @param measurementStdDevs The standard deviation of the measurement noise. */ DCMotorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox, - double gearing, - const std::array& measurementStdDevs = {0.0, 0.0}); - - /** - * Creates a simulated DC motor mechanism. - * - * @param gearbox The type of and number of motors in the DC motor - * gearbox. - * @param gearing The gearing of the DC motor (numbers greater than - * 1 represent reductions). - * @param moi The moment of inertia of the DC motor. - * @param measurementStdDevs The standard deviation of the measurement noise. - */ - DCMotorSim(const DCMotor& gearbox, double gearing, - units::kilogram_square_meter_t moi, const std::array& measurementStdDevs = {0.0, 0.0}); using LinearSystemSim::SetState; @@ -74,6 +59,20 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { */ units::radians_per_second_t GetAngularVelocity() const; + /** + * Returns the DC motor acceleration. + * + * @return The DC motor acceleration + */ + units::radians_per_second_squared_t GetAngularAcceleration() const; + + /** + * Returns the DC motor torque. + * + * @return The DC motor torque + */ + units::newton_meter_t GetTorque() const; + /** * Returns the DC motor current draw. * @@ -81,6 +80,13 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { */ units::ampere_t GetCurrentDraw() const; + /** + * Gets the input voltage for the DC motor. + * + * @return The DC motor input voltage. + */ + units::volt_t GetInputVoltage() const; + /** * Sets the input voltage for the DC motor. * @@ -88,8 +94,24 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { */ void SetInputVoltage(units::volt_t voltage); + /** + * Returns the gearbox. + */ + const 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/DCMotorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp index 75b8a4b560..caa152a927 100644 --- a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp @@ -12,11 +12,13 @@ #include "frc/simulation/DCMotorSim.h" #include "frc/simulation/EncoderSim.h" #include "frc/simulation/RoboRioSim.h" +#include "frc/system/plant/LinearSystemId.h" TEST(DCMotorSimTest, VoltageSteadyState) { frc::DCMotor gearbox = frc::DCMotor::NEO(1); - frc::sim::DCMotorSim sim{gearbox, 1.0, - units::kilogram_square_meter_t{0.0005}}; + auto plant = frc::LinearSystemId::DCMotorSystem( + frc::DCMotor::NEO(1), units::kilogram_square_meter_t{0.0005}, 1.0); + frc::sim::DCMotorSim sim{plant, gearbox}; frc::Encoder encoder{0, 1}; frc::sim::EncoderSim encoderSim{encoder}; @@ -60,8 +62,9 @@ TEST(DCMotorSimTest, VoltageSteadyState) { TEST(DCMotorSimTest, PositionFeedbackControl) { frc::DCMotor gearbox = frc::DCMotor::NEO(1); - frc::sim::DCMotorSim sim{gearbox, 1.0, - units::kilogram_square_meter_t{0.0005}}; + auto plant = frc::LinearSystemId::DCMotorSystem( + frc::DCMotor::NEO(1), units::kilogram_square_meter_t{0.0005}, 1.0); + frc::sim::DCMotorSim sim{plant, gearbox}; frc::PIDController controller{0.04, 0.0, 0.001}; 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 169c288612..4440a2d778 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 @@ -4,13 +4,22 @@ package edu.wpi.first.wpilibj.simulation; +import static edu.wpi.first.units.Units.Radians; +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.numbers.N2; 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.measure.Angle; +import edu.wpi.first.units.measure.AngularAcceleration; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularAcceleration; +import edu.wpi.first.units.measure.MutAngularVelocity; import edu.wpi.first.wpilibj.RobotController; /** Represents a simulated DC motor mechanism. */ @@ -21,6 +30,19 @@ public class DCMotorSim extends LinearSystemSim { // The gearing from the motors to the output. private final double m_gearing; + // The moment of inertia for the DC motor mechanism. + private final double m_jKgMetersSquared; + + // The angle of the system. + private final MutAngle m_angle = Radians.mutable(0.0); + + // The angular velocity of the system. + private final MutAngularVelocity m_angularVelocity = RadiansPerSecond.mutable(0.0); + + // The angular acceleration of the system. + private final MutAngularAcceleration m_angularAcceleration = + RadiansPerSecondPerSecond.mutable(0.0); + /** * Creates a simulated DC motor mechanism. * @@ -31,38 +53,32 @@ public class DCMotorSim extends LinearSystemSim { * {@link edu.wpi.first.math.system.plant.LinearSystemId#createDCMotorSystem(double, double)} * is used, the distance unit must be radians. * @param gearbox The type of and number of motors in the DC motor gearbox. - * @param gearing The gearing of the DC motor (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 2 elements. The first element is for position. The * second element is for velocity. */ - public DCMotorSim( - LinearSystem plant, - DCMotor gearbox, - double gearing, - double... measurementStdDevs) { + public DCMotorSim(LinearSystem plant, DCMotor gearbox, double... measurementStdDevs) { super(plant, measurementStdDevs); m_gearbox = gearbox; - m_gearing = gearing; - } - /** - * Creates a simulated DC motor mechanism. - * - * @param gearbox The type of and number of motors in the DC motor gearbox. - * @param gearing The gearing of the DC motor (numbers greater than 1 represent reductions). - * @param jKgMetersSquared The moment of inertia of the DC motor. If this is unknown, use the - * {@link #DCMotorSim(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 2 elements. The first element is for position. The - * second element is for velocity. - */ - public DCMotorSim( - DCMotor gearbox, double gearing, double jKgMetersSquared, double... measurementStdDevs) { - super( - LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs); - m_gearbox = gearbox; - m_gearing = gearing; + // By theorem 6.10.1 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf, + // the DC motor 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(1, 1) / plant.getB(1, 0); + m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0)); } /** @@ -76,45 +92,139 @@ public class DCMotorSim extends LinearSystemSim { } /** - * Returns the DC motor position. + * Sets the DC motor's angular position. * - * @return The DC motor position. + * @param angularPositionRad The new position in radians. + */ + public void setAngle(double angularPositionRad) { + setState(angularPositionRad, getAngularVelocityRadPerSec()); + } + + /** + * Sets the DC motor's angular velocity. + * + * @param angularVelocityRadPerSec The new velocity in radians per second. + */ + public void setAngularVelocity(double angularVelocityRadPerSec) { + setState(getAngularPositionRad(), angularVelocityRadPerSec); + } + + /** + * Returns the gear ratio of the DC motor. + * + * @return the DC motor's gear ratio. + */ + public double getGearing() { + return m_gearing; + } + + /** + * Returns the moment of inertia of the DC motor. + * + * @return The DC motor's moment of inertia. + */ + public double getJKgMetersSquared() { + return m_jKgMetersSquared; + } + + /** + * Returns the gearbox for the DC motor. + * + * @return The DC motor's gearbox. + */ + public DCMotor getGearbox() { + return m_gearbox; + } + + /** + * Returns the DC motor's position. + * + * @return The DC motor's position. */ public double getAngularPositionRad() { return getOutput(0); } /** - * Returns the DC motor position in rotations. + * Returns the DC motor's position in rotations. * - * @return The DC motor position in rotations. + * @return The DC motor's position in rotations. */ public double getAngularPositionRotations() { return Units.radiansToRotations(getAngularPositionRad()); } /** - * Returns the DC motor velocity. + * Returns the DC motor's position. * - * @return The DC motor velocity. + * @return The DC motor's position + */ + public Angle getAngularPosition() { + m_angle.mut_setMagnitude(getAngularPositionRad()); + return m_angle; + } + + /** + * Returns the DC motor's velocity. + * + * @return The DC motor's velocity. */ public double getAngularVelocityRadPerSec() { return getOutput(1); } /** - * Returns the DC motor velocity in RPM. + * Returns the DC motor's velocity in RPM. * - * @return The DC motor velocity in RPM. + * @return The DC motor's velocity in RPM. */ public double getAngularVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); } /** - * Returns the DC motor current draw. + * Returns the DC motor's velocity. * - * @return The DC motor current draw. + * @return The DC motor's velocity + */ + public AngularVelocity getAngularVelocity() { + m_angularVelocity.mut_setMagnitude(getAngularVelocityRadPerSec()); + return m_angularVelocity; + } + + /** + * Returns the DC motor's acceleration in Radians Per Second Squared. + * + * @return The DC motor'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 DC motor's acceleration. + * + * @return The DC motor's acceleration. + */ + public AngularAcceleration getAngularAcceleration() { + m_angularAcceleration.mut_setMagnitude(getAngularAccelerationRadPerSecSq()); + return m_angularAcceleration; + } + + /** + * Returns the DC motor's torque in Newton-Meters. + * + * @return The DC motor's torque in Newton-Meters. + */ + public double getTorqueNewtonMeters() { + return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared; + } + + /** + * Returns the DC motor's current draw. + * + * @return The DC motor's current draw. */ public double getCurrentDrawAmps() { // I = V / R - omega / (Kv * R) @@ -124,6 +234,15 @@ public class DCMotorSim extends LinearSystemSim { * Math.signum(m_u.get(0, 0)); } + /** + * Gets the input voltage for the DC motor. + * + * @return The DC motor's input voltage. + */ + public double getInputVoltage() { + return getInput(0); + } + /** * Sets the input voltage for the DC motor. * 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 372dec7456..f1fe29daf6 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 @@ -104,7 +104,7 @@ public class FlywheelSim extends LinearSystemSim { * * @return The flywheel's gearbox. */ - public DCMotor getGearBox() { + public DCMotor getGearbox() { return m_gearbox; } @@ -181,7 +181,7 @@ public class FlywheelSim extends LinearSystemSim { /** * Gets the input voltage for the flywheel. * - * @return The flywheel input voltage. + * @return The flywheel's input voltage. */ public double getInputVoltage() { return getInput(0); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java index 681e6b41c5..b7a4a6dd87 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java @@ -7,7 +7,11 @@ package edu.wpi.first.wpilibj.simulation; import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +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.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; @@ -19,7 +23,8 @@ class DCMotorSimTest { RoboRioSim.resetData(); DCMotor gearbox = DCMotor.getNEO(1); - DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005); + LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); + DCMotorSim sim = new DCMotorSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1)) { @@ -59,7 +64,8 @@ class DCMotorSimTest { RoboRioSim.resetData(); DCMotor gearbox = DCMotor.getNEO(1); - DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005); + LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); + DCMotorSim sim = new DCMotorSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); var encoder = new Encoder(0, 1);