Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Dean Brettle
2024-02-23 14:13:43 -08:00
committed by GitHub
parent ca6e307ea5
commit 8b669330eb
2 changed files with 30 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
View File

@@ -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};
}