diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e297c851..ae3ad2da 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,15 +55,13 @@ public class RobotContainer { private AlgaeSuperstructure algaeSuperstructure = new AlgaeSuperstructure(algaePivot, algaeRollers); - private AutomaticAutonomousMaker3000 automaker = - new AutomaticAutonomousMaker3000(drivetrain, coralSuperstructure); - private Vision vision = Vision.create( // Java 21 pattern matching switch would be nice (drivetrain instanceof DrivetrainSim) ? ((DrivetrainSim) drivetrain)::getActualPose : drivetrain::getPose, + () -> drivetrain.getPose().getRotation(), visionEst -> drivetrain.addVisionMeasurement( visionEst.estimate().estimatedPose.toPose2d(), @@ -74,6 +72,8 @@ public class RobotContainer { reefVisionEst.estimate().estimatedPose.toPose2d(), reefVisionEst.estimate().timestampSeconds, reefVisionEst.stdDevs())); + private AutomaticAutonomousMaker3000 automaker = + new AutomaticAutonomousMaker3000(drivetrain, coralSuperstructure, vision::canSeeReefTag); private CommandXboxController driver = new CommandXboxController(0); private XboxController manipulator = new XboxController(1); @@ -252,7 +252,7 @@ private void configureTuningBindings() { // System.out.println("Changing volts to: " + volts); // })); - driver.y().whileTrue(ReefAlign.tuneAlignment(drivetrain)); + driver.y().whileTrue(ReefAlign.tuneAlignment(drivetrain, vision::canSeeReefTag)); // driver.b().whileTrue(coralSuperstructure.feedCoral()); @@ -410,7 +410,10 @@ private void configureBindings() { // .getTargetAngle() // .isEquivalent( // queuedSetpoint.getArmAngle())), - ReefAlign.alignToReef(drivetrain, () -> queuedReefPosition)) + ReefAlign.alignToReef( + drivetrain, + () -> queuedReefPosition, + vision::canSeeReefTag)) .onlyWhile( () -> ReefAlign.isWithinReefRange( diff --git a/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java b/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java index 4dfb2fcc..7dd736eb 100644 --- a/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java +++ b/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java @@ -26,6 +26,7 @@ import java.io.IOException; import java.util.ArrayList; import java.util.List; +import java.util.function.IntPredicate; import org.json.simple.parser.ParseException; @Logged @@ -103,7 +104,10 @@ public class AutomaticAutonomousMaker3000 { private Command storedAuto; - public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coralSuperstructure) { + public AutomaticAutonomousMaker3000( + SwerveDrive drive, + CoralSuperstructure coralSuperstructure, + IntPredicate useReefPoseEstimate) { this.drive = drive; this.coralSuperstructure = coralSuperstructure; @@ -131,14 +135,17 @@ public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coral switch (preBuiltAuto.getSelected()) { case TAXI -> runPath(autoChooser.build().startingPosition.pathID + " to Brake"); - case TOPAUTO -> buildAuto(kTopLaneAuto); - case MIDTOPAUTO -> buildAuto(kMidLaneTopAuto); - case MIDBOTAUTO -> buildAuto(kMidLaneBotAuto); - case BOTAUTO -> buildAuto(kBotLaneAuto); - case MIDPRELOADAUTO -> buildAuto(kMidLaneBotPreloadAuto); // test auto again + case TOPAUTO -> buildAuto(kTopLaneAuto, useReefPoseEstimate); + case MIDTOPAUTO -> buildAuto(kMidLaneTopAuto, useReefPoseEstimate); + case MIDBOTAUTO -> buildAuto(kMidLaneBotAuto, useReefPoseEstimate); + case BOTAUTO -> buildAuto(kBotLaneAuto, useReefPoseEstimate); + case MIDPRELOADAUTO -> + buildAuto( + kMidLaneBotPreloadAuto, useReefPoseEstimate); // test auto again case MIDOPPOSITESIDEAUTO -> - buildAuto(kMidLaneOppositeSideAuto); // test auto x2 - case CUSTOM -> buildAuto(autoChooser.build()); + buildAuto( + kMidLaneOppositeSideAuto, useReefPoseEstimate); // test auto x2 + case CUSTOM -> buildAuto(autoChooser.build(), useReefPoseEstimate); default -> new PathsAndAuto(Commands.none(), new ArrayList<>()); }; @@ -192,7 +199,7 @@ private void visualizeAuto(List paths) { } // Returns the path list for visualization and autonomous command - public PathsAndAuto buildAuto(CycleAutoConfig config) { + public PathsAndAuto buildAuto(CycleAutoConfig config, IntPredicate useReefPoseEstimate) { pathError = ""; try { Command auto = @@ -235,7 +242,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) { withScoring( toPathCommand(pathNewGoalEndState, true), config.scoringGroup.get(i).pole, - config.scoringGroup.get(i).level)); + config.scoringGroup.get(i).level, + useReefPoseEstimate)); paths.add(path); } else { @@ -269,7 +277,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) { withScoring( toPathCommand(scorePathNewGoalEndState), config.scoringGroup.get(i).pole, - config.scoringGroup.get(i).level)); + config.scoringGroup.get(i).level, + useReefPoseEstimate)); lastReefSide = config.scoringGroup.get(i).reefSide; paths.add(intakePath); @@ -320,7 +329,8 @@ public Command withIntaking(Command path, FeedLocation location) { coralSuperstructure.feedCoral().until(() -> coralSuperstructure.hasCoral()))); } - public Command withScoring(Command path, Pole pole, Level level) { + public Command withScoring( + Command path, Pole pole, Level level, IntPredicate useReefPoseEstimate) { CoralScorerSetpoint setpoint = switch (level) { default -> CoralScorerSetpoint.L1; @@ -342,25 +352,30 @@ public Command withScoring(Command path, Pole pole, Level level) { .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected())) .andThen( ReefAlign.alignToReef( - drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT) - .withDeadline( + drive, + () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT, + useReefPoseEstimate)) + .asProxy() + .alongWith( + coralSuperstructure + .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle()) + .asProxy()) + .asProxy() + .withDeadline( + coralSuperstructure + .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle()) + .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected()) + .until(() -> drive.atPoseSetpoint()) + .withTimeout(2.5) + .andThen( coralSuperstructure - .goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle()) + .goToSetpointProfiled(() -> setpoint) .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected()) - .until(() -> drive.atPoseSetpoint()) - .withTimeout(2.5) - .andThen( - coralSuperstructure - .goToSetpointProfiled(() -> setpoint) - .alongWith( - coralSuperstructure.getEndEffector().stallCoralIfDetected()) - .until(() -> coralSuperstructure.atTargetState(setpoint))) + .until(() -> coralSuperstructure.atTargetState(setpoint))) + .andThen( + Commands.waitSeconds(0.5) .andThen( - Commands.waitSeconds(0.5) - .andThen( - coralSuperstructure - .outtakeCoral(() -> setpoint) - .withTimeout(0.5))))); + coralSuperstructure.outtakeCoral(() -> setpoint).withTimeout(0.5)))); } private Command toPathCommand(PathPlannerPath path, boolean zero) { diff --git a/src/main/java/frc/robot/commands/ReefAlign.java b/src/main/java/frc/robot/commands/ReefAlign.java index 33551d51..96cece57 100644 --- a/src/main/java/frc/robot/commands/ReefAlign.java +++ b/src/main/java/frc/robot/commands/ReefAlign.java @@ -30,6 +30,7 @@ import java.util.Map; import java.util.Optional; import java.util.function.DoubleSupplier; +import java.util.function.IntPredicate; import java.util.function.Supplier; public class ReefAlign { @@ -197,7 +198,9 @@ private static Pose2d getNearestRightAlign(int reefTagID) { } public static Command alignToReef( - SwerveDrive swerveDrive, Supplier targetReefPosition) { + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true) .andThen( swerveDrive.driveToFieldPose( @@ -211,12 +214,22 @@ public static Command alignToReef( }; return new AlignmentSetpoint(target, true); + }, + () -> { + final int nearestReefID = getNearestReefID(swerveDrive.getPose()); + if (useReefPoseEstimate.test(nearestReefID)) { + return swerveDrive.getReefVisionPose(); + } else { + return swerveDrive.getPose(); + } })) .finallyDo(() -> Leds.getInstance().isReefAligning = false); } public static Command alignToPrealignReef( - SwerveDrive swerveDrive, Supplier targetReefPosition) { + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true) .andThen( swerveDrive.driveToFieldPose( @@ -235,12 +248,22 @@ public static Command alignToPrealignReef( new Translation2d(kIntermediateDistance, Meters.zero()), Rotation2d.kZero)); return new AlignmentSetpoint(target, false); + }, + () -> { + final int nearestReefID = getNearestReefID(swerveDrive.getPose()); + if (useReefPoseEstimate.test(nearestReefID)) { + return swerveDrive.getReefVisionPose(); + } else { + return swerveDrive.getPose(); + } })) .finallyDo(() -> Leds.getInstance().isReefAligning = false); } public static Command alignToTag( - SwerveDrive swerveDrive, Supplier targetReefPosition) { + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { return swerveDrive.driveToFieldPose( () -> { Pose2d target = @@ -258,10 +281,18 @@ public static Command alignToTag( target.plus(new Transform2d(translationError.getX(), 0, Rotation2d.kZero)); return new AlignmentSetpoint(newTarget, false); + }, + () -> { + final int nearestReefID = getNearestReefID(swerveDrive.getPose()); + if (useReefPoseEstimate.test(nearestReefID)) { + return swerveDrive.getReefVisionPose(); + } else { + return swerveDrive.getPose(); + } }); } - public static Command tuneAlignment(SwerveDrive swerveDrive) { + public static Command tuneAlignment(SwerveDrive swerveDrive, IntPredicate useReefPoseEstimate) { TunableConstant depth = new TunableConstant("/ReefAlign/Depth", kReefDistance.in(Inch)); TunableConstant side = new TunableConstant("/ReefAlign/Side", kLeftAlignDistance.in(Inch)); @@ -273,6 +304,14 @@ public static Command tuneAlignment(SwerveDrive swerveDrive) { new Transform2d( Inches.of(depth.get()), Inches.of(side.get()), kReefAlignmentRotation)); return new AlignmentSetpoint(pose, true); + }, + () -> { + final int nearestReefID = getNearestReefID(swerveDrive.getPose()); + if (useReefPoseEstimate.test(nearestReefID)) { + return swerveDrive.getReefVisionPose(); + } else { + return swerveDrive.getPose(); + } }); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java index bb96b27b..8a23d86b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java @@ -198,12 +198,9 @@ public void driveRobotCentric( } @Override - public void driveToFieldPose(Pose2d pose) { - - final var currentPose = getPose(); - + public void driveToFieldPose(Pose2d target, Pose2d current) { double distance = - currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()); + current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()); // increase weighting of velocity from PID radius (weight = 0) to velocity radius (weight = 1) double autoFfFactor = @@ -219,21 +216,20 @@ public void driveToFieldPose(Pose2d pose) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize( - xPoseController.calculate(getPose().getX(), pose.getX()) + xPoseController.calculate(current.getX(), target.getX()) + xPoseController.getSetpoint().velocity * ffFactor, - yPoseController.calculate(getPose().getY(), pose.getY()) + yPoseController.calculate(current.getY(), target.getY()) + yPoseController.getSetpoint().velocity * ffFactor, thetaController.calculate( - getPose().getRotation().getRadians(), pose.getRotation().getRadians()) + current.getRotation().getRadians(), target.getRotation().getRadians()) + thetaController.getSetpoint().velocity * ffFactor, RobotConstants.kRobotLoopPeriod.in(Seconds)); - if (currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()) + if (current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()) < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters)) targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond); - if (Math.abs( - currentPose.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees()) + if (Math.abs(current.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees()) < DrivetrainConstants.kAlignmentSetpointRotationTolerance.in(Degrees)) targetSpeeds = new ChassisSpeeds(targetSpeeds.vxMetersPerSecond, targetSpeeds.vyMetersPerSecond, 0); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java index b637bfbc..f698d255 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java @@ -172,13 +172,11 @@ public void driveRobotCentric( } @Override - public void driveToFieldPose(Pose2d pose) { + public void driveToFieldPose(Pose2d pose, Pose2d current) { if (pose == null) return; - final Pose2d currentPose = getPose(); - double distance = - currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()); + current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()); // increase weighting of velocity from PID radius (weight = 0) to velocity radius (weight = 1) double autoFfFactor = @@ -194,21 +192,20 @@ public void driveToFieldPose(Pose2d pose) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize( - xPoseController.calculate(getPose().getX(), pose.getX()) + xPoseController.calculate(current.getX(), pose.getX()) + xPoseController.getSetpoint().velocity * ffFactor, - yPoseController.calculate(getPose().getY(), pose.getY()) + yPoseController.calculate(current.getY(), pose.getY()) + yPoseController.getSetpoint().velocity * ffFactor, thetaController.calculate( - getPose().getRotation().getRadians(), pose.getRotation().getRadians()) + current.getRotation().getRadians(), pose.getRotation().getRadians()) + thetaController.getSetpoint().velocity * ffFactor, RobotConstants.kRobotLoopPeriod.in(Seconds)); - if (currentPose.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()) + if (current.getTranslation().getDistance(alignmentSetpoint.pose().getTranslation()) < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters)) targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond); - if (Math.abs( - currentPose.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees()) + if (Math.abs(current.getRotation().minus(alignmentSetpoint.pose().getRotation()).getDegrees()) < DrivetrainConstants.kAlignmentSetpointRotationTolerance.in(Degrees)) targetSpeeds = new ChassisSpeeds(targetSpeeds.vxMetersPerSecond, targetSpeeds.vyMetersPerSecond, 0); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 78f3660e..77776035 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -166,9 +166,13 @@ default Command driveFixedHeading( } // field relative auto drive w/ external pid controllers - void driveToFieldPose(Pose2d pose); + void driveToFieldPose(Pose2d target, Pose2d current); - default Command driveToFieldPose(Supplier pose) { + default Command driveToFieldPose(Supplier pose) { + return driveToFieldPose(() -> new AlignmentSetpoint(pose.get(), true), this::getPose); + } + + default Command driveToFieldPose(Supplier pose, Supplier robotPose) { return runOnce( () -> { ChassisSpeeds speeds = @@ -186,7 +190,7 @@ default Command driveToFieldPose(Supplier pose) { run( () -> { setAlignmentSetpoint(pose.get()); - driveToFieldPose(pose.get().pose); + driveToFieldPose(pose.get().pose, robotPose.get()); })); } diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 67b793ac..035cf0fd 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -7,11 +7,14 @@ import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; import frc.robot.RobotConstants; import frc.robot.subsystems.vision.VisionConstants.CameraConfig; +import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -26,27 +29,73 @@ public class Camera { private final PhotonCamera camera; - private final PhotonPoseEstimator poseEstimator; + private final Supplier robotHeadingSupplier; + + private final PhotonPoseEstimator multiTagEstimator; + + private final PhotonPoseEstimator singleTagEstimator; + + private final CameraConfig config; // for logging - private VisionEstimate latestValidEstimate; + private VisionEstimate latestMultiTagEstimate; + + private VisionEstimate latestSingleTagEstimate; + + public Camera( + CameraConfig config, PhotonCamera camera, Supplier robotHeadingSupplier) { + + this.config = config; - public Camera(CameraConfig config, PhotonCamera camera) { this.name = config.cameraName(); this.usage = config.usage(); this.camera = camera; - this.poseEstimator = + this.robotHeadingSupplier = robotHeadingSupplier; + + this.multiTagEstimator = new PhotonPoseEstimator( RobotConstants.kAprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, config.robotToCamera()); + + this.singleTagEstimator = + new PhotonPoseEstimator( + RobotConstants.kAprilTagFieldLayout, + PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + config.robotToCamera()); + } + + public boolean isReefCamera() { + return usage == CameraUsage.REEF; + } + + public boolean canSeeTag(int id) { + final var unreadResults = camera.getAllUnreadResults(); + + if (unreadResults.isEmpty()) return false; + + final var latestResult = unreadResults.get(unreadResults.size() - 1); + + if (!latestResult.hasTargets()) return false; + + return latestResult.targets.stream().anyMatch(target -> target.fiducialId == id); + } + + public VisionEstimate getLatestMultiTagEstimate() { + return latestMultiTagEstimate; + } + + public VisionEstimate getLatestSingleTagEstimate() { + return latestSingleTagEstimate; } @NotLogged public VisionEstimate tryLatestEstimate() { + singleTagEstimator.addHeadingData(Timer.getFPGATimestamp(), robotHeadingSupplier.get()); + if (!camera.isConnected()) return null; final var unreadResults = camera.getAllUnreadResults(); @@ -57,31 +106,71 @@ public VisionEstimate tryLatestEstimate() { if (!latestResult.hasTargets()) return null; - final var estimate = poseEstimator.update(latestResult); - - return estimate - .filter( - poseEst -> - VisionConstants.kAllowedFieldArea.contains( - poseEst.estimatedPose.getTranslation().toTranslation2d()) - && poseEst - .estimatedPose - .getMeasureZ() - .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) - .map( - photonEst -> { - final var visionEst = - new VisionEstimate(photonEst, calculateStdDevs(photonEst), name, usage); - latestValidEstimate = visionEst; - return visionEst; - }) - .orElse(null); + final var multiTagEstimate = + multiTagEstimator + .update(latestResult) + .filter( + poseEst -> + VisionConstants.kAllowedFieldArea.contains( + poseEst.estimatedPose.getTranslation().toTranslation2d()) + && poseEst + .estimatedPose + .getMeasureZ() + .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) + .map( + photonEst -> { + final var visionEst = + new VisionEstimate( + photonEst, + calculateStdDevs(photonEst, EstimateType.MULTI_TAG), + name, + usage, + EstimateType.MULTI_TAG); + + latestMultiTagEstimate = visionEst; + + return visionEst; + }) + .orElse(null); + + final var singleTagEstimate = + singleTagEstimator + .update(latestResult) + .filter( + poseEst -> + VisionConstants.kAllowedFieldArea.contains( + poseEst.estimatedPose.getTranslation().toTranslation2d()) + && poseEst + .estimatedPose + .getMeasureZ() + .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) + .map( + photonEst -> { + final var visionEst = + new VisionEstimate( + photonEst, + calculateStdDevs(photonEst, EstimateType.SINGLE_TAG), + name, + usage, + EstimateType.SINGLE_TAG); + + latestSingleTagEstimate = visionEst; + + return visionEst; + }) + .orElse(null); + + return switch (latestResult.targets.size()) { + case 1 -> singleTagEstimate; + default -> multiTagEstimate; + }; } // could be absolute nonsense, open to tuning constants for each robot camera config // assumes `result` has targets @NotLogged - private Matrix calculateStdDevs(EstimatedRobotPose visionPoseEstimate) { + private Matrix calculateStdDevs( + EstimatedRobotPose visionPoseEstimate, EstimateType estimateType) { // weighted average by ambiguity final double avgTargetDistance = visionPoseEstimate.targetsUsed.stream() @@ -97,13 +186,40 @@ private Matrix calculateStdDevs(EstimatedRobotPose visionPoseEstimate) { .reduce(0.0, Double::sum) / (visionPoseEstimate.targetsUsed.size()); + final double estimateTypeMultiplier = + (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagStdDevMultiplier : 1; + + final double targetDistancePower = + (estimateType == EstimateType.SINGLE_TAG) + ? VisionConstants.kSingleTagTargetDistancePower + : VisionConstants.kMultiTagTargetDistancePower; + + if (visionPoseEstimate.targetsUsed.get(0).poseAmbiguity > VisionConstants.kAmbiguityThreshold + && estimateType == EstimateType.SINGLE_TAG) { + return null; + } + + double poseAmbiguityMultiplier = + estimateType != EstimateType.SINGLE_TAG + ? 1 + : Math.max( + 1, + (visionPoseEstimate.targetsUsed.get(0).poseAmbiguity + + VisionConstants.kAmbiguityShifter) + * VisionConstants.kAmbiguityScalar); + final double translationStdDev = - VisionConstants.kTranslationStdDevCoeff - * Math.pow(avgTargetDistance, 3) + estimateTypeMultiplier + * VisionConstants.kTranslationStdDevCoeff + * Math.pow(avgTargetDistance, targetDistancePower) + * poseAmbiguityMultiplier / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); + final double rotationStdDev = - VisionConstants.kRotationStdDevCoeff - * Math.pow(avgTargetDistance, 3) + estimateTypeMultiplier + * VisionConstants.kRotationStdDevCoeff + * Math.pow(avgTargetDistance, targetDistancePower) + * poseAmbiguityMultiplier / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/vision/EstimateType.java b/src/main/java/frc/robot/subsystems/vision/EstimateType.java new file mode 100644 index 00000000..edf84e0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/EstimateType.java @@ -0,0 +1,10 @@ +/* (C) Robolancers 2025 */ +package frc.robot.subsystems.vision; + +import edu.wpi.first.epilogue.Logged; + +@Logged +public enum EstimateType { + SINGLE_TAG, + MULTI_TAG +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 0c687313..0c68e07f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -3,6 +3,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.RobotBase; import frc.robot.util.VirtualSubsystem; import java.util.function.Consumer; @@ -17,15 +18,17 @@ public class Vision extends VirtualSubsystem { public static Vision create( Supplier robotPoseSupplier, + Supplier robotHeadingSupplier, Consumer visionDataConsumer, Consumer reefVisionDataConsumer) { return RobotBase.isReal() ? new Vision( - new VisionIOReal(VisionConstants.kCameraConfigs), + new VisionIOReal(robotHeadingSupplier, VisionConstants.kCameraConfigs), visionDataConsumer, reefVisionDataConsumer) : new Vision( - new VisionIOSim(robotPoseSupplier, VisionConstants.kCameraConfigs), + new VisionIOSim( + robotPoseSupplier, robotHeadingSupplier, VisionConstants.kCameraConfigs), visionDataConsumer, reefVisionDataConsumer); } @@ -39,15 +42,23 @@ private Vision( this.reefVisionDataConsumer = reefVisionDataConsumer; } + public boolean canSeeReefTag(int tagID) { + return io.reefCameraCanSeeReefTag(tagID); + } + @Override public void periodic() { final var latestEstimates = io.getLatestEstimates(); for (final var est : latestEstimates) { - visionDataConsumer.accept(est); - - if (est.sourceType() == CameraUsage.REEF) { - reefVisionDataConsumer.accept(est); + switch (est.estimateType()) { + case MULTI_TAG -> { + visionDataConsumer.accept(est); + reefVisionDataConsumer.accept(est); + } + case SINGLE_TAG -> { // Java 21 when clauses would be nice + if (est.sourceType() == CameraUsage.REEF) reefVisionDataConsumer.accept(est); + } } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 7a906a23..fbb0d976 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -21,6 +21,15 @@ public class VisionConstants { public static final double kTranslationStdDevCoeff = 1e-1; public static final double kRotationStdDevCoeff = 1e-1; + // TODO: tune more in sim, represents (to some extent) how much more single tag estimates are + // trusted + public static final double kSingleTagStdDevMultiplier = 1; + + public static final int kMultiTagTargetDistancePower = 3; + + // TODO: tune more + public static final int kSingleTagTargetDistancePower = 5; + public static record CameraCalibration( int resolutionWidth, int resolutionHeight, @@ -149,4 +158,8 @@ public static record CameraConfig( + kAllowedFieldDistance.in(Meters), RobotConstants.kAprilTagFieldLayout.getFieldWidth() + kAllowedFieldDistance.in(Meters))); + + public static final double kAmbiguityThreshold = 0.4; + public static final double kAmbiguityShifter = 0.2; + public static final double kAmbiguityScalar = 4; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionEstimate.java b/src/main/java/frc/robot/subsystems/vision/VisionEstimate.java index 163f1895..13639fd2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionEstimate.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionEstimate.java @@ -15,4 +15,5 @@ public record VisionEstimate( EstimatedRobotPose estimate, Matrix stdDevs, String sourceName, - CameraUsage sourceType) {} + CameraUsage sourceType, + EstimateType estimateType) {} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 51a889ba..cbdad184 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -7,5 +7,7 @@ public interface VisionIO { VisionEstimate[] getLatestEstimates(); + boolean reefCameraCanSeeReefTag(int tagID); + boolean areCamerasConnected(); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java index f68c9241..831612a0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java @@ -2,9 +2,11 @@ package frc.robot.subsystems.vision; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.subsystems.vision.VisionConstants.CameraConfig; import java.util.List; import java.util.Objects; +import java.util.function.Supplier; import java.util.stream.Stream; import org.photonvision.PhotonCamera; @@ -12,10 +14,12 @@ public class VisionIOReal implements VisionIO { private final List cameras; - public VisionIOReal(CameraConfig... configs) { + public VisionIOReal(Supplier robotHeadingSupplier, CameraConfig... configs) { cameras = Stream.of(configs) - .map(config -> new Camera(config, new PhotonCamera(config.cameraName()))) + .map( + config -> + new Camera(config, new PhotonCamera(config.cameraName()), robotHeadingSupplier)) .toList(); } @@ -27,6 +31,19 @@ public VisionEstimate[] getLatestEstimates() { .toArray(VisionEstimate[]::new); } + @Override + public boolean reefCameraCanSeeReefTag(int tagID) { + for (Camera camera : cameras) { + if (!camera.isReefCamera()) continue; + + if (!camera.canSeeTag(tagID)) continue; + + return true; + } + + return false; + } + @Override public boolean areCamerasConnected() { boolean isConnected = false; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java index 6acb4ec2..10d71689 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java @@ -4,6 +4,7 @@ import edu.wpi.first.epilogue.Logged; import edu.wpi.first.epilogue.NotLogged; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.RobotConstants; import frc.robot.subsystems.vision.VisionConstants.CameraConfig; import java.util.ArrayList; @@ -26,7 +27,10 @@ public class VisionIOSim implements VisionIO { @NotLogged private final List targets; - public VisionIOSim(Supplier robotPoseSupplier, CameraConfig... configs) { + public VisionIOSim( + Supplier robotPoseSupplier, + Supplier robotHeadingSupplier, + CameraConfig... configs) { this.sim = new VisionSystemSim("main"); sim.addAprilTags(RobotConstants.kAprilTagFieldLayout); @@ -37,7 +41,7 @@ public VisionIOSim(Supplier robotPoseSupplier, CameraConfig... configs) final var camera = new PhotonCamera(config.cameraName()); final var cameraSim = new PhotonCameraSim(camera, config.calib().simProperties()); sim.addCamera(cameraSim, config.robotToCamera()); - return new Camera(config, camera); + return new Camera(config, camera, robotHeadingSupplier); }) .toList(); @@ -56,6 +60,19 @@ public VisionEstimate[] getLatestEstimates() { .toArray(VisionEstimate[]::new); } + @Override + public boolean reefCameraCanSeeReefTag(int tagID) { + for (Camera camera : cameras) { + if (!camera.isReefCamera()) continue; + + if (!camera.canSeeTag(tagID)) continue; + + return true; + } + + return false; + } + @Override public boolean areCamerasConnected() { boolean isConnected = false;