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

[wpimath] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,7 +41,7 @@ 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,
return m_gearbox.Current(units::radians_per_second_t{m_x(1)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

8
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -12,11 +12,11 @@
using namespace frc;
using namespace frc::sim;
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 2>& plant,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_minHeight(minHeight),
@@ -30,7 +30,7 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
gearbox, minHeight, maxHeight, simulateGravity,
@@ -44,7 +44,7 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox,
minHeight, maxHeight, simulateGravity, startingHeight,
measurementStdDevs) {}

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

@@ -36,7 +36,7 @@ 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
// is spinning 10x faster than the output.
return m_gearbox.Current(GetAngularVelocity() * m_gearing,
return m_gearbox.Current(units::radians_per_second_t{m_x(0)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

8
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -16,12 +16,12 @@ using namespace frc;
using namespace frc::sim;
SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
const LinearSystem<2, 1, 2>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim<2, 1, 2>(system, measurementStdDevs),
m_armLen(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
@@ -36,7 +36,7 @@ SingleJointedArmSim::SingleJointedArmSim(
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,