From daa08688d52e719d5dac3d35af75d853f0fd9bf7 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Thu, 4 Apr 2024 23:01:14 -0400
Subject: [PATCH 1/3] switched to megatag 2 and limelight lib (new limelight
 update)

---
 src/main/java/frc/robot/Constants.java        |    5 +
 src/main/java/frc/robot/RobotContainer.java   |    5 +-
 .../subsystems/SwerveDriveSubsystem.java      |   48 +-
 .../frc/robot/subsystems/VisionSubsystem.java |   65 +-
 .../robot/utils/helpers/LimelightHelper.java  | 1059 +++++++++++++++--
 5 files changed, 1022 insertions(+), 160 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 81561beb..cea1f614 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -23,6 +23,11 @@
 public final class Constants {
   public static final Alliance SAFE_ALLIANCE = Alliance.Red;
 
+  public static class Limelights {
+    public static final String MAIN = "limelight-main";
+    public static final String INTAKE = "limelight-intake";
+  }
+
   public static class CAN {
     public static final int DRIVE_FRONT_LEFT = 1;
     public static final int ROT_FRONT_LEFT = 2;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0af5a85f..3a3a0159 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -39,6 +39,7 @@
 import frc.robot.subsystems.IntakeSubsystem.FeedMode;
 import frc.robot.subsystems.ShooterSubsystem.ShooterState;
 import frc.robot.utils.UtilFuncs;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
@@ -196,10 +197,10 @@ private void configureBindings() {
 
     // TESTING ONLY!!!
     _driveController.circle().onTrue(Commands.runOnce(() -> {
-      Optional<double[]> pose = _visionSubsystem.getBotposeBlue();
+      Optional<PoseEstimate> pose = _visionSubsystem.getBotposeBlue();
       
       if (pose.isPresent()) {
-        Pose2d botpose = UtilFuncs.ToPose(pose.get());
+        Pose2d botpose = pose.get().pose;
         _swerveSubsystem.resetPose(new Pose2d(botpose.getX(), botpose.getY(), _swerveSubsystem.getHeading()));
       }
     }, _swerveSubsystem));
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 2a969cd5..4b1b05ef 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -31,10 +31,13 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.Limelights;
 import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
 import frc.robot.utils.SwerveModule;
 import frc.robot.utils.UtilFuncs;
+import frc.robot.utils.helpers.LimelightHelper;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 
 /**
  * @author Peter Gutkovich
@@ -211,39 +214,28 @@ private boolean updateBotpose() {
       _backLeft.getPosition()
     });
 
-    Optional<double[]> visionBotpose = _visionSubsystem.getBotposeBlue();
+    Optional<PoseEstimate> visionBotpose = _visionSubsystem.getBotposeBlue();
 
     SmartDashboard.putBoolean("SEES TAG(S)", visionBotpose.isPresent());
 
     // UPDATE BOTPOSE WITH VISION
     if (visionBotpose.isPresent()) {
-      double[] llBotpose = visionBotpose.get();
-
-      double tagDistance = llBotpose[9];
-      double tagCount = llBotpose[7];
-
-      double xyStds = 0;
-      double yawStd = 9999999;
-
-      SmartDashboard.putNumber("TAG(S) DISTANCE", tagDistance);
-      SmartDashboard.putNumber("TAG COUNT", tagCount);
-
-      if (tagDistance <= FieldConstants.CLOSE_TAG_DISTANCE_THRESHOLD) {
-        if (tagCount >= 2) { xyStds = 0.25; yawStd = 9000; }
-        else { xyStds = 0.8; yawStd = 10000; }
-      }
-
-      else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) {
-        if (tagCount >= 2) { xyStds = 0.9; yawStd = 13000; }
-        else { xyStds = 1.2; yawStd = 16000; }
-      }
-
-      else {
-        return false;
-      }
-
-      visionPublisher.set(UtilFuncs.ToPose(llBotpose));
-      _estimator.addVisionMeasurement(UtilFuncs.ToPose(llBotpose), _visionSubsystem.getLatency(), VecBuilder.fill(xyStds, xyStds, yawStd));
+      LimelightHelper.SetRobotOrientation(
+        Limelights.MAIN,
+        getHeading().getDegrees(),
+        0,
+        0,
+        0,
+        0,
+        0
+      );
+
+      if (Math.toRadians(Math.abs(_gyro.getRate())) >= Math.PI * 2) return false;
+
+      _estimator.addVisionMeasurement(
+        visionBotpose.get().pose,
+        visionBotpose.get().latency
+      );
     }
 
     return true;
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
index f30c9b02..e6afb43b 100644
--- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -3,14 +3,11 @@
 
 import java.util.Optional;
 
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.node.ArrayNode;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Limelights;
 import frc.robot.utils.helpers.LimelightHelper;
+import frc.robot.utils.helpers.LimelightHelper.PoseEstimate;
 
 /**
  * @author Lucas Ou
@@ -18,9 +15,6 @@
  * @author Peter Gutkovich
  */
 public class VisionSubsystem extends SubsystemBase {
-  private final LimelightHelper _main = new LimelightHelper("limelight-main");
-  private final LimelightHelper _intake = new LimelightHelper("limelight-intake");
-
   /** Creates a new VisionSubsystem. */
   public VisionSubsystem() {}
 
@@ -34,63 +28,29 @@ public void periodic() {
     }
   }
 
-  /**
-   * Returns the latency from the last time data was sent from the main limelight. This
-   * should be used in the pose estimator.
-   */
-  public double getLatency() {
-    double tl = _main.getEntry("tl").getDouble(0);
-    double cl = _main.getEntry("cl").getDouble(0);
-
-    return Timer.getFPGATimestamp() - (tl / 1000.0) - (cl / 1000.0);
-  }
-
   /** Return a boolean for whether a tag is seen by main cam. */
   public boolean isApriltagVisible() {
-    double tv = _main.getEntry("tv").getDouble(0);
-
-    if (tv == 0) {
-      return false;
-    }
-
-    if (tv == 1) {
-      return true;
-    }
-
-    return false;
+    return LimelightHelper.getTV(Limelights.MAIN);
   }
 
   /** Return a boolean for whether a note is seen by intake cam. */
   public boolean isNoteVisible() {
-    double tv = _intake.getEntry("tv").getDouble(0);
-
-    if (tv == 0) {
-      return false;
-    }
-
-    if (tv == 1) {
-      return true;
-    }
-
-    return false;
+    return LimelightHelper.getTV(Limelights.INTAKE);
   }
 
   /**
    * Returns the NT "botpose_wpiblue" as an array from the limelight if tags are visible. 
    * 
-   * @return NT "botpose_wpiblue" limelight topic.
+   * @return NT "botpose_wpiblue" limelight topic. This uses megatag 2.
    * 
    * @see Optional
    */
-  public Optional<double[]> getBotposeBlue() {
+  public Optional<PoseEstimate> getBotposeBlue() {
     if (!isApriltagVisible()) return Optional.empty();
 
-    NetworkTableEntry botpose_entry = _main.getEntry("botpose_wpiblue");
-    if (!botpose_entry.exists()) return Optional.empty();
-
-    double[] botpose_array = botpose_entry.getDoubleArray(new double[11]);
+    PoseEstimate botpose_blue = LimelightHelper.getBotPoseEstimate_wpiBlue_MegaTag2(Limelights.MAIN);
 
-    return Optional.of(botpose_array);
+    return Optional.of(botpose_blue);
   }
 
   /**
@@ -101,14 +61,9 @@ public Optional<double[]> getBotposeBlue() {
   public Optional<double[]> getNoteAngles() {
     if (!isNoteVisible()) return Optional.empty();
 
-    ArrayNode targets = _intake.getNeuralTargets();
-    JsonNode target = targets.get(0);
-
-    if (target == null) return Optional.empty();
-
     double[] angles = {
-      target.get("tx").asDouble(0),
-      target.get("ty").asDouble(0)
+      LimelightHelper.getTX(Limelights.INTAKE),
+      LimelightHelper.getTY(Limelights.INTAKE)
     };
 
     return Optional.of(angles);
diff --git a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
index c7c9b3a1..7714fe67 100644
--- a/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
+++ b/src/main/java/frc/robot/utils/helpers/LimelightHelper.java
@@ -1,82 +1,991 @@
-/* Copyright (C) 2024 Team 334. All Rights Reserved.*/
-package frc.robot.utils.helpers;
+//LimelightHelpers v1.5.0 (March 27, 2024)
 
-import com.fasterxml.jackson.databind.JsonNode;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.fasterxml.jackson.databind.node.ArrayNode;
+package frc.robot.utils.helpers;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+import java.io.IOException;
+import java.net.HttpURLConnection;
+import java.net.MalformedURLException;
+import java.net.URL;
+import java.util.concurrent.CompletableFuture;
+
+import com.fasterxml.jackson.annotation.JsonFormat;
+import com.fasterxml.jackson.annotation.JsonFormat.Shape;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationFeature;
+import com.fasterxml.jackson.databind.ObjectMapper;
 
-/** A class that helps retrieve limelight info from network tables. */
 public class LimelightHelper {
-  private final NetworkTableInstance _inst = NetworkTableInstance.getDefault();
-  private final NetworkTable _limelight;
-
-  private final ObjectMapper _objectMapper = new ObjectMapper();
-
-  public LimelightHelper(String name) {
-    _limelight = _inst.getTable(name);
-  }
-
-  /**
-   * Returns a NetworkTableEntry from the limelight network table.
-   *
-   * @param name
-   *            The name of the entry.
-   */
-  public NetworkTableEntry getEntry(String name) {
-    return _limelight.getEntry(name);
-  }
-
-  /** Return json from limelight. */
-  public JsonNode getJson() {
-    String jsonString = getEntry("json").getString("");
-    try {
-      return _objectMapper.readTree(jsonString);
-    } catch (Exception e) {
-      throw new Error("Cannot Read JSON From Limelight.");
-    }
-  }
-
-  /** 
-   * Returns the neural targets from the limelight.
-   * 
-   * @see ArrayNode
-   */
-  public ArrayNode getNeuralTargets() {
-    ArrayNode targets = (ArrayNode) getJson().get("Results").get("Detector");
-    return targets;
-  }
-
-  /**
-   * Returns the AprilTag targets from the limelight.
-   *
-   * @see ArrayNode
-   */
-  public ArrayNode getTagTargets() {
-    ArrayNode tags = (ArrayNode) getJson().get("Results").get("Fiducial");
-
-    return tags;
-  }
-
-  /**
-   * Returns a JsonNode containing info of a tag.
-   *
-   * @param ID
-   *            The ID of the tag to look for.
-   * @see JsonNode
-   */
-  public JsonNode getTag(int ID) {
-    JsonNode tags = getTagTargets();
-
-    for (JsonNode tag : tags) {
-      if (tag.get("fID").asInt() == ID) {
-        return tag;
-      }
-    }
-
-    return null;
-  }
-}
+    public static class LimelightTarget_Retro {
+        
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private  double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("ts")
+        public double ts;
+
+        public LimelightTarget_Retro() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+
+    }
+
+    public static class LimelightTarget_Fiducial {
+
+        @JsonProperty("fID")
+        public double fiducialID;
+
+        @JsonProperty("fam")
+        public String fiducialFamily;
+
+        @JsonProperty("t6c_ts")
+        private double[] cameraPose_TargetSpace;
+
+        @JsonProperty("t6r_fs")
+        private double[] robotPose_FieldSpace;
+
+        @JsonProperty("t6r_ts")
+        private double[] robotPose_TargetSpace;
+
+        @JsonProperty("t6t_cs")
+        private double[] targetPose_CameraSpace;
+
+        @JsonProperty("t6t_rs")
+        private double[] targetPose_RobotSpace;
+
+        public Pose3d getCameraPose_TargetSpace()
+        {
+            return toPose3D(cameraPose_TargetSpace);
+        }
+        public Pose3d getRobotPose_FieldSpace()
+        {
+            return toPose3D(robotPose_FieldSpace);
+        }
+        public Pose3d getRobotPose_TargetSpace()
+        {
+            return toPose3D(robotPose_TargetSpace);
+        }
+        public Pose3d getTargetPose_CameraSpace()
+        {
+            return toPose3D(targetPose_CameraSpace);
+        }
+        public Pose3d getTargetPose_RobotSpace()
+        {
+            return toPose3D(targetPose_RobotSpace);
+        }
+
+        public Pose2d getCameraPose_TargetSpace2D()
+        {
+            return toPose2D(cameraPose_TargetSpace);
+        }
+        public Pose2d getRobotPose_FieldSpace2D()
+        {
+            return toPose2D(robotPose_FieldSpace);
+        }
+        public Pose2d getRobotPose_TargetSpace2D()
+        {
+            return toPose2D(robotPose_TargetSpace);
+        }
+        public Pose2d getTargetPose_CameraSpace2D()
+        {
+            return toPose2D(targetPose_CameraSpace);
+        }
+        public Pose2d getTargetPose_RobotSpace2D()
+        {
+            return toPose2D(targetPose_RobotSpace);
+        }
+        
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        @JsonProperty("ts")
+        public double ts;
+        
+        public LimelightTarget_Fiducial() {
+            cameraPose_TargetSpace = new double[6];
+            robotPose_FieldSpace = new double[6];
+            robotPose_TargetSpace = new double[6];
+            targetPose_CameraSpace = new double[6];
+            targetPose_RobotSpace = new double[6];
+        }
+    }
+
+    public static class LimelightTarget_Barcode {
+
+    }
+
+    public static class LimelightTarget_Classifier {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("zone")
+        public double zone;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        public  LimelightTarget_Classifier() {
+        }
+    }
+
+    public static class LimelightTarget_Detector {
+
+        @JsonProperty("class")
+        public String className;
+
+        @JsonProperty("classID")
+        public double classID;
+
+        @JsonProperty("conf")
+        public double confidence;
+
+        @JsonProperty("ta")
+        public double ta;
+
+        @JsonProperty("tx")
+        public double tx;
+
+        @JsonProperty("txp")
+        public double tx_pixels;
+
+        @JsonProperty("ty")
+        public double ty;
+
+        @JsonProperty("typ")
+        public double ty_pixels;
+
+        public LimelightTarget_Detector() {
+        }
+    }
+
+    public static class Results {
+
+        @JsonProperty("pID")
+        public double pipelineID;
+
+        @JsonProperty("tl")
+        public double latency_pipeline;
+
+        @JsonProperty("cl")
+        public double latency_capture;
+
+        public double latency_jsonParse;
+
+        @JsonProperty("ts")
+        public double timestamp_LIMELIGHT_publish;
+
+        @JsonProperty("ts_rio")
+        public double timestamp_RIOFPGA_capture;
+
+        @JsonProperty("v")
+        @JsonFormat(shape = Shape.NUMBER)
+        public boolean valid;
+
+        @JsonProperty("botpose")
+        public double[] botpose;
+
+        @JsonProperty("botpose_wpired")
+        public double[] botpose_wpired;
+
+        @JsonProperty("botpose_wpiblue")
+        public double[] botpose_wpiblue;
+
+        @JsonProperty("botpose_tagcount")
+        public double botpose_tagcount;
+       
+        @JsonProperty("botpose_span")
+        public double botpose_span;
+       
+        @JsonProperty("botpose_avgdist")
+        public double botpose_avgdist;
+       
+        @JsonProperty("botpose_avgarea")
+        public double botpose_avgarea;
+
+        @JsonProperty("t6c_rs")
+        public double[] camerapose_robotspace;
+
+        public Pose3d getBotPose3d() {
+            return toPose3D(botpose);
+        }
+    
+        public Pose3d getBotPose3d_wpiRed() {
+            return toPose3D(botpose_wpired);
+        }
+    
+        public Pose3d getBotPose3d_wpiBlue() {
+            return toPose3D(botpose_wpiblue);
+        }
+
+        public Pose2d getBotPose2d() {
+            return toPose2D(botpose);
+        }
+    
+        public Pose2d getBotPose2d_wpiRed() {
+            return toPose2D(botpose_wpired);
+        }
+    
+        public Pose2d getBotPose2d_wpiBlue() {
+            return toPose2D(botpose_wpiblue);
+        }
+
+        @JsonProperty("Retro")
+        public LimelightTarget_Retro[] targets_Retro;
+
+        @JsonProperty("Fiducial")
+        public LimelightTarget_Fiducial[] targets_Fiducials;
+
+        @JsonProperty("Classifier")
+        public LimelightTarget_Classifier[] targets_Classifier;
+
+        @JsonProperty("Detector")
+        public LimelightTarget_Detector[] targets_Detector;
+
+        @JsonProperty("Barcode")
+        public LimelightTarget_Barcode[] targets_Barcode;
+
+        public Results() {
+            botpose = new double[6];
+            botpose_wpired = new double[6];
+            botpose_wpiblue = new double[6];
+            camerapose_robotspace = new double[6];
+            targets_Retro = new LimelightTarget_Retro[0];
+            targets_Fiducials = new LimelightTarget_Fiducial[0];
+            targets_Classifier = new LimelightTarget_Classifier[0];
+            targets_Detector = new LimelightTarget_Detector[0];
+            targets_Barcode = new LimelightTarget_Barcode[0];
+
+        }
+    }
+
+    public static class LimelightResults {
+        @JsonProperty("Results")
+        public Results targetingResults;
+        
+        public String error;
+
+        public LimelightResults() {
+            targetingResults = new Results();
+            error = "";
+        }
+
+
+    }
+
+    public static class RawFiducial {
+        public int id;
+        public double txnc;
+        public double tync;
+        public double ta;
+        public double distToCamera;
+        public double distToRobot;
+        public double ambiguity;
+
+
+        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
+            this.id = id;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.distToCamera = distToCamera;
+            this.distToRobot = distToRobot;
+            this.ambiguity = ambiguity;
+        }
+    }
+
+    public static class PoseEstimate {
+        public Pose2d pose;
+        public double timestampSeconds;
+        public double latency;
+        public int tagCount;
+        public double tagSpan;
+        public double avgTagDist;
+        public double avgTagArea;
+        public RawFiducial[] rawFiducials; 
+
+        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, 
+            int tagCount, double tagSpan, double avgTagDist, 
+            double avgTagArea, RawFiducial[] rawFiducials) {
+
+            this.pose = pose;
+            this.timestampSeconds = timestampSeconds;
+            this.latency = latency;
+            this.tagCount = tagCount;
+            this.tagSpan = tagSpan;
+            this.avgTagDist = avgTagDist;
+            this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
+        }
+    }
+
+    private static ObjectMapper mapper;
+
+    /**
+     * Print JSON Parse time to the console in milliseconds
+     */
+    static boolean profileJSON = false;
+
+    static final String sanitizeName(String name) {
+        if (name == "" || name == null) {
+            return "limelight";
+        }
+        return name;
+    }
+
+    private static Pose3d toPose3D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 3D Pose Data!");
+            return new Pose3d();
+        }
+        return new Pose3d(
+            new Translation3d(inData[0], inData[1], inData[2]),
+            new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]),
+                    Units.degreesToRadians(inData[5])));
+    }
+
+    private static Pose2d toPose2D(double[] inData){
+        if(inData.length < 6)
+        {
+            //System.err.println("Bad LL 2D Pose Data!");
+            return new Pose2d();
+        }
+        Translation2d tran2d = new Translation2d(inData[0], inData[1]);
+        Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5]));
+        return new Pose2d(tran2d, r2d);
+    }
+
+    private static double extractBotPoseEntry(double[] inData, int position){
+        if(inData.length < position+1)
+        {
+            return 0;
+        }
+        return inData[position];
+    }
+
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
+        var poseEntry = LimelightHelper.getLimelightNTTableEntry(limelightName, entryName);
+        var poseArray = poseEntry.getDoubleArray(new double[0]);
+        var pose = toPose2D(poseArray);
+        double latency = extractBotPoseEntry(poseArray,6);
+        int tagCount = (int)extractBotPoseEntry(poseArray,7);
+        double tagSpan = extractBotPoseEntry(poseArray,8);
+        double tagDist = extractBotPoseEntry(poseArray,9);
+        double tagArea = extractBotPoseEntry(poseArray,10);
+        //getlastchange() in microseconds, ll latency in milliseconds
+        var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0);
+
+
+        RawFiducial[] rawFiducials = new RawFiducial[tagCount];
+        int valsPerFiducial = 7;
+        int expectedTotalVals = 11 + valsPerFiducial*tagCount;
+
+        if (poseArray.length != expectedTotalVals) {
+            // Don't populate fiducials
+        }
+        else{
+            for(int i = 0; i < tagCount; i++) {
+                int baseIndex = 11 + (i * valsPerFiducial);
+                int id = (int)poseArray[baseIndex];
+                double txnc = poseArray[baseIndex + 1];
+                double tync = poseArray[baseIndex + 2];
+                double ta = poseArray[baseIndex + 3];
+                double distToCamera = poseArray[baseIndex + 4];
+                double distToRobot = poseArray[baseIndex + 5];
+                double ambiguity = poseArray[baseIndex + 6];
+                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+            }
+        }
+
+        return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
+    }
+
+    private static void printPoseEstimate(PoseEstimate pose) {
+        if (pose == null) {
+            System.out.println("No PoseEstimate available.");
+            return;
+        }
+    
+        System.out.printf("Pose Estimate Information:%n");
+        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+        System.out.printf("Latency: %.3f ms%n", pose.latency);
+        System.out.printf("Tag Count: %d%n", pose.tagCount);
+        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.println();
+    
+        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+            System.out.println("No RawFiducials data available.");
+            return;
+        }
+    
+        System.out.println("Raw Fiducials Details:");
+        for (int i = 0; i < pose.rawFiducials.length; i++) {
+            RawFiducial fiducial = pose.rawFiducials[i];
+            System.out.printf(" Fiducial #%d:%n", i + 1);
+            System.out.printf("  ID: %d%n", fiducial.id);
+            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
+            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
+            System.out.printf("  TA: %.2f%n", fiducial.ta);
+            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
+            System.out.println();
+        }
+    }
+
+    public static NetworkTable getLimelightNTTable(String tableName) {
+        return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
+    }
+
+    public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) {
+        return getLimelightNTTable(tableName).getEntry(entryName);
+    }
+
+    public static double getLimelightNTDouble(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0);
+    }
+
+    public static void setLimelightNTDouble(String tableName, String entryName, double val) {
+        getLimelightNTTableEntry(tableName, entryName).setDouble(val);
+    }
+
+    public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) {
+        getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val);
+    }
+
+    public static double[] getLimelightNTDoubleArray(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]);
+    }
+
+    public static String getLimelightNTString(String tableName, String entryName) {
+        return getLimelightNTTableEntry(tableName, entryName).getString("");
+    }
+
+    public static URL getLimelightURLString(String tableName, String request) {
+        String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request;
+        URL url;
+        try {
+            url = new URL(urlString);
+            return url;
+        } catch (MalformedURLException e) {
+            System.err.println("bad LL URL");
+        }
+        return null;
+    }
+    /////
+    /////
+
+    public static double getTX(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tx");
+    }
+
+    public static double getTY(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ty");
+    }
+
+    public static double getTA(String limelightName) {
+        return getLimelightNTDouble(limelightName, "ta");
+    }
+
+    public static double getLatency_Pipeline(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tl");
+    }
+
+    public static double getLatency_Capture(String limelightName) {
+        return getLimelightNTDouble(limelightName, "cl");
+    }
+
+    public static double getCurrentPipelineIndex(String limelightName) {
+        return getLimelightNTDouble(limelightName, "getpipe");
+    }
+
+    public static String getJSONDump(String limelightName) {
+        return getLimelightNTString(limelightName, "json");
+    }
+
+    /**
+     * Switch to getBotPose
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    /**
+     * Switch to getBotPose_wpiRed
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    /**
+     * Switch to getBotPose_wpiBlue
+     * 
+     * @param limelightName
+     * @return
+     */
+    @Deprecated
+    public static double[] getBotpose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose");
+    }
+
+    public static double[] getBotPose_wpiRed(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+    }
+
+    public static double[] getBotPose_wpiBlue(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+    }
+
+    public static double[] getBotPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+    }
+
+    public static double[] getCameraPose_TargetSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+    }
+
+    public static double[] getTargetPose_CameraSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+    }
+
+    public static double[] getTargetPose_RobotSpace(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+    }
+
+    public static double[] getTargetColor(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "tc");
+    }
+
+    public static double getFiducialID(String limelightName) {
+        return getLimelightNTDouble(limelightName, "tid");
+    }
+
+    public static String getNeuralClassID(String limelightName) {
+        return getLimelightNTString(limelightName, "tclass");
+    }
+
+    /////
+    /////
+
+    public static Pose3d getBotPose3d(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_wpiRed(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_wpiBlue(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getBotPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getCameraPose3d_TargetSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getTargetPose3d_CameraSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getTargetPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    public static Pose3d getCameraPose3d_RobotSpace(String limelightName) {
+        double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace");
+        return toPose3D(poseArray);
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
+
+        double[] result = getBotPose_wpiBlue(limelightName);
+        return toPose2D(result);
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue");
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d_wpiRed(String limelightName) {
+
+        double[] result = getBotPose_wpiRed(limelightName);
+        return toPose2D(result);
+
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpired");
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
+    }
+
+    /**
+     * Gets the Pose2d for easy use with Odometry vision pose estimator
+     * (addVisionMeasurement)
+     * 
+     * @param limelightName
+     * @return
+     */
+    public static Pose2d getBotPose2d(String limelightName) {
+
+        double[] result = getBotPose(limelightName);
+        return toPose2D(result);
+
+    }
+
+    public static boolean getTV(String limelightName) {
+        return 1.0 == getLimelightNTDouble(limelightName, "tv");
+    }
+
+    /////
+    /////
+
+    public static void setPipelineIndex(String limelightName, int pipelineIndex) {
+        setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
+    }
+
+    
+    public static void setPriorityTagID(String limelightName, int ID) {
+        setLimelightNTDouble(limelightName, "priorityid", ID);
+    }
+
+    /**
+     * The LEDs will be controlled by Limelight pipeline settings, and not by robot
+     * code.
+     */
+    public static void setLEDMode_PipelineControl(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 0);
+    }
+
+    public static void setLEDMode_ForceOff(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 1);
+    }
+
+    public static void setLEDMode_ForceBlink(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 2);
+    }
+
+    public static void setLEDMode_ForceOn(String limelightName) {
+        setLimelightNTDouble(limelightName, "ledMode", 3);
+    }
+
+    public static void setStreamMode_Standard(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 0);
+    }
+
+    public static void setStreamMode_PiPMain(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 1);
+    }
+
+    public static void setStreamMode_PiPSecondary(String limelightName) {
+        setLimelightNTDouble(limelightName, "stream", 2);
+    }
+
+    public static void setCameraMode_Processor(String limelightName) {
+        setLimelightNTDouble(limelightName, "camMode", 0);
+    }
+    public static void setCameraMode_Driver(String limelightName) {
+        setLimelightNTDouble(limelightName, "camMode", 1);
+    }
+
+
+    /**
+     * Sets the crop window. The crop window in the UI must be completely open for
+     * dynamic cropping to work.
+     */
+    public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) {
+        double[] entries = new double[4];
+        entries[0] = cropXMin;
+        entries[1] = cropXMax;
+        entries[2] = cropYMin;
+        entries[3] = cropYMax;
+        setLimelightNTDoubleArray(limelightName, "crop", entries);
+    }
+
+    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, 
+        double pitch, double pitchRate, 
+        double roll, double rollRate) {
+
+        double[] entries = new double[6];
+        entries[0] = yaw;
+        entries[1] = yawRate;
+        entries[2] = pitch;
+        entries[3] = pitchRate;
+        entries[4] = roll;
+        entries[5] = rollRate;
+        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+    }
+
+    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+        double[] validIDsDouble = new double[validIDs.length];
+        for (int i = 0; i < validIDs.length; i++) {
+            validIDsDouble[i] = validIDs[i];
+        }        
+        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+    }
+    
+    public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
+        double[] entries = new double[6];
+        entries[0] = forward;
+        entries[1] = side;
+        entries[2] = up;
+        entries[3] = roll;
+        entries[4] = pitch;
+        entries[5] = yaw;
+        setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries);
+    }
+
+    /////
+    /////
+
+    public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) {
+        setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData);
+    }
+
+    public static double[] getPythonScriptData(String limelightName) {
+        return getLimelightNTDoubleArray(limelightName, "llpython");
+    }
+
+    /////
+    /////
+
+    /**
+     * Asynchronously take snapshot.
+     */
+    public static CompletableFuture<Boolean> takeSnapshot(String tableName, String snapshotName) {
+        return CompletableFuture.supplyAsync(() -> {
+            return SYNCH_TAKESNAPSHOT(tableName, snapshotName);
+        });
+    }
+
+    private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) {
+        URL url = getLimelightURLString(tableName, "capturesnapshot");
+        try {
+            HttpURLConnection connection = (HttpURLConnection) url.openConnection();
+            connection.setRequestMethod("GET");
+            if (snapshotName != null && snapshotName != "") {
+                connection.setRequestProperty("snapname", snapshotName);
+            }
+
+            int responseCode = connection.getResponseCode();
+            if (responseCode == 200) {
+                return true;
+            } else {
+                System.err.println("Bad LL Request");
+            }
+        } catch (IOException e) {
+            System.err.println(e.getMessage());
+        }
+        return false;
+    }
+
+    /**
+     * Parses Limelight's JSON results dump into a LimelightResults Object
+     */
+    public static LimelightResults getLatestResults(String limelightName) {
+
+        long start = System.nanoTime();
+        LimelightHelper.LimelightResults results = new LimelightHelper.LimelightResults();
+        if (mapper == null) {
+            mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false);
+        }
+
+        try {
+            results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
+        } catch (JsonProcessingException e) {
+            results.error = "lljson error: " + e.getMessage();
+        }
+
+        long end = System.nanoTime();
+        double millis = (end - start) * .000001;
+        results.targetingResults.latency_jsonParse = millis;
+        if (profileJSON) {
+            System.out.printf("lljson: %.2f\r\n", millis);
+        }
+
+        return results;
+    }
+}
\ No newline at end of file

