Merge pull request #109 from frc-862/use-timestamped-pose

Use the timestamp now stored in Pose4d (be sure and update Thunder first)

src/main/java/frc/robot/subsystems/Swerve.java

@@ -64,11 +64,8 @@ public void simulationPeriodic() {
 
     @Override
     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);
+        for (Pose4d pose : Limelight.filteredPoses(limelights)) {
+            addVisionMeasurement(pose.toPose2d(), pose.getFPGATimestamp());
         }
 
         LightningShuffleboard.setDouble("Swerve", "Robot Heading", getPigeon2().getAngle());