From 3db7354cb1aca84fa901aba1e6c0a304e1f33373 Mon Sep 17 00:00:00 2001
From: DarkMetalMouse <51059131+DarkMetalMouse@users.noreply.github.com>
Date: Tue, 7 Mar 2023 11:19:10 +0200
Subject: [PATCH] calculated latency since it is not returned by botpose

---
 src/main/java/frc/robot/subsystems/LimeLight.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/LimeLight.java b/src/main/java/frc/robot/subsystems/LimeLight.java
index 619846ab..57477e33 100644
--- a/src/main/java/frc/robot/subsystems/LimeLight.java
+++ b/src/main/java/frc/robot/subsystems/LimeLight.java
@@ -11,8 +11,8 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.LimelightConstants;
 
@@ -86,12 +86,12 @@ public BotPose getBotPose() {
         double[] poseComponents = limeLightTable.getEntry(entryName).getDoubleArray(new double[6]);
         if (poseComponents.length == 0)
             return null;
-
+        double latency = limeLightTable.getEntry("tl").getDouble(0) + limeLightTable.getEntry("cl").getDouble(0);
         return new BotPose(new Pose2d(
                 poseComponents[0],
                 poseComponents[1],
                 new Rotation2d(Math.toRadians(poseComponents[5]))),
-                Timer.getFPGATimestamp() - (poseComponents[6] / 1000.0));
+                Timer.getFPGATimestamp() - (latency / 1000.0));
 
     }
 }