my 13th reason

src/main/java/frc/robot/RobotContainer.java

@@ -1,15 +1,11 @@
 package frc.robot;
 
-import com.ctre.phoenix6.swerve.SwerveRequest;
-
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.algaeflywheel.AlgaeRoller;
 import frc.robot.algaepivot.AlgaeSubsystem;
 import frc.robot.auton.AutonMaster;
@@ -19,13 +15,22 @@
 import frc.robot.elevator.ElevatorSubsystem;
 
 import static edu.wpi.first.units.Units.*;
+import static frc.robot.vision.VisionConfig.fieldLayout;
+
+import org.photonvision.PhotonCamera;
+
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import frc.robot.operator.OperatorXbox;
-import frc.robot.vision.PoseEstimatorSubsystem;
-import frc.robot.vision.VisionConfig;
+// import frc.robot.vision.PoseEstimatorSubsystem;
+// import frc.robot.vision.VisionConfig;
 
 import frc.robot.rushinator.*;
 import frc.robot.rushinator.commands.*;
+import frc.robot.vision.PhotonVisionLocalizer;
+import frc.robot.vision.VisionConfig.PhotonConfig;
+import frc.robot.vision.VisionLocalizationSystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -39,6 +44,8 @@ public class RobotContainer {
 
     public static boolean modeFast = true;
 
+    private final VisionLocalizationSystem m_vision = new VisionLocalizationSystem();
+
     /* Setting up bindings for necessary control of the swerve drive platform */
     public static SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
         .withDeadband(kMaxVelocity * 0.1)
@@ -56,10 +63,24 @@ public RobotContainer() {
         autonTab.add(mAutonChooser);
         SmartDashboard.putData(mAutonChooser);
 
-        var mPoseEstimator = PoseEstimatorSubsystem.getInstance();
+        // var mPoseEstimator = PoseEstimatorSubsystem.getInstance();
 
         // ShuffleboardTab visionTab = Shuffleboard.getTab("Vision Pose Estimator");
         // PoseEstimatorSubsystem.getInstance().addDashboardWidgets(visionTab);
+
+        for (PhotonConfig config : PhotonConfig.values()){
+
+            var cam = new PhotonCamera(config.name);
+            m_vision.addCamera(new PhotonVisionLocalizer(
+                cam, 
+                config.offset,
+                config.multiTagPoseStrategy,
+                config.singleTagPoseStrategy,
+                () -> CommandSwerveDrivetrain.getInstance().getPose().getRotation(),
+                fieldLayout,
+                config.defaultSingleTagStdDevs, 
+                config.defaultMultiTagStdDevs));
+        }
     }
 
         /**
@@ -144,5 +165,5 @@ public void setDefaultCommands() {
 
 //        CoralSubsystem.getInstance().setDefaultCommand(new CoralSubsystem.TuningCommand(() -> (driver.getRightX() + 1) / 2.0f));
     }
-}
+}  
 

src/main/java/frc/robot/driver/DriverXbox.java

@@ -1,7 +1,7 @@
 package frc.robot.driver;
 
-import static frc.robot.vision.VisionConfig.AlignmentConfig.Cblue;
-import static frc.robot.vision.VisionConfig.AlignmentConfig.Dblue;
+// import static frc.robot.vision.VisionConfig.AlignmentConfig.Cblue;
+// import static frc.robot.vision.VisionConfig.AlignmentConfig.Dblue;
 
 import edu.wpi.first.math.estimator.PoseEstimator;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -22,7 +22,7 @@
 import frc.robot.climber.commands.SetClimberAngle;
 import frc.robot.commands.RobotCommands;
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
-import frc.robot.drivetrain.commands.DriveToPoseCommand;
+// import frc.robot.drivetrain.commands.DriveToPoseCommand;
 import frc.robot.elevator.ElevatorSubsystem;
 import frc.robot.elevator.commands.ElevatorCommands;
 import frc.robot.elevator.commands.SetElevatorState;
@@ -34,10 +34,10 @@
 import frc.robot.rushinator.commands.SetWristState;
 import frc.robot.rushinator.commands.ToggleWristState;
 import frc.robot.rushinator.commands.SetRollersVoltage;
-import frc.robot.vision.PoseEstimatorSubsystem;
-import frc.robot.vision.VisionConfig;
-import frc.robot.vision.commands.AutoAlign;
-import frc.robot.vision.commands.LineupCommand;
+// import frc.robot.vision.PoseEstimatorSubsystem;
+// import frc.robot.vision.VisionConfig;
+// import frc.robot.vision.commands.AutoAlign;
+// import frc.robot.vision.commands.LineupCommand;
 
 
 public class DriverXbox extends XboxGamepad {

src/main/java/frc/robot/drivetrain/CommandSwerveDrivetrain.java

@@ -35,7 +35,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.RobotContainer;
 import frc.robot.drivetrain.TunerConstants.TunerSwerveDrivetrain;
-import frc.robot.vision.PoseEstimatorSubsystem;
+// import frc.robot.vision.PoseEstimatorSubsystem;
 
 /**
  * Class that extends the Phoenix 6 SwerveDrivetrain class and implements

src/main/java/frc/robot/drivetrain/commands/DriveAndHoldAngle.java

@@ -17,7 +17,7 @@
 import frc.robot.RobotContainer;
 import frc.robot.driver.DriverXbox;
 import frc.robot.drivetrain.CommandSwerveDrivetrain;
-import frc.robot.vision.PoseEstimatorSubsystem;
+// import frc.robot.vision.PoseEstimatorSubsystem;
 
 public class DriveAndHoldAngle extends Command{
     private static class Settings {