From dcf22caea2af00dd797d19edffefc76ac8802b68 Mon Sep 17 00:00:00 2001 From: Vincent Zheng <92338199+Vncero@users.noreply.github.com> Date: Thu, 6 Mar 2025 17:14:13 +0000 Subject: [PATCH 1/6] feat(6328 single tag estimation): add support for single tag estimates when close to reef, add EstimateType field to VisionEstimate for logging which type of estimate was produced --- gradlew | 0 src/main/java/frc/robot/RobotContainer.java | 1 + .../frc/robot/subsystems/vision/Camera.java | 40 ++++++++++++++++--- .../robot/subsystems/vision/EstimateType.java | 10 +++++ .../frc/robot/subsystems/vision/Vision.java | 4 +- .../subsystems/vision/VisionEstimate.java | 3 +- .../robot/subsystems/vision/VisionIOReal.java | 8 +++- 7 files changed, 57 insertions(+), 9 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/java/frc/robot/subsystems/vision/EstimateType.java 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 2133a2c5..3ad7eb8f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,6 +58,7 @@ public class RobotContainer { (drivetrain instanceof DrivetrainSim) ? ((DrivetrainSim) drivetrain)::getActualPose : drivetrain::getPose, + drivetrain::getHeading, visionEst -> drivetrain.addVisionMeasurement( visionEst.estimate().estimatedPose.toPose2d(), diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index fa23689a..53d3f1a6 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -5,11 +5,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; @@ -24,27 +27,43 @@ public class Camera { private final PhotonCamera camera; - private final PhotonPoseEstimator poseEstimator; + private final Supplier robotHeadingSupplier; + + private final PhotonPoseEstimator multiTagEstimator; + + private final PhotonPoseEstimator singleTagEstimator; // for logging private VisionEstimate latestValidEstimate; - public Camera(CameraConfig config, PhotonCamera camera) { + public Camera( + CameraConfig config, PhotonCamera camera, Supplier robotHeadingSupplier) { 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()); } @NotLogged public VisionEstimate tryLatestEstimate() { + // TODO: still some confusion with exactly what time base to use, should be NT time? + singleTagEstimator.addHeadingData(Timer.getFPGATimestamp(), robotHeadingSupplier.get()); + if (!camera.isConnected()) return null; final var unreadResults = camera.getAllUnreadResults(); @@ -55,7 +74,17 @@ public VisionEstimate tryLatestEstimate() { if (!latestResult.hasTargets()) return null; - final var estimate = poseEstimator.update(latestResult); + final var estimateType = + switch (latestResult.targets.size()) { + case 1 -> EstimateType.SINGLE_TAG; + default -> EstimateType.MULTI_TAG; + }; + + final var estimate = + switch (estimateType) { + case SINGLE_TAG -> singleTagEstimator.update(latestResult); + case MULTI_TAG -> multiTagEstimator.update(latestResult); + }; return estimate .filter( @@ -65,7 +94,8 @@ public VisionEstimate tryLatestEstimate() { .map( photonEst -> { final var visionEst = - new VisionEstimate(photonEst, calculateStdDevs(photonEst), name, usage); + new VisionEstimate( + photonEst, calculateStdDevs(photonEst), name, usage, estimateType); latestValidEstimate = visionEst; return visionEst; }) 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 26d0e9f2..b20d36ab 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,11 +18,12 @@ 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( 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/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java index 9e15c105..4a122f79 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(); } From f0cc7ac1ca00347a579d04f2065b570c09f9e591 Mon Sep 17 00:00:00 2001 From: Vincent Zheng <92338199+Vncero@users.noreply.github.com> Date: Fri, 7 Mar 2025 13:12:59 +0000 Subject: [PATCH 2/6] fix(6328 single tag estimation): resolve build errors (which were not caught due to lack of tooling, githooks do not work) --- src/main/java/frc/robot/subsystems/vision/Camera.java | 1 - src/main/java/frc/robot/subsystems/vision/Vision.java | 3 ++- .../java/frc/robot/subsystems/vision/VisionIOSim.java | 8 ++++++-- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 53d3f1a6..1138905b 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -61,7 +61,6 @@ public Camera( @NotLogged public VisionEstimate tryLatestEstimate() { - // TODO: still some confusion with exactly what time base to use, should be NT time? singleTagEstimator.addHeadingData(Timer.getFPGATimestamp(), robotHeadingSupplier.get()); if (!camera.isConnected()) return null; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index b20d36ab..d78c5621 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -27,7 +27,8 @@ public static Vision create( visionDataConsumer, reefVisionDataConsumer) : new Vision( - new VisionIOSim(robotPoseSupplier, VisionConstants.kCameraConfigs), + new VisionIOSim( + robotPoseSupplier, robotHeadingSupplier, VisionConstants.kCameraConfigs), visionDataConsumer, reefVisionDataConsumer); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java index 0f79ef34..f6b9818f 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(); From 93f089ad6189fbeae6e390da4f1f182b00663d96 Mon Sep 17 00:00:00 2001 From: Vincent Zheng <92338199+Vncero@users.noreply.github.com> Date: Fri, 7 Mar 2025 18:18:11 +0000 Subject: [PATCH 3/6] feat(6328 single tag estimation): address review comments --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/subsystems/vision/Camera.java | 100 ++++++++++++------ .../frc/robot/subsystems/vision/Vision.java | 10 +- .../subsystems/vision/VisionConstants.java | 3 + 4 files changed, 80 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3ad7eb8f..5879c095 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,7 +58,7 @@ public class RobotContainer { (drivetrain instanceof DrivetrainSim) ? ((DrivetrainSim) drivetrain)::getActualPose : drivetrain::getPose, - drivetrain::getHeading, + () -> drivetrain.getPose().getRotation(), visionEst -> drivetrain.addVisionMeasurement( visionEst.estimate().estimatedPose.toPose2d(), diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 1138905b..60e2af1f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -34,7 +34,9 @@ public class Camera { private final PhotonPoseEstimator singleTagEstimator; // for logging - private VisionEstimate latestValidEstimate; + private VisionEstimate latestMultiTagEstimate; + + private VisionEstimate latestSingleTagEstimate; public Camera( CameraConfig config, PhotonCamera camera, Supplier robotHeadingSupplier) { @@ -59,6 +61,14 @@ public Camera( config.robotToCamera()); } + public VisionEstimate getLatestMultiTagEstimate() { + return latestMultiTagEstimate; + } + + public VisionEstimate getLatestSingleTagEstimate() { + return latestSingleTagEstimate; + } + @NotLogged public VisionEstimate tryLatestEstimate() { singleTagEstimator.addHeadingData(Timer.getFPGATimestamp(), robotHeadingSupplier.get()); @@ -73,38 +83,63 @@ public VisionEstimate tryLatestEstimate() { if (!latestResult.hasTargets()) return null; - final var estimateType = - switch (latestResult.targets.size()) { - case 1 -> EstimateType.SINGLE_TAG; - default -> EstimateType.MULTI_TAG; - }; - - final var estimate = - switch (estimateType) { - case SINGLE_TAG -> singleTagEstimator.update(latestResult); - case MULTI_TAG -> multiTagEstimator.update(latestResult); - }; - - return estimate - .filter( - poseEst -> - VisionConstants.kAllowedFieldArea.contains( - poseEst.estimatedPose.getTranslation().toTranslation2d())) - .map( - photonEst -> { - final var visionEst = - new VisionEstimate( - photonEst, calculateStdDevs(photonEst), name, usage, estimateType); - latestValidEstimate = visionEst; - return visionEst; - }) - .orElse(null); + final var multiTagEstimate = + multiTagEstimator + .update(latestResult) + .filter( + poseEst -> + VisionConstants.kAllowedFieldArea.contains( + poseEst.estimatedPose.getTranslation().toTranslation2d())) + .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())) + .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() @@ -120,12 +155,17 @@ 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 translationStdDev = - VisionConstants.kTranslationStdDevCoeff + estimateTypeMultiplier + * VisionConstants.kTranslationStdDevCoeff * Math.pow(avgTargetDistance, 3) / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); final double rotationStdDev = - VisionConstants.kRotationStdDevCoeff + estimateTypeMultiplier + * VisionConstants.kRotationStdDevCoeff * Math.pow(avgTargetDistance, 3) / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d78c5621..554e0d5c 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -47,11 +47,13 @@ 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); + 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 573dbea2..ce4dd8c2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -21,6 +21,9 @@ public class VisionConstants { public static final double kTranslationStdDevCoeff = 1e-1; public static final double kRotationStdDevCoeff = 1e-1; + // TODO: tune in sim, represents (to some extent) how much more single tag estimates are trusted + public static final double kSingleTagStdDevMultiplier = 1e-2; + public static record CameraCalibration( int resolutionWidth, int resolutionHeight, From 7b199b1d42bccef75b6e4fc825ac66dc0dc748fd Mon Sep 17 00:00:00 2001 From: at Date: Fri, 14 Mar 2025 09:44:09 -0400 Subject: [PATCH 4/6] feat(ambiguity): added admbiguity to std dev calc --- .../frc/robot/subsystems/vision/Camera.java | 21 +++++++++++++++++++ .../subsystems/vision/VisionConstants.java | 4 ++++ 2 files changed, 25 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 60e2af1f..879cf813 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -33,6 +33,8 @@ public class Camera { private final PhotonPoseEstimator singleTagEstimator; + private final CameraConfig config; + // for logging private VisionEstimate latestMultiTagEstimate; @@ -40,6 +42,9 @@ public class Camera { public Camera( CameraConfig config, PhotonCamera camera, Supplier robotHeadingSupplier) { + + this.config = config; + this.name = config.cameraName(); this.usage = config.usage(); @@ -158,15 +163,31 @@ private Matrix calculateStdDevs( final double estimateTypeMultiplier = (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagStdDevMultiplier : 1; + 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 = estimateTypeMultiplier * VisionConstants.kTranslationStdDevCoeff * Math.pow(avgTargetDistance, 3) + * poseAmbiguityMultiplier / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); final double rotationStdDev = estimateTypeMultiplier * VisionConstants.kRotationStdDevCoeff * Math.pow(avgTargetDistance, 3) + * poseAmbiguityMultiplier / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index ce4dd8c2..e32ec928 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -149,4 +149,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; } From 8a423c01eb361faf069d6dbae8e39788500507cf Mon Sep 17 00:00:00 2001 From: Vincent Zheng <92338199+Vncero@users.noreply.github.com> Date: Fri, 28 Mar 2025 17:41:11 +0000 Subject: [PATCH 5/6] fix(vision): address review comment with logic to use reef pose estimate when one tag is seen Co-authored-by: Cam Huang --- src/main/java/frc/robot/RobotContainer.java | 9 ++--- .../auto/AutomaticAutonomousMaker3000.java | 30 ++++++++------- .../java/frc/robot/commands/ReefAlign.java | 38 ++++++++++++++----- .../subsystems/drivetrain/DrivetrainReal.java | 8 ++-- .../subsystems/drivetrain/DrivetrainSim.java | 8 ++-- .../subsystems/drivetrain/SwerveDrive.java | 15 ++++++-- .../frc/robot/subsystems/vision/Camera.java | 27 +++++++++++-- .../frc/robot/subsystems/vision/Vision.java | 10 ++++- .../subsystems/vision/VisionConstants.java | 9 ++++- .../frc/robot/subsystems/vision/VisionIO.java | 1 + .../robot/subsystems/vision/VisionIOReal.java | 21 ++++++++-- .../robot/subsystems/vision/VisionIOSim.java | 13 +++++++ 12 files changed, 138 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5879c095..de5880e7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,9 +49,6 @@ 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 @@ -69,6 +66,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); @@ -191,7 +190,7 @@ private void configureTuningBindings() { // System.out.println("Changing volts to: " + volts); // })); - driver.a().whileTrue(ReefAlign.tuneAlignment(drivetrain)); + driver.a().whileTrue(ReefAlign.tuneAlignment(drivetrain, vision::canSeeReefTag)); driver.b().whileTrue(coralSuperstructure.feedCoral()); @@ -266,7 +265,7 @@ private void configureBindings() { .andThen( // when we get close enough, align to reef, but only while we're // close enough - ReefAlign.alignToReef(drivetrain, () -> queuedReefPosition) + ReefAlign.alignToReef(drivetrain, () -> queuedReefPosition, vision::canSeeReefTag) // .until(drivetrain::atPoseSetpoint) // .andThen( // ReefAlign.rotateToNearestReefTag(drivetrain, driverForward, diff --git a/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java b/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java index 47fb78f8..5401e64f 100644 --- a/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java +++ b/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java @@ -22,6 +22,8 @@ import java.io.IOException; import java.util.ArrayList; import java.util.List; +import java.util.function.IntPredicate; + import org.json.simple.parser.ParseException; @Logged @@ -123,7 +125,7 @@ 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; @@ -151,14 +153,14 @@ 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<>()); }; @@ -212,7 +214,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 = @@ -238,7 +240,7 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) { withScoring( toPathCommand(path, true).asProxy(), config.scoringGroup.get(i).pole, - config.scoringGroup.get(i).level)); + config.scoringGroup.get(i).level, useReefPoseEstimate)); paths.add(path); } else { @@ -261,7 +263,7 @@ public PathsAndAuto buildAuto(CycleAutoConfig config) { withScoring( toPathCommand(scorePath).asProxy(), 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); @@ -296,7 +298,7 @@ public Command withIntaking(Command path, CoralSide side) { .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; @@ -313,14 +315,14 @@ public Command withScoring(Command path, Pole pole, Level level) { .asProxy()) .andThen( ReefAlign.alignToReef( - drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT) + drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT, useReefPoseEstimate) .asProxy() .alongWith(coralSuperstructure.goToSetpoint(() -> setpoint).asProxy()) .until(() -> drive.atPoseSetpoint() && coralSuperstructure.atTargetState(setpoint)) .withTimeout(2.5)) .andThen( ReefAlign.alignToReef( - drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT) + drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT, useReefPoseEstimate) .asProxy() .alongWith(coralSuperstructure.goToSetpoint(() -> setpoint).asProxy()) .asProxy() diff --git a/src/main/java/frc/robot/commands/ReefAlign.java b/src/main/java/frc/robot/commands/ReefAlign.java index 1c3052c1..448b8b5b 100644 --- a/src/main/java/frc/robot/commands/ReefAlign.java +++ b/src/main/java/frc/robot/commands/ReefAlign.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotConstants; import frc.robot.subsystems.drivetrain.SwerveDrive; +import frc.robot.subsystems.vision.EstimateType; import frc.robot.util.AprilTagUtil; import frc.robot.util.MyAlliance; import frc.robot.util.ReefPosition; @@ -25,6 +26,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 { @@ -178,22 +180,31 @@ private static Pose2d getNearestRightAlign(int reefTagID) { } public static Command alignToReef( - SwerveDrive swerveDrive, Supplier targetReefPosition) { - return swerveDrive.driveToFieldPose( + SwerveDrive swerveDrive, Supplier targetReefPosition, IntPredicate useReefPoseEstimate) { + return swerveDrive.driveToFieldPose( () -> { - final Pose2d target = - switch (targetReefPosition.get()) { - case ALGAE -> centerAlignPoses.get(getNearestReefID(swerveDrive.getPose())); - case LEFT -> leftAlignPoses.get(getNearestReefID(swerveDrive.getPose())); - case RIGHT -> rightAlignPoses.get(getNearestReefID(swerveDrive.getPose())); - default -> swerveDrive.getPose(); // more or less a no-op - }; + final int nearestReefID = getNearestReefID(swerveDrive.getPose()); + final Pose2d target = switch (targetReefPosition.get()) { + case ALGAE -> centerAlignPoses.get(nearestReefID); + case LEFT -> centerAlignPoses.get(nearestReefID); + case RIGHT -> rightAlignPoses.get(nearestReefID); + default -> swerveDrive.getPose(); + }; + swerveDrive.setAlignmentSetpoint(target); + return target; + }, () -> { + 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", kRightAlignDistance.in(Inch)); @@ -206,6 +217,13 @@ public static Command tuneAlignment(SwerveDrive swerveDrive) { Inches.of(depth.get()), Inches.of(side.get()), kReefAlignmentRotation)); swerveDrive.setAlignmentSetpoint(pose); return pose; + }, () -> { + 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 fdf477c2..8abdd5d7 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java @@ -229,13 +229,13 @@ public Command driveToRobotPose(Supplier pose) { } @Override - public void driveToFieldPose(Pose2d pose) { + public void driveToFieldPose(Pose2d target, Pose2d current) { ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize( - xPoseController.calculate(getPose().getX(), pose.getX()), - yPoseController.calculate(getPose().getY(), pose.getY()), + xPoseController.calculate(current.getX(), target.getX()), + yPoseController.calculate(current.getY(), target.getY()), thetaController.calculate( - getPose().getRotation().getRadians(), pose.getRotation().getRadians()), + current.getRotation().getRadians(), target.getRotation().getRadians()), DrivetrainConstants.kLoopDt.in(Seconds)); if (atPoseSetpoint()) targetSpeeds = new ChassisSpeeds(); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java index 361fc208..1bdb56e3 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java @@ -191,15 +191,15 @@ public Command driveToRobotPose(Supplier pose) { } @Override - public void driveToFieldPose(Pose2d pose) { + public void driveToFieldPose(Pose2d pose, Pose2d current) { if (pose == null) return; ChassisSpeeds targetSpeeds = new ChassisSpeeds( - xPoseController.calculate(getPose().getX(), pose.getX()), - yPoseController.calculate(getPose().getY(), pose.getY()), + xPoseController.calculate(current.getX(), pose.getX()), + yPoseController.calculate(current.getY(), pose.getY()), thetaController.calculate( - getPose().getRotation().getRadians(), pose.getRotation().getRadians())); + current.getRotation().getRadians(), pose.getRotation().getRadians())); if (atPoseSetpoint()) targetSpeeds = new ChassisSpeeds(); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java index 16c1523e..47e9768d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -1,11 +1,15 @@ /* (C) Robolancers 2025 */ package frc.robot.subsystems.drivetrain; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.util.DriveFeedforwards; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; @@ -21,9 +25,8 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; +import frc.robot.subsystems.vision.EstimateType; import frc.robot.util.MyAlliance; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; /* * drive interface. A real and sim implementation is made out of this. Using this, so we can implement maplesim. @@ -153,9 +156,13 @@ default Command driveFixedHeading( Command driveToRobotPose(Supplier pose); // field relative auto drive w/ external pid controllers - void driveToFieldPose(Pose2d pose); + void driveToFieldPose(Pose2d target, Pose2d current); default Command driveToFieldPose(Supplier pose) { + return driveToFieldPose(pose, this::getPose); + } + + default Command driveToFieldPose(Supplier pose, Supplier robotPose) { return runOnce( () -> { xPoseController.reset(); @@ -163,7 +170,7 @@ default Command driveToFieldPose(Supplier pose) { thetaController.reset(); setAlignmentSetpoint(pose.get()); }) - .andThen(run(() -> driveToFieldPose(pose.get()))); + .andThen(run(() -> driveToFieldPose(pose.get(), robotPose.get()))); } void resetPose(Pose2d pose); diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 60e2af1f..7d60fa74 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -61,6 +61,22 @@ public Camera( 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; } @@ -158,15 +174,20 @@ private Matrix calculateStdDevs( final double estimateTypeMultiplier = (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagStdDevMultiplier : 1; + final double targetDistancePower = + (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagTargetDistancePower : VisionConstants.kMultiTagTargetDistancePower; + + final double translationStdDev = - estimateTypeMultiplier + estimateTypeMultiplier * VisionConstants.kTranslationStdDevCoeff - * Math.pow(avgTargetDistance, 3) + * Math.pow(avgTargetDistance, targetDistancePower) / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); + final double rotationStdDev = estimateTypeMultiplier * VisionConstants.kRotationStdDevCoeff - * Math.pow(avgTargetDistance, 3) + * Math.pow(avgTargetDistance, targetDistancePower) / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); return VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 554e0d5c..b8fc721d 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -42,18 +42,24 @@ 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) { switch (est.estimateType()) { - case MULTI_TAG -> visionDataConsumer.accept(est); + 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 ce4dd8c2..0b93c539 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -21,8 +21,13 @@ public class VisionConstants { public static final double kTranslationStdDevCoeff = 1e-1; public static final double kRotationStdDevCoeff = 1e-1; - // TODO: tune in sim, represents (to some extent) how much more single tag estimates are trusted - public static final double kSingleTagStdDevMultiplier = 1e-2; + // 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, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 829ec4a6..1c82f7cc 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -6,4 +6,5 @@ @Logged public interface VisionIO { VisionEstimate[] getLatestEstimates(); + boolean reefCameraCanSeeReefTag(int tagID); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java index 4a122f79..bb8db9c9 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java @@ -1,15 +1,17 @@ /* (C) Robolancers 2025 */ 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; +import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.vision.VisionConstants.CameraConfig; + @Logged public class VisionIOReal implements VisionIO { private final List cameras; @@ -30,4 +32,17 @@ public VisionEstimate[] getLatestEstimates() { .filter(Objects::nonNull) .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; + } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java index f6b9818f..9033e8a3 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java @@ -59,4 +59,17 @@ public VisionEstimate[] getLatestEstimates() { .filter(Objects::nonNull) .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; + } } From 1bb15dfcaa91c968e51d36694b0da0e9679fe884 Mon Sep 17 00:00:00 2001 From: Vincent Zheng <92338199+Vncero@users.noreply.github.com> Date: Fri, 28 Mar 2025 18:44:48 +0000 Subject: [PATCH 6/6] fix(spotless): ran spotless --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../auto/AutomaticAutonomousMaker3000.java | 64 +++++++++++-------- .../java/frc/robot/commands/ReefAlign.java | 27 +++++--- .../subsystems/drivetrain/DrivetrainReal.java | 3 +- .../subsystems/drivetrain/DrivetrainSim.java | 5 +- .../subsystems/drivetrain/SwerveDrive.java | 7 +- .../frc/robot/subsystems/vision/Camera.java | 27 ++++---- .../frc/robot/subsystems/vision/Vision.java | 2 +- .../subsystems/vision/VisionConstants.java | 5 +- .../frc/robot/subsystems/vision/VisionIO.java | 3 +- .../robot/subsystems/vision/VisionIOReal.java | 8 +-- 11 files changed, 86 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f69ca91d..ae3ad2da 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -410,7 +410,10 @@ private void configureBindings() { // .getTargetAngle() // .isEquivalent( // queuedSetpoint.getArmAngle())), - ReefAlign.alignToReef(drivetrain, () -> queuedReefPosition, vision::canSeeReefTag)) + 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 495e4377..7dd736eb 100644 --- a/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java +++ b/src/main/java/frc/robot/auto/AutomaticAutonomousMaker3000.java @@ -27,7 +27,6 @@ import java.util.ArrayList; import java.util.List; import java.util.function.IntPredicate; - import org.json.simple.parser.ParseException; @Logged @@ -105,7 +104,10 @@ public class AutomaticAutonomousMaker3000 { private Command storedAuto; - public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coralSuperstructure, IntPredicate useReefPoseEstimate) { + public AutomaticAutonomousMaker3000( + SwerveDrive drive, + CoralSuperstructure coralSuperstructure, + IntPredicate useReefPoseEstimate) { this.drive = drive; this.coralSuperstructure = coralSuperstructure; @@ -137,9 +139,12 @@ public AutomaticAutonomousMaker3000(SwerveDrive drive, CoralSuperstructure coral case MIDTOPAUTO -> buildAuto(kMidLaneTopAuto, useReefPoseEstimate); case MIDBOTAUTO -> buildAuto(kMidLaneBotAuto, useReefPoseEstimate); case BOTAUTO -> buildAuto(kBotLaneAuto, useReefPoseEstimate); - case MIDPRELOADAUTO -> buildAuto(kMidLaneBotPreloadAuto, useReefPoseEstimate); // test auto again + case MIDPRELOADAUTO -> + buildAuto( + kMidLaneBotPreloadAuto, useReefPoseEstimate); // test auto again case MIDOPPOSITESIDEAUTO -> - buildAuto(kMidLaneOppositeSideAuto, useReefPoseEstimate); // test auto x2 + buildAuto( + kMidLaneOppositeSideAuto, useReefPoseEstimate); // test auto x2 case CUSTOM -> buildAuto(autoChooser.build(), useReefPoseEstimate); default -> new PathsAndAuto(Commands.none(), new ArrayList<>()); }; @@ -237,7 +242,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config, IntPredicate useReefPoseEs withScoring( toPathCommand(pathNewGoalEndState, true), config.scoringGroup.get(i).pole, - config.scoringGroup.get(i).level, useReefPoseEstimate)); + config.scoringGroup.get(i).level, + useReefPoseEstimate)); paths.add(path); } else { @@ -271,7 +277,8 @@ public PathsAndAuto buildAuto(CycleAutoConfig config, IntPredicate useReefPoseEs withScoring( toPathCommand(scorePathNewGoalEndState), config.scoringGroup.get(i).pole, - config.scoringGroup.get(i).level, useReefPoseEstimate)); + config.scoringGroup.get(i).level, + useReefPoseEstimate)); lastReefSide = config.scoringGroup.get(i).reefSide; paths.add(intakePath); @@ -322,7 +329,8 @@ public Command withIntaking(Command path, FeedLocation location) { coralSuperstructure.feedCoral().until(() -> coralSuperstructure.hasCoral()))); } - public Command withScoring(Command path, Pole pole, Level level, IntPredicate useReefPoseEstimate) { + public Command withScoring( + Command path, Pole pole, Level level, IntPredicate useReefPoseEstimate) { CoralScorerSetpoint setpoint = switch (level) { default -> CoralScorerSetpoint.L1; @@ -342,30 +350,32 @@ public Command withScoring(Command path, Pole pole, Level level, IntPredicate us coralSuperstructure .goToSetpointProfiled(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle()) .alongWith(coralSuperstructure.getEndEffector().stallCoralIfDetected())) - .andThen( + .andThen( ReefAlign.alignToReef( - drive, () -> pole == Pole.LEFTPOLE ? ReefPosition.LEFT : ReefPosition.RIGHT, useReefPoseEstimate)) - .asProxy() - .alongWith(coralSuperstructure.goToSetpointPID(() -> preAlignElevatorHeight, () -> setpoint.getArmAngle()).asProxy()) - .asProxy() - .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 bd76c7fc..96cece57 100644 --- a/src/main/java/frc/robot/commands/ReefAlign.java +++ b/src/main/java/frc/robot/commands/ReefAlign.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.RobotConstants; import frc.robot.subsystems.drivetrain.SwerveDrive; -import frc.robot.subsystems.vision.EstimateType; import frc.robot.subsystems.drivetrain.SwerveDrive.AlignmentSetpoint; import frc.robot.subsystems.leds.Leds; import frc.robot.util.AprilTagUtil; @@ -199,7 +198,9 @@ private static Pose2d getNearestRightAlign(int reefTagID) { } public static Command alignToReef( - SwerveDrive swerveDrive, Supplier targetReefPosition, IntPredicate useReefPoseEstimate) { + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true) .andThen( swerveDrive.driveToFieldPose( @@ -213,7 +214,8 @@ public static Command alignToReef( }; return new AlignmentSetpoint(target, true); - }, () -> { + }, + () -> { final int nearestReefID = getNearestReefID(swerveDrive.getPose()); if (useReefPoseEstimate.test(nearestReefID)) { return swerveDrive.getReefVisionPose(); @@ -225,7 +227,9 @@ public static Command alignToReef( } public static Command alignToPrealignReef( - SwerveDrive swerveDrive, Supplier targetReefPosition, IntPredicate useReefPoseEstimate) { + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { return Commands.runOnce(() -> Leds.getInstance().isReefAligning = true) .andThen( swerveDrive.driveToFieldPose( @@ -244,7 +248,8 @@ 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(); @@ -256,8 +261,10 @@ public static Command alignToPrealignReef( } public static Command alignToTag( - SwerveDrive swerveDrive, Supplier targetReefPosition, IntPredicate useReefPoseEstimate) { - return swerveDrive.driveToFieldPose( + SwerveDrive swerveDrive, + Supplier targetReefPosition, + IntPredicate useReefPoseEstimate) { + return swerveDrive.driveToFieldPose( () -> { Pose2d target = switch (targetReefPosition.get()) { @@ -274,7 +281,8 @@ 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(); @@ -296,7 +304,8 @@ public static Command tuneAlignment(SwerveDrive swerveDrive, IntPredicate useRee 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(); diff --git a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java index bcd40f96..8a23d86b 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainReal.java @@ -229,8 +229,7 @@ public void driveToFieldPose(Pose2d target, Pose2d current) { < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters)) targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond); - if (Math.abs( - current.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 0f768b7c..f698d255 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/DrivetrainSim.java @@ -7,8 +7,6 @@ import static edu.wpi.first.units.Units.Pounds; import static edu.wpi.first.units.Units.Seconds; -import java.util.function.Supplier; - import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.MathUtil; @@ -207,8 +205,7 @@ public void driveToFieldPose(Pose2d pose, Pose2d current) { < DrivetrainConstants.kAlignmentSetpointTranslationTolerance.in(Meters)) targetSpeeds = new ChassisSpeeds(0, 0, targetSpeeds.omegaRadiansPerSecond); - if (Math.abs( - current.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 85949efa..77776035 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java @@ -1,15 +1,11 @@ /* (C) Robolancers 2025 */ package frc.robot.subsystems.drivetrain; -import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.util.DriveFeedforwards; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -27,8 +23,9 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.robot.subsystems.vision.EstimateType; import frc.robot.util.MyAlliance; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; /* * drive interface. A real and sim implementation is made out of this. Using this, so we can implement maplesim. diff --git a/src/main/java/frc/robot/subsystems/vision/Camera.java b/src/main/java/frc/robot/subsystems/vision/Camera.java index 833268a3..035cf0fd 100644 --- a/src/main/java/frc/robot/subsystems/vision/Camera.java +++ b/src/main/java/frc/robot/subsystems/vision/Camera.java @@ -112,11 +112,11 @@ public VisionEstimate tryLatestEstimate() { .filter( poseEst -> VisionConstants.kAllowedFieldArea.contains( - poseEst.estimatedPose.getTranslation().toTranslation2d()) + poseEst.estimatedPose.getTranslation().toTranslation2d()) && poseEst - .estimatedPose - .getMeasureZ() - .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) + .estimatedPose + .getMeasureZ() + .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) .map( photonEst -> { final var visionEst = @@ -140,10 +140,10 @@ public VisionEstimate tryLatestEstimate() { poseEst -> VisionConstants.kAllowedFieldArea.contains( poseEst.estimatedPose.getTranslation().toTranslation2d()) - && poseEst - .estimatedPose - .getMeasureZ() - .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) + && poseEst + .estimatedPose + .getMeasureZ() + .isNear(Meters.zero(), VisionConstants.kAllowedFieldHeight)) .map( photonEst -> { final var visionEst = @@ -189,9 +189,10 @@ private Matrix calculateStdDevs( final double estimateTypeMultiplier = (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagStdDevMultiplier : 1; - final double targetDistancePower = - (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagTargetDistancePower : VisionConstants.kMultiTagTargetDistancePower; - + final double targetDistancePower = + (estimateType == EstimateType.SINGLE_TAG) + ? VisionConstants.kSingleTagTargetDistancePower + : VisionConstants.kMultiTagTargetDistancePower; if (visionPoseEstimate.targetsUsed.get(0).poseAmbiguity > VisionConstants.kAmbiguityThreshold && estimateType == EstimateType.SINGLE_TAG) { @@ -208,12 +209,12 @@ private Matrix calculateStdDevs( * VisionConstants.kAmbiguityScalar); final double translationStdDev = - estimateTypeMultiplier + estimateTypeMultiplier * VisionConstants.kTranslationStdDevCoeff * Math.pow(avgTargetDistance, targetDistancePower) * poseAmbiguityMultiplier / Math.pow(visionPoseEstimate.targetsUsed.size(), 3); - + final double rotationStdDev = estimateTypeMultiplier * VisionConstants.kRotationStdDevCoeff diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d1e75018..0c68e07f 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -44,7 +44,7 @@ private Vision( public boolean canSeeReefTag(int tagID) { return io.reefCameraCanSeeReefTag(tagID); - } + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 3b4a2465..fbb0d976 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -21,9 +21,10 @@ 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 + // 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 diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index e32ae9ff..cbdad184 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -6,7 +6,8 @@ @Logged public interface VisionIO { VisionEstimate[] getLatestEstimates(); - boolean reefCameraCanSeeReefTag(int tagID); + + 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 a9dc86f3..831612a0 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java @@ -1,17 +1,15 @@ /* (C) Robolancers 2025 */ 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; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.vision.VisionConstants.CameraConfig; - @Logged public class VisionIOReal implements VisionIO { private final List cameras;