[sysid] Fix position feedback latency compensation (wpilibsuite#6392)

KangarooKoala · Feb 23, 2024 · 8b66933 · 8b66933
1 parent ca6e307
commit 8b66933
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 2 deletions.
diff --git 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
@@ -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);
+}