w changes

TitaniumTigers4829 · Sep 20, 2024 · a945add · a945add
1 parent 4461705
commit a945add
Showing 1 changed file with 9 additions and 8 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -73,17 +73,18 @@ public PoseEstimate enabledPoseUpdate(int limelightNumber) {
     // Soon to be implemented code:
     if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) {
       if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
-          && getLimelightAprilTagDistance(limelightNumber) <
-      VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
+          && getLimelightAprilTagDistance(limelightNumber)
+              < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
         return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
       } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) {
-      LimelightHelpers.SetRobotOrientation(
-          getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0);
-      return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber);
-    } else {
-      return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
-    }
+        LimelightHelpers.SetRobotOrientation(
+            getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0);
+        return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber);
+      } else {
+        return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
       }
+    }
+    return new PoseEstimate();
   }
 
   /**