Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51: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());