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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-02-20 00:26:23 -08:00
parent da47f06d70 0c3c2c1fda
commit 98f933eca5
123 changed files with 4634 additions and 2291 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -20,6 +20,13 @@ units::volt_t ArmFeedforward::Calculate(
units::unit_t<Velocity> nextVelocity) const {
using VarMat = sleipnir::VariableMatrix;
// Small kₐ values make the solver ill-conditioned
if (kA < units::unit_t<ka_unit>{1e-1}) {
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
return kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * units::math::cos(currentAngle);
}
// Arm dynamics
Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};