diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 44a7f13..3d9e3c1 100644 --- 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(); } /**