From 4f9cc786c6a175e213823ba61c8bf742cda7aab6 Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 5 Apr 2024 09:01:45 -0400
Subject: [PATCH 2/3] changed timestamp to be correct for limelight

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

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 4b1b05ef..5a3f676f 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -101,7 +101,7 @@ Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(),
     new SwerveModulePosition[] {_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(), _backLeft.getPosition()},
     new Pose2d(), 
     VecBuilder.fill(0.03, 0.03, 0.01), 
-    VecBuilder.fill(0.9, 0.9, 9999999)
+    VecBuilder.fill(0.4, 0.4, 9999999)
   );
 
   // OTHER POSSIBLE STD DEV VALUES:
@@ -230,11 +230,11 @@ private boolean updateBotpose() {
         0
       );
 
-      if (Math.toRadians(Math.abs(_gyro.getRate())) >= Math.PI * 2) return false;
+      if (Math.abs(_gyro.getRate()) >= 500) return false;
 
       _estimator.addVisionMeasurement(
         visionBotpose.get().pose,
-        visionBotpose.get().latency
+        visionBotpose.get().timestampSeconds
       );
     }
 

From 2f2eb08d013517902b7e96b550a6957134f5805e Mon Sep 17 00:00:00 2001
From: PGgit08 <peter.mty8@gmail.com>
Date: Fri, 5 Apr 2024 10:24:39 -0400
Subject: [PATCH 3/3] got it to work

