From 8b669330eb9be8c5de00838fd41e01e35570b82b Mon Sep 17 00:00:00 2001
From: Dean Brettle <dean@brettle.com>
Date: Fri, 23 Feb 2024 14:13:43 -0800
Subject: [PATCH] [sysid] Fix position feedback latency compensation (#6392)

---
 .../native/cpp/analysis/FeedbackAnalysis.cpp  |  5 ++--
 .../cpp/analysis/FeedbackAnalysisTest.cpp     | 27 +++++++++++++++++++
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
index 7f8a3765e3f..a4e6d2e3b09 100644
--- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
+++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
@@ -32,7 +32,8 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
     frc::LinearQuadraticRegulator<2, 1> controller{
         system, {params.qp, params.qv}, {params.r}, preset.period};
     // Compensate for any latency from sensor measurements, filtering, etc.
-    controller.LatencyCompensate(system, preset.period, 0.0_s);
+    controller.LatencyCompensate(system, preset.period,
+                                 preset.measurementDelay);
 
     return {controller.K(0, 0) * preset.outputConversionFactor,
             controller.K(0, 1) * preset.outputConversionFactor /
@@ -47,7 +48,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qp}, {params.r}, preset.period};
   // Compensate for any latency from sensor measurements, filtering, etc.
-  controller.LatencyCompensate(system, preset.period, 0.0_s);
+  controller.LatencyCompensate(system, preset.period, preset.measurementDelay);
 
   return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0};
 }
diff --git a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
index 44f664c800d..3e1474ef37e 100644
--- a/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
+++ b/sysid/src/test/native/cpp/analysis/FeedbackAnalysisTest.cpp
@@ -103,3 +103,30 @@ TEST(FeedbackAnalysisTest, VelocityREVConversion) {
   EXPECT_NEAR(Kp, 0.00241 / 3, 0.005);
   EXPECT_NEAR(Kd, 0.00, 0.05);
 }
+
+TEST(FeedbackAnalysisTest, Position) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(
+      sysid::presets::kDefault, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 6.41, 0.05);
+  EXPECT_NEAR(Kd, 2.48, 0.05);
+}
+
+TEST(FeedbackAnalysisTest, PositionWithLatencyCompensation) {
+  auto Kv = 3.060;
+  auto Ka = 0.327;
+
+  sysid::LQRParameters params{1, 1.5, 7};
+  sysid::FeedbackControllerPreset preset{sysid::presets::kDefault};
+
+  preset.measurementDelay = 10_ms;
+  auto [Kp, Kd] = sysid::CalculatePositionFeedbackGains(preset, params, Kv, Ka);
+
+  EXPECT_NEAR(Kp, 5.92, 0.05);
+  EXPECT_NEAR(Kd, 2.12, 0.05);
+}