Fixed hazardous storing an dupdating of limelight pose

src/main/java/frc/robot/subsystems/limelight/Limelight.java

@@ -14,6 +14,7 @@
 public class Limelight implements LimelightIO {
   private String limelightName;
   private NetworkTable networkTable;
+  private LimelightPoseData limelightPoseData;
 
   /**
    * Creates a new limelight
@@ -33,6 +34,11 @@ public Limelight(String limelightName) {
   public void updateData(LimelightIOData limelightIOData) {
     NetworkTableEntry robotPoseEntry;
 
+    // Creates a new limelight pose data instance if it doesn't exist
+    if (this.limelightPoseData == null) {
+      this.limelightPoseData = new LimelightPoseData(new double[7]);
+    }
+
     // Sets the pose of the limelight based on what alliance we are on
     if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
       robotPoseEntry = this.networkTable.getEntry("botpose_wpiblue");
@@ -42,8 +48,8 @@ public void updateData(LimelightIOData limelightIOData) {
       robotPoseEntry = this.networkTable.getEntry("botpose");
     }
 
-    LimelightPoseData limelightPoseData =
-        new LimelightPoseData(robotPoseEntry.getDoubleArray(new double[7]));
+    // Update the limelight pose data
+    this.limelightPoseData.update(robotPoseEntry.getDoubleArray(new double[7]));
 
     // Create a 3d pose from data from the limelight
     Pose3d limelightPose = limelightPoseData.toPose3d();

src/main/java/frc/robot/subsystems/limelight/LimelightPoseData.java

@@ -26,31 +26,77 @@ public LimelightPoseData(double[] data) {
     this.totalLatency = data[6];
   }
 
+  /**
+   * Updates the pose data without creating a new instance of limelight pose data
+   * @param data The data to update the pose data with
+   */
+  public void update(double[] data) {
+    if (data.length != 7) {
+      throw new IllegalArgumentException("Data array must have 7 elements");
+    }
+
+    this.x = data[0];
+    this.y = data[1];
+    this.z = data[2];
+    this.roll = data[3];
+    this.pitch = data[4];
+    this.yaw = data[5];
+    this.totalLatency = data[6];
+  }
+
+  /**
+   * Gets the x position of the limelight
+   * @return The x position of the limelight
+   */
   public double getX() {
     return this.x;
   }
 
+  /**
+   * Gets the y position of the limelight
+   * @return The y position of the limelight
+   */
   public double getY() {
     return this.y;
   }
 
+  /**
+   * Gets the z position of the limelight
+   * @return The z position of the limelight
+   */
   public double getZ() {
     return this.z;
   }
 
+  /**
+   * Gets a 3d rotation position of the limelight
+   * @return The 3d rotation position of the limelight
+   */
   public Rotation3d getRotation() {
     return new Rotation3d(
         Math.toRadians(this.roll), Math.toRadians(this.pitch), Math.toRadians(this.yaw));
   }
 
+  /**
+   * Gets the raw latency of the limelight straight from the network table
+   * @return The raw latency of the limelight straight from the network table
+   */
   public double getRawTotalLatency() {
     return this.totalLatency;
   }
 
+  /**
+   * Gets the total latency of the limelight in seconds
+   * @return The total latency of the limelight in seconds
+   */
   public double getTotalLatency() {
     return this.totalLatency / 1000;
   }
 
+  /**
+   * Converts the limelight pose data to a 3d pose
+   * @return A 3d pose of the limelight pose data
+   */
   public Pose3d toPose3d() {
     return new Pose3d(
         this.x,