From 1ce2f18a5d0406344c1944fd5929f8ca3902a2d7 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Thu, 3 Oct 2024 20:43:05 -0400 Subject: [PATCH] Vision overhaul 2 electric boogaloo (#34) * something like this * apply spotless * yay * push name change * Update src/main/java/frc/robot/extras/LimelightHelpers.java jacks w change Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com> * make stuff work with jacks w change * apply spotless * Update VisionSubsystem.java * push * push other fix * idk what this is * w changes * yay it works im so happy * Update VisionSubsystem.java add some docstrings * format code --------- Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com> Co-authored-by: Ishan1522 <36376931+NNLUliskey@users.noreply.github.com> --- src/main/java/frc/robot/Robot.java | 7 -- .../frc/robot/extras/LimelightHelpers.java | 4 + .../subsystems/vision/VisionSubsystem.java | 103 +++++++++--------- 3 files changed, 57 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8b45599..ec4445e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,10 +1,3 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; diff --git a/src/main/java/frc/robot/extras/LimelightHelpers.java b/src/main/java/frc/robot/extras/LimelightHelpers.java index def8d78..4080809 100644 --- a/src/main/java/frc/robot/extras/LimelightHelpers.java +++ b/src/main/java/frc/robot/extras/LimelightHelpers.java @@ -551,6 +551,10 @@ private static void printPoseEstimate(PoseEstimate pose) { } } + public static Boolean isValidPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 420129a..4c0e896 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants.FieldConstants; @@ -65,29 +66,34 @@ && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS. * * @param limelightNumber the number of the limelight */ - public void updateLimelightPoseEstimate(int limelightNumber) { - // this can probably be removed after testing - if (!canSeeAprilTags(limelightNumber)) { - limelightEstimates[limelightNumber] = new PoseEstimate(); - } - // Soon to be implemented code: - // if (canSeeAprilTags(limelightNumber)) { - // if (isValidPoseEstimate(limelightNumber)) { - - // if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) - // && getLimelightAprilTagDistance(limelightNumber) < - // VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { - // limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); - // } else - if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { - LimelightHelpers.SetRobotOrientation( - getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0); - limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber); - } else { - limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); + public PoseEstimate enabledPoseUpdate(int limelightNumber) { + if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) { + if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) + && 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); + } } - // } - // } + return limelightEstimates[limelightNumber] = new PoseEstimate(); + } + + /** + * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link + * #enabledPoseUpdate(int)} + * + * @param limelightNumber the number of the limelight + */ + public void updatePoseEstimate(int limelightNumber) { + limelightEstimates[limelightNumber] = + DriverStation.isEnabled() + ? enabledPoseUpdate(limelightNumber) + : getMegaTag1PoseEstimate(limelightNumber); } /** @@ -148,43 +154,36 @@ public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) { } /** - * Checks if the MegaTag1 and MegaTag2 pose estimates are within the field parameters + * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field * * @param limelightNumber the number of the limelight - * @return true if the poses are within the field, false if not. + * @return true if the pose estimate exists within the field and the pose estimate is not null */ public boolean isValidPoseEstimate(int limelightNumber) { - return isMegaTag1Good(limelightNumber) && isMegaTag2Good(limelightNumber); - } - - /** - * Checks if the MegaTag1 pose estimate is within the field parameters - * - * @param limelightNumber the number of the limelight - * @return true if the pose is within the field, false if not. - */ - public boolean isMegaTag1Good(int limelightNumber) { PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber); + PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - return ((megaTag1Estimate.pose.getX() > 0 - && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) - && (megaTag1Estimate.pose.getY() > 0 - && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)); + return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate) + && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate) + && isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate); } /** - * Checks if the MegaTag2 pose estimate is within the field parameters + * Checks whether the pose estimate for MT1 and MT2 is within the field * - * @param limelightNumber the number of the limelight - * @return true if the pose is within the field, false if not. + * @param megaTag1Estimate the MT1 pose estimate to check + * @param megaTag2Estimate the MT2 pose estimate to check */ - public boolean isMegaTag2Good(int limelightNumber) { - PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); - - return ((megaTag2Estimate.pose.getX() > 0 + private boolean isWithinFieldBounds( + PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) { + return (megaTag1Estimate.pose.getX() > 0 + && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.pose.getY() > 0 + && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getX() > 0 && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) && (megaTag2Estimate.pose.getY() > 0 - && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)); + && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS); } /** @@ -294,7 +293,7 @@ public void checkAndUpdatePose(int limelightNumber) { // because to get the timestamp of the reading, you need to parse the JSON dump which can be // very demanding whereas this only has to get the Network Table entries for TX and TY. if (current_TX != last_TX || current_TY != last_TY) { - updateLimelightPoseEstimate(limelightNumber); + updatePoseEstimate(limelightNumber); limelightThreads.computeIfPresent( limelightNumber, (key, value) -> new AtomicBoolean(true)); // This is to keep track of the last valid pose calculated by the limelights @@ -344,9 +343,9 @@ public void visionThread(int limelightNumber) { executorService.submit( () -> { try { - while (limelightThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelightNumber); - } + // while (limelightThreads.get(limelightNumber).get()) { + checkAndUpdatePose(limelightNumber); + // } } catch (Exception e) { System.err.println( "Error executing task for the: " @@ -405,5 +404,9 @@ public void periodic() { visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER); visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER); SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0)); + SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString()); + SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString()); + + SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString()); } }