---
 src/main/java/frc/robot/Robot.java                     | 10 +++++++---
 .../frc/robot/subsystems/SwerveDriveSubsystem.java     |  4 ++++
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index fd5f5e59..f6f1a2cf 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -39,9 +39,13 @@ public void robotInit() {
     // for (int port = 5800; port <= 5807; port++) {
     //   PortForwarder.add(port, "limelight.local", port);
     // }
-    PortForwarder.add(5800, "limelight.local", 5800);
-    PortForwarder.add(5801, "limelight.local", 5801);
-    PortForwarder.add(5805, "limelight.local", 5805);
+    PortForwarder.add(5800, "limelight-main.local", 5800);
+    PortForwarder.add(5801, "limelight-main.local", 5801);
+    PortForwarder.add(5805, "limelight-main.local", 5805);
+
+    // PortForwarder.add(5810, "limelight-intake.local", 5810);
+    // PortForwarder.add(5811, "limelight-intake.local", 5811);
+    // PortForwarder.add(5815, "limelight-intake.local", 5815);
 
     // CameraServer.startAutomaticCapture(0);
     UtilFuncs.LoadField();
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index 5a3f676f..c5b87383 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -220,6 +220,8 @@ private boolean updateBotpose() {
 
     // UPDATE BOTPOSE WITH VISION
     if (visionBotpose.isPresent()) {
+      System.out.println("PRESENT");
+
       LimelightHelper.SetRobotOrientation(
         Limelights.MAIN,
         getHeading().getDegrees(),
@@ -232,6 +234,8 @@ private boolean updateBotpose() {
 
       if (Math.abs(_gyro.getRate()) >= 500) return false;
 
+      visionPublisher.set(visionBotpose.get().pose);
+
       _estimator.addVisionMeasurement(
         visionBotpose.get().pose,
         visionBotpose.get().timestampSeconds