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

[sysid] Fix crash on negative feedforward gains (#6425)

Browse Source 
LinearSystemId's linear system factories throw on negative feedforward
gains, but SysId can compute the feedback gains just fine in that case.
Now we construct the system manually instead.

Fixes #6423.
This commit is contained in:
Tyler Veness
2024-03-10 17:43:02 -07:00
committed by GitHub
parent 973bb55e66
commit 7cd4a75323
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
View File

@@ -8,7 +8,6 @@
#include <frc/controller/LinearQuadraticRegulator.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/acceleration.h>
#include <units/velocity.h>
#include <units/voltage.h>
@@ -32,8 +31,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
// instabilities in the LQR.
if (Ka > 1E-7) {
// Create a position system from our feedforward gains.
auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
Kv_t(Kv), Ka_t(Ka));
frc::LinearSystem<2, 1, 1> system{
frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
frc::Matrixd<1, 1>{0.0}};
// Create an LQR with 2 states to control -- position and velocity.
frc::LinearQuadraticRegulator<2, 1> controller{
system, {params.qp, params.qv}, {params.r}, preset.period};
@@ -74,8 +75,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
}
// Create a velocity system from our feedforward gains.
auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
Kv_t(Kv), Ka_t(Ka));
frc::LinearSystem<1, 1, 1> system{
frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
// Create an LQR controller with 1 state -- velocity.
frc::LinearQuadraticRegulator<1, 1> controller{
system, {params.qv}, {params.r}, preset.period};