From 9790e2d2a40dbefc8d56ef820fd515c622156eb4 Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Wed, 21 Aug 2024 18:39:26 -0400 Subject: [PATCH] error handling (#29) * decent changes * Update VisionSubsystem.java * fancy switch statement bc im cool like that * decent changes * hi * Delete .vscode directory * pr things * fix build and apply spotless * docstrings * fix typo * Update src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java Jacks change Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com> * decent changes * format * Update TalonUtil.java * sync changes * remove talonutil for now * cleanup, remove scheduledExcutorService, and ready for merge * run formatter --------- Co-authored-by: Jack <85963782+JacksonElia@users.noreply.github.com> --- .../subsystems/shooter/ShooterSubsystem.java | 2 + .../robot/subsystems/swerve/SwerveModule.java | 1 + .../subsystems/vision/VisionSubsystem.java | 207 ++++++++++++------ 3 files changed, 143 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 0c6c30a..44383e2 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -43,6 +43,7 @@ public ShooterSubsystem() { voltageRequest = new VoltageOut(0); TalonFXConfiguration shooterConfig = new TalonFXConfiguration(); + shooterConfig.Slot0.kP = ShooterConstants.SHOOT_P; shooterConfig.Slot0.kI = ShooterConstants.SHOOT_I; shooterConfig.Slot0.kD = ShooterConstants.SHOOT_D; @@ -63,6 +64,7 @@ public ShooterSubsystem() { followerFlywheel.getConfigurator().apply(shooterConfig, HardwareConstants.TIMEOUT_S); TalonFXConfiguration rollerConfig = new TalonFXConfiguration(); + rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; rollerConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; rollerMotor.getConfigurator().apply(rollerConfig, HardwareConstants.TIMEOUT_S); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index 94dc32d..2da158b 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -114,6 +114,7 @@ public SwerveModule( driveMotor.setPosition(0); turnMotor.setPosition(0); + driveMotor.optimizeBusUtilization(); BaseStatusSignal.setUpdateFrequencyForAll( HardwareConstants.SIGNAL_FREQUENCY, turnEncoderPos, driveMotorPosition, driveMotorVelocity); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 07d32ed..420129a 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.FieldConstants; import frc.robot.Constants.VisionConstants; import frc.robot.extras.LimelightHelpers; import frc.robot.extras.LimelightHelpers.PoseEstimate; @@ -19,24 +20,20 @@ public class VisionSubsystem extends SubsystemBase { private Pose2d lastSeenPose = new Pose2d(); private double headingDegrees = 0; private double headingRateDegreesPerSecond = 0; - private final Map runningThreads = new ConcurrentHashMap<>(); - private final ExecutorService executorService = - Executors.newFixedThreadPool( - 3); // Thread pool size of 3 for the number of limelights being threaded + private final Map limelightThreads = new ConcurrentHashMap<>(); + private final ExecutorService executorService = Executors.newFixedThreadPool(3); /** * The pose estimates from the limelights in the following order {shooterLimelight, * frontLeftLimelight, frontRightLimelight} */ - private PoseEstimate[] limelightEstimates = { - new PoseEstimate(), new PoseEstimate(), new PoseEstimate() - }; + private PoseEstimate[] limelightEstimates; public VisionSubsystem() { - for (int limelightThreads = 0; - limelightThreads < limelightEstimates.length; - limelightThreads++) { - runningThreads.put(limelightThreads, new AtomicBoolean(true)); + limelightEstimates = new PoseEstimate[3]; + for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) { + limelightThreads.put(limelightNumber, new AtomicBoolean(true)); + limelightEstimates[limelightNumber] = new PoseEstimate(); } } @@ -69,14 +66,17 @@ && 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(); } - - // double distanceToAprilTags = getLimelightAprilTagDistance(limelightNumber); + // Soon to be implemented code: + // if (canSeeAprilTags(limelightNumber)) { + // if (isValidPoseEstimate(limelightNumber)) { // if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber) - // && distanceToAprilTags < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { + // && getLimelightAprilTagDistance(limelightNumber) < + // VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) { // limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); // } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) { @@ -86,6 +86,8 @@ public void updateLimelightPoseEstimate(int limelightNumber) { } else { limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber); } + // } + // } } /** @@ -99,18 +101,26 @@ public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) { PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); // Extract the positions of the two poses - Translation2d megaTag1Translation = megaTag1Estimate.pose.getTranslation(); - Translation2d megaTag2Translation = megaTag2Estimate.pose.getTranslation(); + Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation(); + Translation2d megaTag2TranslationMeters = megaTag2Estimate.pose.getTranslation(); + + double megaTag1RotationDegrees = megaTag1Estimate.pose.getRotation().getDegrees(); + double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees(); // Calculate the discrepancy between the two MegaTag translations in meters - double megaTagDiscrepancyMeters = megaTag1Translation.getDistance(megaTag2Translation); + double megaTagTranslationDiscrepancyMeters = + megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters); + double megaTagRotationDiscrepancyDegrees = + Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees); // Define a threshold (meters) for what constitutes a "large" discrepancy // This value should be determined based on your testing double thresholdMeters = 0.5; + double thresholdDegrees = 45; // Check if the discrepancy is larger than the threshold (meters) - return megaTagDiscrepancyMeters > thresholdMeters; + return megaTagTranslationDiscrepancyMeters > thresholdMeters + || megaTagRotationDiscrepancyDegrees > thresholdDegrees; } /** @@ -137,6 +147,46 @@ public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) { return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber)); } + /** + * Checks if the MegaTag1 and MegaTag2 pose estimates are within the field parameters + * + * @param limelightNumber the number of the limelight + * @return true if the poses are within the field, false if not. + */ + 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); + + return ((megaTag1Estimate.pose.getX() > 0 + && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag1Estimate.pose.getY() > 0 + && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)); + } + + /** + * Checks if the MegaTag2 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 isMegaTag2Good(int limelightNumber) { + PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber); + + return ((megaTag2Estimate.pose.getX() > 0 + && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS) + && (megaTag2Estimate.pose.getY() > 0 + && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)); + } + /** * Gets the pose of the robot calculated by specified limelight via any April Tags it sees * @@ -170,6 +220,11 @@ public double getLatencySeconds(int limelightNumber) { return (limelightEstimates[limelightNumber].latency) / 1000.0; } + /** Gets the pose calculated the last time a limelight saw an April Tag */ + public Pose2d getLastSeenPose() { + return lastSeenPose; + } + /** * Gets the average distance between the specified limelight and the April Tags it sees * @@ -204,21 +259,13 @@ public void setHeadingInfo(double headingDegrees, double headingRateDegrees) { * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right */ public String getLimelightName(int limelightNumber) { - switch (limelightNumber) { - case 0: - return VisionConstants.SHOOTER_LIMELIGHT_NAME; - case 1: - return VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; - case 2: - return VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; - default: - throw new IllegalArgumentException("You entered a number for a non-existent limelight"); - } - } - - /** Gets the pose calculated the last time a limelight saw an April Tag */ - public Pose2d getLastSeenPose() { - return lastSeenPose; + return switch (limelightNumber) { + case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME; + case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME; + case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME; + default -> + throw new IllegalArgumentException("You entered a number for a non-existent limelight"); + }; } /** @@ -226,49 +273,55 @@ public Pose2d getLastSeenPose() { * * @param limelightNumber the limelight number */ - private void checkAndUpdatePose(int limelightNumber) { + public void checkAndUpdatePose(int limelightNumber) { double last_TX = 0; double last_TY = 0; // Syncronization block to ensure thread safety during the critical section where pose // information is read and compared. - // This helps prevents race conditions, where one limelight may be updating an object that + // This helps prevent race conditions, where one limelight may be updating an object that // another limelight is reading. // A race condition could cause unpredictable things to happen. Such as causing a limelight to // be unable to reference an // object, as its reference was modified earlier. synchronized (this) { - double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber)); - double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber)); - - // This checks if the limelight reading is new. The reasoning being that if the TX and TY - // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, - // 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); - runningThreads.computeIfPresent(limelightNumber, (key, value) -> new AtomicBoolean(true)); - // This is to keep track of the last valid pose calculated by the limelights - // it is used when the driver resets the robot odometry to the limelight calculated position - if (canSeeAprilTags(limelightNumber)) { - lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose; - } - } else { - // Retrieve the AtomicBoolean for the given limelight number - AtomicBoolean runningThread = - runningThreads.getOrDefault(limelightNumber, new AtomicBoolean()); - // Only stop the thread if it's currently running - if (runningThread.get()) { - // Since we can't see an April Tag, set the estimate for the specified limelight to an - // empty PoseEstimate() - limelightEstimates[limelightNumber] = new PoseEstimate(); - // stop the thread for the specified limelight - stopThread(limelightNumber); + try { + double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber)); + double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber)); + + // This checks if the limelight reading is new. The reasoning being that if the TX and TY + // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way, + // 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); + limelightThreads.computeIfPresent( + limelightNumber, (key, value) -> new AtomicBoolean(true)); + // This is to keep track of the last valid pose calculated by the limelights + // it is used when the driver resets the robot odometry to the limelight calculated + // position + if (canSeeAprilTags(limelightNumber)) { + lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose; + } + } else { + // Retrieve the AtomicBoolean for the given limelight number + AtomicBoolean isThreadRunning = + limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean()); + // Only stop the thread if it's currently running + if (isThreadRunning.get()) { + // stop the thread for the specified limelight + stopThread(limelightNumber); + } } + last_TX = current_TX; + last_TY = current_TY; + } catch (Exception e) { + System.err.println( + "Error communicating with the: " + + getLimelightName(limelightNumber) + + ": " + + e.getMessage()); } - - last_TX = current_TX; - last_TY = current_TY; } } @@ -287,10 +340,19 @@ private void checkAndUpdatePose(int limelightNumber) { * @param limelightNumber the limelight number */ public void visionThread(int limelightNumber) { + executorService.submit( () -> { - while (runningThreads.get(limelightNumber).get()) { - checkAndUpdatePose(limelightNumber); + try { + while (limelightThreads.get(limelightNumber).get()) { + checkAndUpdatePose(limelightNumber); + } + } catch (Exception e) { + System.err.println( + "Error executing task for the: " + + getLimelightName(limelightNumber) + + ": " + + e.getMessage()); } }); } @@ -302,7 +364,18 @@ public void visionThread(int limelightNumber) { * @param limelightNumber the limelight number */ public void stopThread(int limelightNumber) { - runningThreads.get(limelightNumber).set(false); + try { + // Since we can't see an April Tag, set the estimate for the specified limelight to an empty + // PoseEstimate() + limelightEstimates[limelightNumber] = new PoseEstimate(); + limelightThreads.get(limelightNumber).set(false); + } catch (Exception e) { + System.err.println( + "Error stopping thread for the: " + + getLimelightName(limelightNumber) + + ": " + + e.getMessage()); + } } /** Shuts down all the threads. */