diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 28096255e55..0a4f53a65c2 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -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( - 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( - 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};