Final commit before merging to main to continue progress

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();

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();

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<PhotonPipelineResult> latestResults;
 
     private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;
 
     public PhotonVision() {
         cameras = new PhotonCamera[PhotonConstants.configs.size()];
         estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
-        latestResults = new PhotonPipelineResult[PhotonConstants.configs.size()];
+        latestResults = new ArrayList<PhotonPipelineResult>();
 
         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<Consumer<VisualPose>>();
@@ -57,10 +59,10 @@ public class PhotonVision extends SubsystemBase implements IAprilTagProvider {
             List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
 
             if(!results.isEmpty()) {
-                latestResults[i] = results.get(results.size() - 1);
+                latestResults.set(i, results.get(results.size() - 1));
             }
 
-            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults[i]);
+            Optional<EstimatedRobotPose> 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<PhotonTrackedTarget> 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<VisualPose> 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();
     }
-    
+    */
 }