Update at 'Sun Sep 29 14:11:47 PDT 2024'

flamingchickens1540 · Sep 29, 2024 · 5e276e2 · 5e276e2
1 parent 5936ee6
commit 5e276e2
Showing 1 changed file with 12 additions and 10 deletions.
diff --git a/...main/java/org/team1540/robot2024/subsystems/vision/apriltag/AprilTagVisionIOMegaTag2.java b/...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