diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp new file mode 100644 index 0000000000..7860e32098 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/simulation/DCMotorSim.h" + +#include + +#include "frc/system/plant/LinearSystemId.h" + +using namespace frc; +using namespace frc::sim; + +DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant, + const DCMotor& gearbox, double gearing, + 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) {} + +units::radian_t DCMotorSim::GetAngularPosition() const { + return units::radian_t{GetOutput(0)}; +} + +units::radians_per_second_t DCMotorSim::GetAngularVelocity() const { + return units::radians_per_second_t{GetOutput(1)}; +} + +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 + // is spinning 10x faster than the output. + return m_gearbox.Current(GetAngularVelocity() * m_gearing, + units::volt_t{m_u(0)}) * + wpi::sgn(m_u(0)); +} + +void DCMotorSim::SetInputVoltage(units::volt_t voltage) { + SetInput(Eigen::Vector{voltage.value()}); +} diff --git a/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h new file mode 100644 index 0000000000..b1388bd99a --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h @@ -0,0 +1,81 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include "frc/simulation/LinearSystemSim.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/plant/DCMotor.h" + +namespace frc::sim { +/** + * Represents a simulated DC motor mechanism. + */ +class DCMotorSim : public LinearSystemSim<2, 1, 2> { + public: + /** + * Creates a simulated DC motor mechanism. + * + * @param plant The linear system representing the DC motor. + * @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}); + + /** + * Returns the DC motor position. + * + * @return The DC motor position. + */ + units::radian_t GetAngularPosition() const; + + /** + * Returns the DC motor velocity. + * + * @return The DC motor velocity. + */ + units::radians_per_second_t GetAngularVelocity() const; + + /** + * Returns the DC motor current draw. + * + * @return The DC motor current draw. + */ + units::ampere_t GetCurrentDraw() const override; + + /** + * Sets the input voltage for the DC motor. + * + * @param voltage The input voltage. + */ + void SetInputVoltage(units::volt_t voltage); + + private: + DCMotor m_gearbox; + double m_gearing; +}; +} // namespace frc::sim diff --git a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp new file mode 100644 index 0000000000..efed939bb3 --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/Encoder.h" +#include "frc/RobotController.h" +#include "frc/controller/PIDController.h" +#include "frc/motorcontrol/PWMVictorSPX.h" +#include "frc/simulation/BatterySim.h" +#include "frc/simulation/DCMotorSim.h" +#include "frc/simulation/EncoderSim.h" +#include "frc/simulation/RoboRioSim.h" +#include "gtest/gtest.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}}; + + frc::Encoder encoder{0, 1}; + frc::sim::EncoderSim encoderSim{encoder}; + frc::PWMVictorSPX motor{0}; + + frc::sim::RoboRioSim::ResetData(); + encoderSim.ResetData(); + + // Spin-up + for (int i = 0; i < 100; i++) { + // RobotPeriodic runs first + motor.SetVoltage(12_V); + + // Then, SimulationPeriodic runs + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()})); + sim.SetInputVoltage(motor.Get() * + frc::RobotController::GetBatteryVoltage()); + sim.Update(20_ms); + encoderSim.SetRate(sim.GetAngularVelocity().value()); + } + + EXPECT_NEAR((gearbox.Kv * 12_V).value(), encoder.GetRate(), 0.1); + + // Decay + for (int i = 0; i < 100; i++) { + // RobotPeriodic runs first + motor.SetVoltage(0_V); + + // Then, SimulationPeriodic runs + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()})); + sim.SetInputVoltage(motor.Get() * + frc::RobotController::GetBatteryVoltage()); + sim.Update(20_ms); + encoderSim.SetRate(sim.GetAngularVelocity().value()); + } + + EXPECT_NEAR(0, encoder.GetRate(), 0.1); +} + +TEST(DCMotorSimTest, PositionFeedbackControl) { + frc::DCMotor gearbox = frc::DCMotor::NEO(1); + frc::sim::DCMotorSim sim{gearbox, 1.0, + units::kilogram_square_meter_t{0.0005}}; + + frc2::PIDController controller{0.04, 0.0, 0.001}; + + frc::Encoder encoder{0, 1}; + frc::sim::EncoderSim encoderSim{encoder}; + frc::PWMVictorSPX motor{0}; + + frc::sim::RoboRioSim::ResetData(); + encoderSim.ResetData(); + + for (int i = 0; i < 140; i++) { + // RobotPeriodic runs first + motor.Set(controller.Calculate(encoder.GetDistance(), 750)); + + // Then, SimulationPeriodic runs + frc::sim::RoboRioSim::SetVInVoltage( + frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()})); + sim.SetInputVoltage(motor.Get() * + frc::RobotController::GetBatteryVoltage()); + sim.Update(20_ms); + encoderSim.SetDistance(sim.GetAngularPosition().value()); + encoderSim.SetRate(sim.GetAngularVelocity().value()); + } + + EXPECT_NEAR(encoder.GetDistance(), 750, 1.0); + EXPECT_NEAR(encoder.GetRate(), 0, 0.1); +} 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 new file mode 100644 index 0000000000..957075de3b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java @@ -0,0 +1,145 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.math.Matrix; +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; + +/** Represents a simulated DC motor mechanism. */ +public class DCMotorSim extends LinearSystemSim { + // Gearbox for the DC motor. + private final DCMotor m_gearbox; + + // The gearing from the motors to the output. + private final double m_gearing; + + /** + * Creates a simulated DC motor mechanism. + * + * @param plant The linear system that represents the DC motor. + * @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). + */ + public DCMotorSim(LinearSystem plant, DCMotor gearbox, double gearing) { + super(plant); + m_gearbox = gearbox; + m_gearing = gearing; + } + + /** + * Creates a simulated DC motor mechanism. + * + * @param plant The linear system that represents the DC motor. + * @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. + */ + public DCMotorSim( + LinearSystem plant, + DCMotor gearbox, + double gearing, + Matrix 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, Matrix)} constructor. + */ + @SuppressWarnings("ParameterName") + public DCMotorSim(DCMotor gearbox, double gearing, double jKgMetersSquared) { + super(LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing)); + 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, Matrix)} constructor. + * @param measurementStdDevs The standard deviations of the measurements. + */ + @SuppressWarnings("ParameterName") + public DCMotorSim( + DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix measurementStdDevs) { + super( + LinearSystemId.createDCMotorSystem(gearbox, jKgMetersSquared, gearing), measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + } + + /** + * Returns the DC motor position. + * + * @return The DC motor position. + */ + public double getAngularPositionRad() { + return getOutput(0); + } + + /** + * Returns the DC motor position in rotations. + * + * @return The DC motor position in rotations. + */ + public double getAngularPositionRotations() { + return Units.radiansToRotations(getAngularPositionRad()); + } + + /** + * Returns the DC motor velocity. + * + * @return The DC motor velocity. + */ + public double getAngularVelocityRadPerSec() { + return getOutput(1); + } + + /** + * Returns the DC motor velocity in RPM. + * + * @return The DC motor velocity in RPM. + */ + public double getAngularVelocityRPM() { + return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec()); + } + + /** + * Returns the DC motor current draw. + * + * @return The DC motor current draw. + */ + @Override + 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 output + return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) + * Math.signum(m_u.get(0, 0)); + } + + /** + * Sets the input voltage for the DC motor. + * + * @param volts The input voltage. + */ + public void setInputVoltage(double volts) { + setInput(volts); + } +} 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 new file mode 100644 index 0000000000..fbe3af0a43 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +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.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX; +import org.junit.jupiter.api.Test; + +class DCMotorSimTest { + @Test + void testVoltageSteadyState() { + RoboRioSim.resetData(); + + DCMotor gearbox = DCMotor.getNEO(1); + DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005); + + try (var motor = new PWMVictorSPX(0); + var encoder = new Encoder(0, 1)) { + var encoderSim = new EncoderSim(encoder); + encoderSim.resetData(); + + for (int i = 0; i < 100; i++) { + motor.setVoltage(12); + + // ------ SimulationPeriodic() happens after user code ------- + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); + sim.update(0.020); + encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + } + + assertEquals(gearbox.KvRadPerSecPerVolt * 12, encoder.getRate(), 0.1); + + for (int i = 0; i < 100; i++) { + motor.setVoltage(0); + + // ------ SimulationPeriodic() happens after user code ------- + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); + sim.update(0.020); + encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + } + + assertEquals(0, encoder.getRate(), 0.1); + } + } + + @Test + void testPositionFeedbackControl() { + RoboRioSim.resetData(); + + DCMotor gearbox = DCMotor.getNEO(1); + DCMotorSim sim = new DCMotorSim(gearbox, 1.0, 0.0005); + + try (var motor = new PWMVictorSPX(0); + var encoder = new Encoder(0, 1); + var controller = new PIDController(0.04, 0.0, 0.001); ) { + var encoderSim = new EncoderSim(encoder); + encoderSim.resetData(); + + for (int i = 0; i < 140; i++) { + motor.set(controller.calculate(encoder.getDistance(), 750)); + + // ------ SimulationPeriodic() happens after user code ------- + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps())); + sim.setInputVoltage(motor.get() * RobotController.getBatteryVoltage()); + sim.update(0.020); + encoderSim.setDistance(sim.getAngularPositionRad()); + encoderSim.setRate(sim.getAngularVelocityRadPerSec()); + } + + assertEquals(750, encoder.getDistance(), 1.0); + assertEquals(0, encoder.getRate(), 0.1); + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java index 409be5d2f6..1a05eb6104 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java +++ b/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java @@ -89,6 +89,42 @@ public final class LinearSystemId { new Matrix<>(Nat.N1(), Nat.N1())); } + /** + * Create a state-space model of a DC motor system. The states of the system are [angular + * position, angular velocity], inputs are [voltage], and outputs are [angular position, angular + * velocity]. + * + * @param motor The motor (or gearbox) attached to the DC motor. + * @param jKgMetersSquared The moment of inertia J of the DC motor. + * @param G The reduction between motor and drum, as a ratio of output to input. + * @return A LinearSystem representing the given characterized constants. + * @throws IllegalArgumentException if jKgMetersSquared <= 0 or G <= 0. + */ + @SuppressWarnings("ParameterName") + public static LinearSystem createDCMotorSystem( + DCMotor motor, double jKgMetersSquared, double G) { + if (jKgMetersSquared <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); + } + if (G <= 0.0) { + throw new IllegalArgumentException("G must be greater than zero."); + } + + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()) + .fill( + 0, + 1, + 0, + -G + * G + * motor.KtNMPerAmp + / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)), + VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)), + Matrix.eye(Nat.N2()), + new Matrix<>(Nat.N2(), Nat.N1())); + } + /** * Create a state-space model of a differential drive drivetrain. In this model, the states are * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ. diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h index ef7c0a8bf5..26142bf889 100644 --- a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -320,6 +320,38 @@ class WPILIB_DLLEXPORT LinearSystemId { return LinearSystem<1, 1, 1>(A, B, C, D); } + /** + * Constructs the state-space model for a DC motor motor. + * + * States: [[angular position, angular velocity]] + * Inputs: [[voltage]] + * Outputs: [[angular position, angular velocity]] + * + * @param motor Instance of DCMotor. + * @param J Moment of inertia. + * @param G Gear ratio from motor to carriage. + * @throws std::domain_error if J <= 0 or G <= 0. + */ + static LinearSystem<2, 1, 2> DCMotorSystem(DCMotor motor, + units::kilogram_square_meter_t J, + double G) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (G <= 0.0) { + throw std::domain_error("G must be greater than zero."); + } + + Eigen::Matrix A{ + {0.0, 1.0}, + {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}}; + Eigen::Matrix B{0.0, (G * motor.Kt / (motor.R * J)).value()}; + Eigen::Matrix C{{1.0, 0.0}, {0.0, 1.0}}; + Eigen::Matrix D{0.0, 0.0}; + + return LinearSystem<2, 1, 2>(A, B, C, D); + } + /** * Constructs the state-space model for a drivetrain. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java index a9f5b09367..3f316b3ba5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java @@ -64,6 +64,19 @@ class LinearSystemIDTest { assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); } + @Test + void testDCMotorSystem() { + var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); + assertTrue( + model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001)); + + assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); + + assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001)); + + assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001)); + } + @Test void testIdentifyPositionSystem() { // By controls engineering in frc, diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp index 1fa12c2575..fbf86f1c57 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -49,6 +49,18 @@ TEST(LinearSystemIDTest, FlywheelSystem) { ASSERT_TRUE(model.D().isApprox(Eigen::Matrix{0.0}, 0.001)); } +TEST(LinearSystemIDTest, DCMotorSystem) { + auto model = frc::LinearSystemId::DCMotorSystem(frc::DCMotor::NEO(2), + 0.00032_kg_sq_m, 1.0); + ASSERT_TRUE(model.A().isApprox( + Eigen::Matrix{{0, 1}, {0, -26.87032}}, 0.001)); + ASSERT_TRUE( + model.B().isApprox(Eigen::Matrix{0, 1354.166667}, 0.001)); + ASSERT_TRUE(model.C().isApprox( + Eigen::Matrix{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); + ASSERT_TRUE(model.D().isApprox(Eigen::Matrix{0.0, 0.0}, 0.001)); +} + TEST(LinearSystemIDTest, IdentifyPositionSystem) { // By controls engineering in frc, // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u