Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] FlywheelSim cleanup (#6629)

Browse Source 
This is a cleanup of the FlywheelSim class with a few added features.

- One FlywheelSim constructor that takes a plant, DCMotor, and a optional number of measurementStdDevs. The documentation now states how to construct the plant either through LinearSystemId.createFlywheelSystem or identifyVelocitySystem.

- The gearbox, gearing and moment of Inertia (J) are now private final fields. The gearing is determined from the plant in the constructor as well as the moment of inertia. There are getter methods that allow the flywheelSim to return the gearbox, gearing, and moment of inertia.

- The getCurrentDrawAmps function now uses m_x instead of getAngularVelocityRadPerSec in accordance with more accuracy and matches the patter in other sims.

- Added getter methods for the InputVoltage, angularAcceleration and torque

- (Java only) A third getter method for returning the AngularVelocity of the flywheel using a MutableMeasure as a backing field that is set when getAngularVelocity is called. This summarily returns the angularVelocity as just a Measure object. This allows the user of this class to handle unit conversions with less numerical manipulation. Alterations in C++ for this feature were not needed.
This commit is contained in:
Nicholas Armstrong
2024-05-24 19:05:14 -04:00
committed by GitHub
parent 8c107e4b75
commit f42bc45ee8
6 changed files with 225 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
View File

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