From d6571008a3d6f648e1b8e33f6fd820f3b42b072c Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:16:46 -0500 Subject: [PATCH 1/3] Use the timestamp now stored in Pose4d (be sure and update Thunder first) --- src/main/java/frc/robot/subsystems/Swerve.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f202139a..69d4aa91 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -66,9 +66,7 @@ public void simulationPeriodic() { public void periodic() { for (Limelight limelight : Limelight.filterLimelights(limelights)) { Pose4d pose = limelight.getAlliancePose(); - addVisionMeasurement(pose.toPose2d(), - Timer.getFPGATimestamp() - Units.millisecondsToSeconds(pose.getLatency()) - - VisionConstants.PROCESS_LATENCY); + addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp()); } LightningShuffleboard.setDouble("Swerve", "yaw", m_yawGetter.getValueAsDouble()); From 120eddc1193afd19fc229d1a309d4c410b53e4b1 Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:31:06 -0500 Subject: [PATCH 2/3] Include atomic method to filter and get pose --- src/main/java/frc/thunder | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index cc1afe9d..62e9221e 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit cc1afe9d9e9419b3dda674293ffb5b7480b2bd1a +Subproject commit 62e9221ef55b37e8c31e03e5357505e998eaf9a3 From 962bf42299b53b328c2a766d23838aedac9732e8 Mon Sep 17 00:00:00 2001 From: Patrick Hurley Date: Fri, 26 Jan 2024 13:35:19 -0500 Subject: [PATCH 3/3] Use new filteredPoses method to close race condition --- src/main/java/frc/robot/subsystems/Swerve.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 69d4aa91..0aa53e2d 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -64,8 +64,7 @@ public void simulationPeriodic() { @Override public void periodic() { - for (Limelight limelight : Limelight.filterLimelights(limelights)) { - Pose4d pose = limelight.getAlliancePose(); + for (Pose4d pose : Limelight.filteredPoses(limelights)) { addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp()); }