From 9549c5343d7f80c236d71c49bfdb20e4abbc1d15 Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Wed, 11 Feb 2026 09:50:40 -0500 Subject: [PATCH] Final commit before merging to main to continue progress --- src/main/java/frc/robot/RobotContainer.java | 5 +++ .../java/frc/robot/subsystems/Drivetrain.java | 12 +++++- .../frc/robot/subsystems/PhotonVision.java | 42 ++++++++++++++----- 3 files changed, 47 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc7c988..3c47a95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,10 +17,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.AutoConstants; import frc.robot.constants.OIConstants; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.PhotonVision; import frc.robot.utilities.Elastic; import frc.robot.utilities.Utilities; public class RobotContainer { + private PhotonVision vision; private Drivetrain drivetrain; private CommandXboxController driver; @@ -30,8 +32,11 @@ public class RobotContainer { private Timer shiftTimer; public RobotContainer() { + vision = new PhotonVision(); drivetrain = new Drivetrain(); + vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); + driver = new CommandXboxController(OIConstants.kDriverControllerPort); shiftTimer = new Timer(); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index bd1138a..192105c 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.AutoConstants; import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.OIConstants; +import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import frc.robot.utilities.SwerveModule; public class Drivetrain extends SubsystemBase { @@ -172,7 +173,7 @@ public class Drivetrain extends SubsystemBase { } // TODO check both cameras - public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { + /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) { if (camera1 == null) { return new PrintCommand("Camera 1 not available"); } @@ -196,7 +197,7 @@ public class Drivetrain extends SubsystemBase { () -> false ) ); - } + }*/ public Command drivePathPlannerPath(PathPlannerPath path) { if(AutoConstants.kAutoConfigOk) { @@ -228,6 +229,13 @@ public class Drivetrain extends SubsystemBase { }); } + public void consumeVisualPose(VisualPose pose) { + estimator.addVisionMeasurement( + pose.visualPose(), + pose.timestamp() + ); + } + public void resetEncoders() { frontLeft.resetEncoders(); frontRight.resetEncoders(); diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index a3981c1..d90dece 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -2,10 +2,12 @@ package frc.robot.subsystems; import java.io.IOException; import java.util.ArrayList; +import java.util.Comparator; import java.util.List; import java.util.Optional; import java.util.OptionalDouble; import java.util.function.Consumer; +import java.util.stream.Stream; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -26,17 +28,17 @@ import frc.robot.interfaces.IVisualPoseProvider; import frc.robot.interfaces.IVisualPoseProvider.VisualPose; import frc.robot.utilities.PhotonVisionConfig; -public class PhotonVision extends SubsystemBase implements IAprilTagProvider { +public class PhotonVision extends SubsystemBase { private PhotonCamera[] cameras; private PhotonPoseEstimator[] estimators; - private PhotonPipelineResult[] latestResults; + private List latestResults; private ArrayList> poseEstimateConsumers; public PhotonVision() { cameras = new PhotonCamera[PhotonConstants.configs.size()]; estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()]; - latestResults = new PhotonPipelineResult[PhotonConstants.configs.size()]; + latestResults = new ArrayList(); for(int i = 0; i < PhotonConstants.configs.size(); i++) { cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName()); @@ -45,7 +47,7 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider { PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, PhotonConstants.configs.get(i).robotToCamera() ); - latestResults[i] = null; + latestResults.add(null); } poseEstimateConsumers = new ArrayList>(); @@ -57,10 +59,10 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider { List results = cameras[i].getAllUnreadResults(); if(!results.isEmpty()) { - latestResults[i] = results.get(results.size() - 1); + latestResults.set(i, results.get(results.size() - 1)); } - Optional pose = estimators[i].update(latestResults[i]); + Optional pose = estimators[i].update(latestResults.get(i)); if(!pose.isEmpty()) { VisualPose visualPose = new VisualPose( @@ -75,7 +77,27 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider { } } - public Trigger tagPrescenseTrigger(int targetTag) { + public void testMethod(int targetID) { + Optional target = latestResults.stream() + .filter((p) -> p != null) + .map(PhotonPipelineResult::getTargets) + .map(List::stream) + .reduce(Stream::concat) + .get() + .filter((p) -> p.getFiducialId() == targetID) + .max( + Comparator.comparingDouble((ptt) -> { + return (double)ptt.getDetectedObjectConfidence(); + }) + ); + + } + + public void addPoseEstimateConsumer(Consumer consumer) { + poseEstimateConsumers.add(consumer); + } + + /*public Trigger tagPrescenseTrigger(int targetTag) { return new Trigger(() -> { return List.of(latestResults).stream() .filter((p) -> p != null) @@ -85,8 +107,8 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider { }); }); }); - } - + }*/ +/* @Override public OptionalDouble getTagDistanceFromCameraByID(int id) { if (latestResult == null) { @@ -172,5 +194,5 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider { return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray(); } - + */ }