Add Object Detection Support

src/main/java/frc/alotobots/RobotContainer.java

@@ -9,7 +9,8 @@
 import frc.alotobots.library.drivetrains.swerve.ctre.mk4il22023.TunerConstants;
 import frc.alotobots.library.pneumatics.PneumaticsSubsystem;
 import frc.alotobots.library.vision.limelight.LimelightSubsystem;
-import frc.alotobots.library.vision.photonvision.PhotonvisionSubsystem;
+import frc.alotobots.library.vision.photonvision.apriltag.PhotonvisionAprilTagSubsystem;
+import frc.alotobots.library.vision.photonvision.objectdetection.PhotonVisionObjectDetectionSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -23,7 +24,8 @@ public class RobotContainer {
   private final SwerveDriveSubsystem drivetrainSubsystem;
   private final LimelightSubsystem limelightSubsystem;
   private final BlingSubsystem blingSubsystem;
-  private final PhotonvisionSubsystem photonvisionSubsystem;
+  private final PhotonvisionAprilTagSubsystem photonvisionAprilTagSubsystem;
+  private final PhotonVisionObjectDetectionSubsystem photonvisionObjectDetectionSubsystem;
   private final PneumaticsSubsystem pneumaticsSubsystem;
 
   // Human-Machine Interface
@@ -42,7 +44,9 @@ public RobotContainer() {
     drivetrainSubsystem = TunerConstants.DRIVE_TRAIN;
     blingSubsystem = new BlingSubsystem();
     limelightSubsystem = new LimelightSubsystem(blingSubsystem, drivetrainSubsystem);
-    photonvisionSubsystem = new PhotonvisionSubsystem();
+    photonvisionAprilTagSubsystem = new PhotonvisionAprilTagSubsystem();
+    photonvisionObjectDetectionSubsystem =
+        new PhotonVisionObjectDetectionSubsystem(drivetrainSubsystem);
     pneumaticsSubsystem = new PneumaticsSubsystem();
 
     // Initialize HMI
@@ -100,6 +104,6 @@ public Command getAutonomousCommand() {
 
   /** Sets up the PhotonVision subsystem for the drivetrain. */
   public void setupVision() {
-    drivetrainSubsystem.setPhotonVisionSubSys(photonvisionSubsystem);
+    drivetrainSubsystem.setPhotonVisionSubSys(photonvisionAprilTagSubsystem);
   }
 }

src/main/java/frc/alotobots/game/HMIStation.java

@@ -11,8 +11,8 @@
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.alotobots.Constants.Robot.Calibrations.DriveTrain;
-import frc.alotobots.library.driverstation.JoystickUtilities;
 import frc.alotobots.game.constants.HMIDeadbands;
+import frc.alotobots.library.driverstation.JoystickUtilities;
 
 /**
  * The DriverStation class represents the available inputs to the robot, providing access to

src/main/java/frc/alotobots/game/constants/HMIDeadbands.java

@@ -1,17 +1,17 @@
 package frc.alotobots.game.constants;
 
 public final class HMIDeadbands {
-    private HMIDeadbands() {} // Prevent instantiation
+  private HMIDeadbands() {} // Prevent instantiation
 
-    // Basic drive control deadbands
-    public static final double DRIVER_FWD_AXIS_DEADBAND = 0.1;
-    public static final double DRIVER_STR_AXIS_DEADBAND = 0.1;
-    public static final double DRIVER_ROT_AXIS_DEADBAND = 0.1;
-    public static final double TRIGGER_DEADBAND = 0.3;
+  // Basic drive control deadbands
+  public static final double DRIVER_FWD_AXIS_DEADBAND = 0.1;
+  public static final double DRIVER_STR_AXIS_DEADBAND = 0.1;
+  public static final double DRIVER_ROT_AXIS_DEADBAND = 0.1;
+  public static final double TRIGGER_DEADBAND = 0.3;
 
-    /* Example game-specific deadbands:
-     * public static final double PRIMARY_MECHANISM_AXIS_DEADBAND = 0.1;
-     * public static final double SECONDARY_MECHANISM_AXIS_DEADBAND = 0.15;
-     * public static final double TERTIARY_MECHANISM_AXIS_DEADBAND = 0.2;
-     */
+  /* Example game-specific deadbands:
+   * public static final double PRIMARY_MECHANISM_AXIS_DEADBAND = 0.1;
+   * public static final double SECONDARY_MECHANISM_AXIS_DEADBAND = 0.15;
+   * public static final double TERTIARY_MECHANISM_AXIS_DEADBAND = 0.2;
+   */
 }

src/main/java/frc/alotobots/library/drivetrains/swerve/ctre/SwerveDriveSubsystem.java

@@ -1,7 +1,7 @@
 package frc.alotobots.library.drivetrains.swerve.ctre;
 
-import static frc.alotobots.library.vision.photonvision.PhotonvisionSubsystemConstants.ONLY_USE_POSE_ESTIMATION_IN_TELEOP;
-import static frc.alotobots.library.vision.photonvision.PhotonvisionSubsystemConstants.USE_VISION_POSE_ESTIMATION;
+import static frc.alotobots.library.vision.photonvision.apriltag.PhotonvisionAprilTagSubsystemConstants.ONLY_USE_POSE_ESTIMATION_IN_TELEOP;
+import static frc.alotobots.library.vision.photonvision.apriltag.PhotonvisionAprilTagSubsystemConstants.USE_VISION_POSE_ESTIMATION;
 
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.Slot0Configs;
@@ -19,7 +19,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import frc.alotobots.library.drivetrains.swerve.ctre.mk4il22023.TunerConstants;
-import frc.alotobots.library.vision.photonvision.PhotonvisionSubsystem;
+import frc.alotobots.library.vision.photonvision.apriltag.PhotonvisionAprilTagSubsystem;
 import java.util.Optional;
 import java.util.function.Supplier;
 import lombok.Getter;
@@ -35,7 +35,7 @@ public class SwerveDriveSubsystem extends SwerveDrivetrain implements Subsystem
   private final SwerveRequest.ApplyChassisSpeeds autoRequest =
       new SwerveRequest.ApplyChassisSpeeds();
 
-  private PhotonvisionSubsystem subSysPhotonvision;
+  private PhotonvisionAprilTagSubsystem subSysPhotonvision;
   private SwerveDriveTelemetry telemetry;
   private SwerveDrivePathPlanner pathPlanner;
 
@@ -143,7 +143,7 @@ private void updateVisionPoseEstimate() {
    *
    * @param subSysPhotonvision The PhotonVision subsystem to use.
    */
-  public void setPhotonVisionSubSys(PhotonvisionSubsystem subSysPhotonvision) {
+  public void setPhotonVisionSubSys(PhotonvisionAprilTagSubsystem subSysPhotonvision) {
     this.subSysPhotonvision = subSysPhotonvision;
   }
 

src/main/java/frc/alotobots/library/vision/photonvision/PhotonvisionSubsystem.java → src/main/java/frc/alotobots/library/vision/photonvision/apriltag/PhotonvisionAprilTagSubsystem.java

@@ -1,6 +1,6 @@
-package frc.alotobots.library.vision.photonvision;
+package frc.alotobots.library.vision.photonvision.apriltag;
 
-import static frc.alotobots.library.vision.photonvision.PhotonvisionSubsystemConstants.*;
+import static frc.alotobots.library.vision.photonvision.apriltag.PhotonvisionAprilTagSubsystemConstants.*;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
@@ -20,26 +20,27 @@
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 /**
- * A subsystem that manages multiple PhotonVision cameras and provides pose estimation
- * functionality.
+ * A subsystem that manages multiple PhotonVision cameras specifically for AprilTag detection and
+ * provides pose estimation functionality using AprilTag fiducial markers.
  */
-public class PhotonvisionSubsystem extends SubsystemBase {
-  private final AprilTagFieldLayout aprilTagFieldLayout;
-  private ArrayList<PhotonPoseEstimator> photonPoseEstimators;
-  private final PhotonvisionTelemetry telemetry;
-  private final boolean[] cameraEnabled;
+public class PhotonvisionAprilTagSubsystem extends SubsystemBase {
+  private final AprilTagFieldLayout fieldLayout;
+  private ArrayList<PhotonPoseEstimator> poseEstimators;
+  private final PhotonvisionAprilTagTelemetry telemetry;
+  private final boolean[] camerasEnabled;
 
   // Smoothing filter state
   private Pose3d lastSmoothedPose;
 
-  /** Constructs a new PhotonvisionSubsystem. */
-  public PhotonvisionSubsystem() {
-    aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
-    photonPoseEstimators = new ArrayList<>();
-    cameraEnabled = new boolean[CAMERAS.length];
-    Arrays.fill(cameraEnabled, true);
+  /** Constructs a new PhotonvisionAprilTagSubsystem for AprilTag detection and pose estimation. */
+  public PhotonvisionAprilTagSubsystem() {
+    this.fieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+    this.poseEstimators = new ArrayList<>();
+    this.camerasEnabled = new boolean[CAMERAS.length];
+    Arrays.fill(camerasEnabled, true);
     initializePoseEstimators();
-    telemetry = new PhotonvisionTelemetry();
+    telemetry = new PhotonvisionAprilTagTelemetry();
+    System.out.println("PhotonVision AprilTag subsystem initialized");
   }
 
   /**
@@ -48,7 +49,7 @@ public PhotonvisionSubsystem() {
    * camera offsets are not properly configured.
    */
   private void initializePoseEstimators() {
-    photonPoseEstimators = new ArrayList<>();
+    poseEstimators = new ArrayList<>();
 
     if (CAMERAS.length != CAMERA_OFFSETS.length) {
       throw new RuntimeException(
@@ -60,17 +61,17 @@ private void initializePoseEstimators() {
       if (CAMERAS[i] != null) {
         PhotonPoseEstimator estimator =
             new PhotonPoseEstimator(
-                aprilTagFieldLayout,
+                fieldLayout,
                 PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
                 CAMERAS[i],
                 CAMERA_OFFSETS[i]);
         estimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY);
-        photonPoseEstimators.add(estimator);
+        poseEstimators.add(estimator);
 
-        cameraEnabled[i] = CAMERAS[i].isConnected();
+        camerasEnabled[i] = CAMERAS[i].isConnected();
       } else {
-        photonPoseEstimators.add(null);
-        cameraEnabled[i] = false;
+        poseEstimators.add(null);
+        camerasEnabled[i] = false;
       }
     }
   }
@@ -116,9 +117,9 @@ private List<PhotonTrackedTarget> getDetectedTags() {
    */
   public Optional<Pair<Pose3d, Double>> getEstimatedVisionPose3d(Pose2d previousPose) {
     ArrayList<EstimatedRobotPose> estimates = new ArrayList<>();
-    for (int i = 0; i < photonPoseEstimators.size(); i++) {
-      PhotonPoseEstimator estimator = photonPoseEstimators.get(i);
-      if (estimator != null && cameraEnabled[i] && CAMERAS[i].isConnected()) {
+    for (int i = 0; i < poseEstimators.size(); i++) {
+      PhotonPoseEstimator estimator = poseEstimators.get(i);
+      if (estimator != null && camerasEnabled[i] && CAMERAS[i].isConnected()) {
         estimator.setLastPose(previousPose);
         var estimate = estimator.update();
         if (estimate.isPresent()) {
@@ -137,9 +138,9 @@ public Optional<Pair<Pose3d, Double>> getEstimatedVisionPose3d(Pose2d previousPo
    */
   public Optional<Pair<Pose3d, Double>> getEstimatedVisionPose3d() {
     ArrayList<EstimatedRobotPose> estimates = new ArrayList<>();
-    for (int i = 0; i < photonPoseEstimators.size(); i++) {
-      PhotonPoseEstimator estimator = photonPoseEstimators.get(i);
-      if (estimator != null && cameraEnabled[i] && CAMERAS[i].isConnected()) {
+    for (int i = 0; i < poseEstimators.size(); i++) {
+      PhotonPoseEstimator estimator = poseEstimators.get(i);
+      if (estimator != null && camerasEnabled[i] && CAMERAS[i].isConnected()) {
         var estimate = estimator.update();
         if (estimate.isPresent()) {
           estimates.add(estimate.get());
@@ -243,34 +244,34 @@ private Optional<Pair<Pose3d, Double>> averageEstimates(ArrayList<EstimatedRobot
           exponentialSmooth(
               lastSmoothedPose.getX(),
               averagePose.getX(),
-              PhotonvisionSubsystemConstants.POSITION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.POSITION_ALPHA);
       double smoothedY =
           exponentialSmooth(
               lastSmoothedPose.getY(),
               averagePose.getY(),
-              PhotonvisionSubsystemConstants.POSITION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.POSITION_ALPHA);
       double smoothedZ =
           exponentialSmooth(
               lastSmoothedPose.getZ(),
               averagePose.getZ(),
-              PhotonvisionSubsystemConstants.POSITION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.POSITION_ALPHA);
 
       // Smooth rotation
       double smoothedRotX =
           exponentialSmooth(
               lastSmoothedPose.getRotation().getX(),
               averagePose.getRotation().getX(),
-              PhotonvisionSubsystemConstants.ROTATION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.ROTATION_ALPHA);
       double smoothedRotY =
           exponentialSmooth(
               lastSmoothedPose.getRotation().getY(),
               averagePose.getRotation().getY(),
-              PhotonvisionSubsystemConstants.ROTATION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.ROTATION_ALPHA);
       double smoothedRotZ =
           exponentialSmooth(
               lastSmoothedPose.getRotation().getZ(),
               averagePose.getRotation().getZ(),
-              PhotonvisionSubsystemConstants.ROTATION_ALPHA);
+              PhotonvisionAprilTagSubsystemConstants.ROTATION_ALPHA);
 
       lastSmoothedPose =
           new Pose3d(
@@ -376,11 +377,11 @@ private double exponentialSmooth(double oldValue, double newValue, double alpha)
    */
   public List<Pair<Integer, Pair<Pose3d, Double>>> getPerCameraEstimatedPoses() {
     List<Pair<Integer, Pair<Pose3d, Double>>> perCameraPoses = new ArrayList<>();
-    for (int i = 0; i < photonPoseEstimators.size(); i++) {
-      PhotonPoseEstimator estimator = photonPoseEstimators.get(i);
+    for (int i = 0; i < poseEstimators.size(); i++) {
+      PhotonPoseEstimator estimator = poseEstimators.get(i);
       PhotonCamera camera = CAMERAS[i];
 
-      if (estimator != null && cameraEnabled[i] && camera != null && camera.isConnected()) {
+      if (estimator != null && camerasEnabled[i] && camera != null && camera.isConnected()) {
         // Get the latest result directly from the camera first
         var result = camera.getLatestResult();
         if (result.hasTargets()) {

src/main/java/frc/alotobots/library/vision/photonvision/PhotonvisionSubsystemConstants.java → src/main/java/frc/alotobots/library/vision/photonvision/apriltag/PhotonvisionAprilTagSubsystemConstants.java

@@ -1,4 +1,4 @@
-package frc.alotobots.library.vision.photonvision;
+package frc.alotobots.library.vision.photonvision.apriltag;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
@@ -15,9 +15,9 @@
 
 @SuppressWarnings("resource")
 @UtilityClass
-public class PhotonvisionSubsystemConstants {
+public class PhotonvisionAprilTagSubsystemConstants {
 
-  public static final AprilTagFieldLayout aprilTagFieldLayout =
+  public static final AprilTagFieldLayout fieldLayout =
       AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
 
   public static final double FIELD_LENGTH = 16.54175; // in meters