diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 0a4f53a65c..7821812bd4 100644 --- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -42,9 +42,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( controller.LatencyCompensate(system, preset.period, preset.measurementDelay); - return {controller.K(0, 0) * preset.outputConversionFactor, - controller.K(0, 1) * preset.outputConversionFactor / - (preset.normalized ? 1 : preset.period.value())}; + return { + controller.K(0, 0) * preset.outputConversionFactor, + controller.K(0, 1) * preset.outputConversionFactor / + (preset.normalized ? 1 : units::second_t{preset.period}.value())}; } // This is our special model to avoid instabilities in the LQR. diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp index 3e1474ef37..511950ed96 100644 --- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp +++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp @@ -130,3 +130,16 @@ TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) { EXPECT_NEAR(Kp, 5.92, 0.05); EXPECT_NEAR(Kd, 2.12, 0.05); } + +TEST(FeedbackAnalysisTest, PositionREV) { + auto Kv = 3.060; + auto Ka = 0.327; + + sysid::LQRParameters params{1, 1.5, 7}; + + auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains( + sysid::presets::kREVNEOBuiltIn, params, Kv, Ka); + + EXPECT_NEAR(Kp, 0.30202, 0.05); + EXPECT_NEAR(Kd, 48.518, 0.05); +}