From 5e276e258970a2f8b91ef91194b0bc141a2d5b69 Mon Sep 17 00:00:00 2001
From: Alvin Zhang <41vin2h4n9@gmail.com>
Date: Sun, 29 Sep 2024 14:11:47 -0700
Subject: [PATCH] Update at 'Sun Sep 29 14:11:47 PDT 2024'

---
 .../apriltag/AprilTagVisionIOMegaTag2.java    | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java
index d69d010..8ea19c8 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java
@@ -36,16 +36,18 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
                 DriverStation.isDisabled()
                         ? LimelightHelpers.getBotPoseEstimate_wpiBlue(name)
                         : LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name);
-        Pose3d limelightPose = new Pose3d(measurement.pose);
-        inputs.estimatedPoseMeters = new Pose3d(
-                limelightPose.getX(),
-                limelightPose.getY() + 0.105,
-                limelightPose.getZ(),
-                limelightPose.getRotation());
-        inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds;
-        inputs.numTagsSeen = measurement.tagCount;
-        inputs.seenTagIDs = Arrays.stream(measurement.rawFiducials).mapToInt(fiducial -> fiducial.id).toArray();
-        inputs.avgTagDistance = measurement.avgTagDist;
+        if (measurement.pose != null) {
+            Pose3d limelightPose = new Pose3d(measurement.pose);
+            inputs.estimatedPoseMeters = new Pose3d(
+                    limelightPose.getX(),
+                    limelightPose.getY() + 0.105,
+                    limelightPose.getZ(),
+                    limelightPose.getRotation());
+            inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds;
+            inputs.numTagsSeen = measurement.tagCount;
+            inputs.seenTagIDs = Arrays.stream(measurement.rawFiducials).mapToInt(fiducial -> fiducial.id).toArray();
+            inputs.avgTagDistance = measurement.avgTagDist;
+        }
     }
 
     @Override