[sysid] Fix wrong position Kd with unnormalized time (#6433)

wpilibsuite · Mar 13, 2024 · 3116f79 · 3116f79
1 parent 0e013dc
commit 3116f79
Show file tree

Hide file tree

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