Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] DCMotorSim cleanup/enhancement (#7021)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-10-11 01:09:22 -04:00
committed by GitHub
parent 5acb4109ff
commit 5d9a553104
6 changed files with 245 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -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<double, 2>& 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<double, 2>& 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());

56
wpilibc/src/main/native/include/frc/simulation/DCMotorSim.h
View File

@@ -5,8 +5,10 @@
#pragma once
#include <units/angle.h>
#include <units/angular_acceleration.h>
#include <units/angular_velocity.h>
#include <units/moment_of_inertia.h>
#include <units/torque.h>
#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<double, 2>& 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<double, 2>& 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

11
wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp
View File

@@ -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};

191
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -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<N2, N1, N2> {
// 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<N2, N1, N2> {
* {@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<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double... measurementStdDevs) {
public DCMotorSim(LinearSystem<N2, N1, N2> 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<N2, N1, N2> {
}
/**
* 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<N2, N1, N2> {
* 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.
*

4
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -104,7 +104,7 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
*
* @return The flywheel's gearbox.
*/
public DCMotor getGearBox() {
public DCMotor getGearbox() {
return m_gearbox;
}
@@ -181,7 +181,7 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
/**
* Gets the input voltage for the flywheel.
*
* @return The flywheel input voltage.
* @return The flywheel's input voltage.
*/
public double getInputVoltage() {
return getInput(0);

10
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DCMotorSimTest.java
View File

@@ -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<N2, N1, N2> 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<N2, N1, N2> 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);