Work on adding advantagekit

src/main/java/frc4388/robot/Robot.java

@@ -15,9 +15,7 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
-import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.robot.constants.BuildConstants;

src/main/java/frc4388/robot/RobotContainer.java

@@ -55,11 +55,10 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 import frc4388.robot.subsystems.Elevator;
 // Subsystems
 import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
 // import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
-
+import frc4388.robot.subsystems.vision.Vision;
 // Utilites
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.compute.TimesNegativeOne;
@@ -695,8 +694,8 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
         
         new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
-            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust *= 2;}))
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust *= 2;}))
-            .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.state.rotSpeedAdjust /= 2;}));
+            .onFalse(new InstantCommand(() -> {m_robotSwerveDrive.rotSpeedAdjust /= 2;}));
 
         new Trigger(() ->getDeadbandedDriverController().getRightTriggerAxis() > 0.8)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.startTurboPeriod()))

src/main/java/frc4388/robot/RobotMap.java

@@ -21,9 +21,12 @@ import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.LiDARConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
 import frc4388.robot.constants.DriveConstants;
-import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.lidar.LidarIO;
+import frc4388.robot.subsystems.lidar.LidarLiteV2;
+import frc4388.robot.subsystems.vision.VisionIO;
+import frc4388.robot.subsystems.vision.VisionPhotonvision;
 import frc4388.utility.status.FaultCANCoder;
-import frc4388.utility.status.FaultPhotonCamera;
 import frc4388.utility.status.FaultPidgeon2;
 import frc4388.utility.status.FaultTalonFX;
 
@@ -35,11 +38,13 @@ public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public final PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+    public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS);
-    public final PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+    public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS);
 
-    public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
+    // public final LiDAR lidar = new 
-    public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
+
+    public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef");
+    public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse");
 
 
     /* LED Subsystem */
@@ -69,8 +74,8 @@ public class RobotMap {
     public RobotMap() {
         configureDriveMotorControllers();
 
-        FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
+        // FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
-        FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
+        // FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
 
         FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro");
 
@@ -111,8 +116,8 @@ public class RobotMap {
         cameraProp.setAvgLatencyMs(35);
         cameraProp.setLatencyStdDevMs(5);
 
-        sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
+        // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
-        sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
+        // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
 
         
         sim.leftCamera.enableRawStream(true);

src/main/java/frc4388/robot/commands/alignment/DriveToReef.java

@@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.subsystems.Vision;
+import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.ReefPositionHelper;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.compute.ReefPositionHelper.Side;
@@ -131,7 +131,7 @@ public class DriveToReef extends Command {
             (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
             (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
             (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
-            (!waitVelocityZero || swerveDrive.state.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
+            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
         );
         // System.out.println(finished);
 

src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java

@@ -2,22 +2,22 @@ package frc4388.robot.commands.alignment;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.lidar.LiDAR;
 
 // Command to repeat a joystick movement for a specific time.
 public class DriveUntilLiDAR extends Command {
     private final SwerveDrive swerveDrive;
     private final Translation2d leftStick;
     private final Translation2d rightStick;
-    private final Lidar m_lidar;
+    private final LiDAR m_lidar;
     private final double mindistance;
 
     public DriveUntilLiDAR(
         SwerveDrive swerveDrive, 
         Translation2d leftStick, 
         Translation2d rightStick, 
-        Lidar lidar,
+        LiDAR lidar,
         double mindistance) {
             addRequirements(swerveDrive);
 

src/main/java/frc4388/robot/commands/alignment/LidarAlign.java

@@ -8,13 +8,13 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.lidar.LiDAR;
 
 /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class LidarAlign extends Command {
   private SwerveDrive swerveDrive;
-  private Lidar lidar;  
+  private LiDAR lidar;  
 
   private int currentFinderTick;
   // private int tickFoundPipe;
@@ -26,7 +26,7 @@ public class LidarAlign extends Command {
   // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
-  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
+  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
     // Use addRequirements() here to declare subsystem dependencies.
 
     this.swerveDrive = swerveDrive;

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2025RidgeScape";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 170;
+  public static final int GIT_REVISION = 171;
-  public static final String GIT_SHA = "2a38f94d5eef00a093f47df192f7c5c8a2b8cf8d";
+  public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6";
-  public static final String GIT_DATE = "2025-07-15 10:24:11 MDT";
+  public static final String GIT_DATE = "2025-07-15 11:07:01 MDT";
   public static final String GIT_BRANCH = "advantagekit";
-  public static final String BUILD_DATE = "2025-07-15 11:04:29 MDT";
+  public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT";
-  public static final long BUILD_UNIX_TIME = 1752599069523L;
+  public static final long BUILD_UNIX_TIME = 1752608435113L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/Elevator.java

@@ -4,8 +4,6 @@
 
 package frc4388.robot.subsystems;
 
-import org.littletonrobotics.junction.AutoLog;
-
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;

src/main/java/frc4388/robot/subsystems/LED.java

@@ -11,7 +11,6 @@ import org.littletonrobotics.junction.AutoLogOutput;
 
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.LEDConstants;
 import frc4388.utility.status.Status;

src/main/java/frc4388/robot/subsystems/Lidar.java

@@ -1,71 +0,0 @@
-package frc4388.robot.subsystems;
-
-import org.littletonrobotics.junction.AutoLogOutput;
-
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.Counter;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.constants.Constants.LiDARConstants;
-import frc4388.utility.status.Status;
-import frc4388.utility.status.FaultReporter;
-import frc4388.utility.status.Queryable;
-import frc4388.utility.status.Status.ReportLevel;
-
-// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
-public class Lidar extends SubsystemBase implements Queryable {
-
-    private Counter LidarPWM;
-    private String name = "Lidar";
-
-    private double distance = -1;
-    public Lidar(int port, String name) {
-        FaultReporter.register(this);
-
-        this.name = name;
-        LidarPWM = new Counter(port);
-        LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
-        LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
-        LidarPWM.reset();
-
-    }
-
-    @Override
-    public void periodic() {
-        if(LidarPWM.get() < 1)
-            distance = -1;
-        else
-            distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
-    }
-
-    @AutoLogOutput(key = "Lidar/{name}")
-    public double getDistance(){
-        return distance;
-    }
-
-    public boolean withinDistance(){
-        if(distance == -1) return false;
-        return distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
-    }
-
-    @Override
-    public String getName() {
-        return "Lidar " + name;
-    }
-
-    @Override
-    public Status diagnosticStatus() {
-        Status s = new Status();
-
-        if(distance == -1)
-            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
-        
-        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
-
-        return s;
-    }
-    
-}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -6,7 +6,6 @@ package frc4388.robot.subsystems;
 
 import java.util.Optional;
 
-import org.littletonrobotics.junction.AutoLog;
 import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.Utils;
@@ -20,15 +19,11 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.DriveConstants;
+import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.status.Status;
 import frc4388.utility.status.FaultReporter;
@@ -44,28 +39,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
     private Vision vision;
 
-    @AutoLog
+    // @AutoLog
-    public class SwerveDriveState {
+    // public class SwerveDriveState {
-        public int gear_index = DriveConstants.STARTING_GEAR;
+    public int gear_index = DriveConstants.STARTING_GEAR;
-        public boolean stopped = false;
+    public boolean stopped = false;
-        public boolean robotKnowsWhereItIs = false;
+    public boolean robotKnowsWhereItIs = false;
 
-        public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
+    public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
-        public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
+    public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
-        public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
+    public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
-                                                                                               // 25%
+                                                                                            // 25%
 
-        public double lastOdomSpeed;
+    public double lastOdomSpeed;
 
-        public Pose2d initalPose2d = null;
+    public Pose2d initalPose2d = null;
 
 
-        public double rotTarget = 0.0;
+    public double rotTarget = 0.0;
-        public Rotation2d orientRotTarget = new Rotation2d();
+    public Rotation2d orientRotTarget = new Rotation2d();
-        public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+    public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
-    }
+    // }
 
-    public SwerveDriveState state = new SwerveDriveState();
+    // public SwerveDriveState state = new SwerveDriveState();
 
     /** Creates a new SwerveDrive. */
     public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
@@ -86,7 +81,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
-                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(state.initalPose2d);
+                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
                 }, // Robot pose supplier
                 this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
                                              // pose)
@@ -135,7 +130,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         //             null,
         //             null,
         //             null,
-        //             (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
+        //             (state) -> Logger.recordOutput("Drive/SysIdState", toString())),
         //         new SysIdRoutine.Mechanism(
         //             (voltage) -> runCharacterization(voltage.in(Volts)), null, this));
 
@@ -143,7 +138,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public void setOdoPose(Pose2d pose) {
         if (pose == null) return;
-        state.initalPose2d = pose;
+        initalPose2d = pose;
         swerveDriveTrain.resetPose(pose);
     }
 
@@ -162,7 +157,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     // }
 
     public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
-        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve drive is still going:
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
             stopModules(); // stop the swerve
 
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05) // if no imput
@@ -170,7 +165,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         
-        state.stopped = false;
+        stopped = false;
         if (fieldRelative) {
             
             leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
@@ -178,18 +173,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
             // ! drift correction
             if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
-                state.rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
+                rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
                 swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
-                    .withVelocityX(leftStick.getX() * state.speedAdjust)
+                    .withVelocityX(leftStick.getX() * speedAdjust)
-                    .withVelocityY(leftStick.getY() * state.speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
-                    .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
                     // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
                 SmartDashboard.putBoolean("drift correction", false);
             } else {
                 var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-                    .withVelocityX(leftStick.getX() * state.speedAdjust)
+                    .withVelocityX(leftStick.getX() * speedAdjust)
-                    .withVelocityY(leftStick.getY() * state.speedAdjust)
+                    .withVelocityY(leftStick.getY() * speedAdjust)
-                    .withTargetDirection(Rotation2d.fromDegrees(state.rotTarget));
+                    .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
                 ctrl.HeadingController.setPID(
                     DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
                     DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
@@ -202,20 +197,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
            
         } else { // Create robot-relative speeds.
             swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
-                    .withVelocityX(leftStick.getX() * state.speedAdjust)
+                    .withVelocityX(leftStick.getX() * speedAdjust)
-                    .withVelocityY(-leftStick.getY() * state.speedAdjust)
+                    .withVelocityY(-leftStick.getY() * speedAdjust)
-                    .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
+                    .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
         }
     }
 
     public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
-        state.stopped = false;
+        stopped = false;
         // Create robot-relative speeds.
         if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
         swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
             .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
             .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
-            .withRotationalRate(rightStick.getX() * state.rotSpeedAdjust));
+            .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
         
     }
 
@@ -225,7 +220,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                                                                                                // relitive version of
                                                                                                // this, and no pre
                                                                                                // provided version
-        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && state.stopped == false) // if no imput and the swerve
+        if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve
                                                                                            // drive is still going:
             stopModules(); // stop the swerve
 
@@ -235,8 +230,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-                .withVelocityX(leftStick.getX() * state.speedAdjust)
+                .withVelocityX(leftStick.getX() * speedAdjust)
-                .withVelocityY(leftStick.getY() * state.speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rightStick.getAngle()));
     }
 
@@ -244,8 +239,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
         leftStick = TimesNegativeOne.invert(leftStick, TimesNegativeOne.XAxis, TimesNegativeOne.YAxis);
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-            .withVelocityX(leftStick.getX() * state.speedAdjust)
+            .withVelocityX(leftStick.getX() * speedAdjust)
-            .withVelocityY(leftStick.getY() * state.speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
         ctrl.HeadingController.setPID(
             DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
@@ -259,8 +254,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(heading);
 
         var ctrl = new SwerveRequest.FieldCentricFacingAngle()
-            .withVelocityX(leftStick.getX() * state.speedAdjust)
+            .withVelocityX(leftStick.getX() * speedAdjust)
-            .withVelocityY(leftStick.getY() * state.speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
         // ctrl.HeadingController.setPID(
         //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
@@ -304,7 +299,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public boolean isStopped() {
-        return state.lastOdomSpeed < AutoConstants.STOP_VELOCITY;
+        return lastOdomSpeed < AutoConstants.STOP_VELOCITY;
     }
 
     public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
@@ -318,8 +313,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
-                .withVelocityX(leftStick.getX() * -state.speedAdjust)
+                .withVelocityX(leftStick.getX() * -speedAdjust)
-                .withVelocityY(leftStick.getY() * state.speedAdjust)
+                .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rot));
         // double
     }
@@ -329,19 +324,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public Pose2d getPose2d() {
-        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(state.initalPose2d);
+        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
     }
 
     public void resetGyro() {
         swerveDriveTrain.tareEverything();
-        state.robotKnowsWhereItIs = false;
+        robotKnowsWhereItIs = false;
-        state.rotTarget = 0;
+        rotTarget = 0;
         // vision.resetRotations();
     }
 
 
     public void softStop() {
-        state.stopped = true;
+        stopped = true;
         swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
             .withVelocityX(0)
             .withVelocityY(0)
@@ -359,7 +354,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void periodic() {
         // This method will be called once per scheduler run\
         SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
-        SmartDashboard.putNumber("RotTartget", state.rotTarget);
+        SmartDashboard.putNumber("RotTartget", rotTarget);
 
         double time = Vision.getTime();
         double freq =  swerveDriveTrain.getOdometryFrequency();
@@ -370,86 +365,95 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         vision.setLastOdomPose(curpose);
         setLastOdomSpeed(curpose, lastPose, freq);
 
-        // if (vision.isTag()) {5
+        if (vision.isTag()) {
+            Pose2d pose = vision.getPose2d();
+            if (!robotKnowsWhereItIs) {
+                robotKnowsWhereItIs = true;
+                Pose2d curPose = getPose2d();
+                rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
+            }
+
+            vision.addVisionMeasurement(swerveDriveTrain);
+        }
 
         // if(e.isPresent())
     }
 
     private void reset_index() {
-        state.gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
+        gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
                                                          // robot start in?)
     }
 
     public void shiftDown() {
-        if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
+        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
-        int i = state.gear_index - 1;
+        int i = gear_index - 1;
         if (i == -1)
             i = 0;
         setPercentOutput(DriveConstants.GEARS[i]);
-        state.gear_index = i;
+        gear_index = i;
     }
 
     public void shiftUp() {
-        if (state.gear_index == -1 || state.gear_index >= DriveConstants.GEARS.length)
+        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
-        int i = state.gear_index + 1;
+        int i = gear_index + 1;
         if (i == DriveConstants.GEARS.length)
             i = DriveConstants.GEARS.length - 1;
         setPercentOutput(DriveConstants.GEARS[i]);
-        state.gear_index = i;
+        gear_index = i;
     }
 
     public void setPercentOutput(double speed) {
-        state.speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+        speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
-        state.gear_index = -1;
+        gear_index = -1;
     }
 
     public void setToSlow() {
         setPercentOutput(DriveConstants.SLOW_SPEED);
-        state.gear_index = 0;
+        gear_index = 0;
     }
 
     public void setToFast() {
         setPercentOutput(DriveConstants.FAST_SPEED);
-        state.gear_index = 1;
+        gear_index = 1;
     }
 
     public void setToTurbo() {
         setPercentOutput(DriveConstants.TURBO_SPEED);
-        state.gear_index = 2;
+        gear_index = 2;
     }
 
     public void shiftUpRot() {
-        state.rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
+        rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
     }
 
     public void shiftDownRot() {
-        state.rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
+        rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
     }
 
     private int tmp_gear_index = DriveConstants.STARTING_GEAR;
 
     public void startSlowPeriod() {
-        tmp_gear_index = state.gear_index;
+        tmp_gear_index = gear_index;
         setToSlow();
     }
 
     public void startTurboPeriod() {
-        tmp_gear_index = state.gear_index;
+        tmp_gear_index = gear_index;
         setToTurbo();
     }
 
     public void endSlowPeriod() {
         setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
-        state.gear_index = tmp_gear_index;
+        gear_index = tmp_gear_index;
     }
 
 
 
     public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
         if(curPose.isPresent() && lastPose.isPresent()){
-            state.lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+            lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
         }
     }
     

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -1,281 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.numbers.N1;
-import edu.wpi.first.math.numbers.N3;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Optional;
-
-import org.littletonrobotics.junction.AutoLog;
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.targeting.PhotonPipelineResult;
-import com.ctre.phoenix6.Utils;
-import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.swerve.SwerveDrivetrain;
-
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.constants.Constants.FieldConstants;
-import frc4388.robot.constants.Constants.VisionConstants;
-import frc4388.utility.status.Status;
-import frc4388.utility.status.FaultReporter;
-import frc4388.utility.status.Queryable;
-
-public class Vision extends SubsystemBase implements Queryable {
-    private PhotonCamera[] cameras;
-    private PhotonPoseEstimator[] estimators;
-
-    @AutoLog
-    public class VisionState {
-        public boolean isTagDetected = false;
-        public boolean isTagProcessed = false;
-        public List<EstimatedRobotPose> poses = new ArrayList<>();
-        public double latency = 0;
-        public Pose2d lastVisionPose = new Pose2d();
-        public Pose2d lastPhysOdomPose = new Pose2d();
-    }
-
-    private VisionState state = new VisionState();
-
-    private Field2d field = new Field2d();
-    public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
-        FaultReporter.register(this);
-        SmartDashboard.putData(field);
-
-        this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
-
-        PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS);
-        PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS);
-
-        photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
-        photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
-
-        this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight};
-    }
-
-    @Override
-    public void periodic() {
-        update();
-        field.setRobotPose(getPose2d());
-    }
-
-    // private Instant lastVisionTime = null;
-
-
-    private void update() {
-        state.isTagProcessed = false;
-        state.isTagDetected = false;
-
-        // Instant now = Instant.now();
-
-        // int cams = 0;
-
-        double latency = 0;
-
-        // Pose2d pose = null;
-        state.poses.clear();
-
-        for(int i = 0; i < cameras.length; i++){
-            PhotonCamera camera = cameras[i];
-            PhotonPoseEstimator estimator = estimators[i];
-
-            var results = camera.getAllUnreadResults();
-
-            // If there are no more updates from the camera
-            if (results.size() == 0) 
-                continue;
-
-            
-            var result = results.get(results.size()-1);
-            latency += result.getTimestampSeconds();
-
-            state.isTagDetected = state.isTagDetected | result.hasTargets();
-
-            // If there are no tags
-            if(!result.hasTargets())
-                continue;
-
-            Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
-
-            // If the tag was failed to be processed
-            if(estimatedRobotPose.isEmpty())
-                continue;
-            
-            state.poses.add(estimatedRobotPose.get());
-
-            // if(pose == null)
-            //     pose = estimatedRobotPose.get().estimatedPose.toPose2d();
-            // else
-            //     pose = pose.interpolate(pose, 0.5);
-            // X += pose.getX();
-            // Y += pose.getY();
-
-            // if(X > 6)
-
-            // Yaw += (pose.getRotation().getDegrees() + 180) % 360;
-            // cams++;
-
-            state.isTagProcessed = true;
-        
-            
-        }
-    }
-
-
-    /**
-     * The latest estimated robot pose on the field from vision data. This may be empty. This should
-     * only be called once per loop.
-     *
-     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
-     * {@link getEstimationStdDevs}
-     *
-     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
-     *     used for estimation.
-     */
-    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
-        Optional<EstimatedRobotPose> visionEst = Optional.empty();
-
-        var targets = change.getTargets();
-        for(int i = targets.size()-1; i >= 0; i--){
-            Transform3d pos = targets.get(i).getBestCameraToTarget();
-            double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
-            if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
-                change.targets.remove(i);
-            }
-        }
-
-        if(targets.size() <= 0)
-            return visionEst; // Will be empty
-
-        visionEst = estimator.update(change);
-        // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
-
-        return visionEst;
-    }
-
-
-    // /**
-    //  * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
-    //  * deviations based on number of tags, estimation strategy, and distance from the tags.
-    //  *
-    //  * @param estimatedPose The estimated pose to guess standard deviations for.
-    //  * @param targets All targets in this camera frame
-    //  */
-    // private void updateEstimationStdDevs(
-    //         Optional<EstimatedRobotPose> estimatedPose, 
-    //         List<PhotonTrackedTarget> targets,
-    //         PhotonPoseEstimator estimator) {
-    //     if (estimatedPose.isEmpty()) {
-    //         // No pose input. Default to single-tag std devs
-    //         curStdDevs = VisionConstants.kSingleTagStdDevs;
-
-    //     } else {
-    //         // Pose present. Start running Heuristic
-    //         var estStdDevs = VisionConstants.kSingleTagStdDevs;
-    //         int numTags = 0;
-    //         double avgDist = 0;
-
-    //         // Precalculation - see how many tags we found, and calculate an average-distance metric
-    //         for (var tgt : targets) {
-    //             var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
-    //             if (tagPose.isEmpty()) continue;
-                
-    //             double distance = tagPose
-    //             .get()
-    //             .toPose2d()
-    //             .getTranslation()
-    //             .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
-                
-    //             if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
-    //                 numTags++;
-    //                 avgDist += distance;
-    //             }
-    //         }
-
-    //         if (numTags == 0) {
-    //             // No tags visible. Default to single-tag std devs
-    //             curStdDevs = VisionConstants.kSingleTagStdDevs;
-    //         } else {
-    //             // One or more tags visible, run the full heuristic.
-    //             avgDist /= numTags;
-    //             // Decrease std devs if multiple targets are visible
-    //             if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
-    //             // Increase std devs based on (average) distance
-    //             if (numTags == 1 && avgDist > 4)
-    //                 estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
-    //             else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
-    //             curStdDevs = estStdDevs;
-    //         }
-    //     }
-    // }
-
-
-
-
-
-    public void setLastOdomPose(Optional<Pose2d> pose){
-        if(pose.isPresent())
-            state.lastPhysOdomPose = pose.get();
-    }
-
-    // public double getLastOdomSpeed(){
-    //     return lastOdomSpeed;
-    // }
-
-    public Pose2d getPose2d() {
-        if(state.lastPhysOdomPose != null)
-            return state.lastPhysOdomPose;
-
-        // if(lastVisionPose != null)
-        //     return lastVisionPose;
-        return new Pose2d();
-
-    }
-
-    public static double getTime() {
-        return Utils.getCurrentTimeSeconds();
-    }
-
-    public boolean isTag(){
-        return state.isTagDetected && state.isTagProcessed;
-    }
-
-
-    public void addVisionMeasurement( SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
-        for(EstimatedRobotPose pose : state.poses){
-            drivetrain.addVisionMeasurement(pose.estimatedPose.toPose2d(), Utils.fpgaToCurrentTime(pose.timestampSeconds));
-        }
-    }
-
-
-
-
-
-
-
-
-    @Override
-    public String getName() {
-        return "Vision";
-    }
-
-    @Override
-    public Status diagnosticStatus() {
-        return new Status();
-    }
-    
-}

src/main/java/frc4388/robot/subsystems/lidar/LiDAR.java

@@ -0,0 +1,55 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.utility.status.Status;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status.ReportLevel;
+
+public class LiDAR extends SubsystemBase implements Queryable {
+    LidarIO io;
+    LidarStateAutoLogged state = new LidarStateAutoLogged();
+
+    private String name = "Lidar";
+
+    public LiDAR(LidarIO device, String name) {
+        FaultReporter.register(this);
+
+        this.io = device;
+        this.name = name;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+    }
+
+    // @AutoLogOutput(key = "Lidar/{name}")
+    public double getDistance(){
+        return state.distance;
+    }
+
+    public boolean withinDistance(){
+        if(state.distance == -1) return false;
+        return state.distance < LiDARConstants.LIDAR_DETECT_DISTANCE;
+    }
+
+    @Override
+    public String getName() {
+        return "Lidar " + name;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status s = new Status();
+
+        if(state.distance == -1)
+            s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
+        
+        s.addReport(ReportLevel.INFO, "LiDAR Distance: " + state.distance);
+
+        return s;
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarIO.java

@@ -0,0 +1,13 @@
+package frc4388.robot.subsystems.lidar;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LidarIO {
+    @AutoLog
+    public class LidarState {
+        public boolean connected;
+        public double distance;
+    }
+    
+    public default void updateInputs(LidarState state) {}
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarLiteV2.java

@@ -0,0 +1,27 @@
+package frc4388.robot.subsystems.lidar;
+
+import edu.wpi.first.wpilibj.Counter;
+import frc4388.robot.constants.Constants.LiDARConstants;
+
+// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
+public class LidarLiteV2 implements LidarIO {
+
+    
+    private Counter LidarPWM;
+
+    public LidarLiteV2(int port) {
+        LidarPWM = new Counter(port);
+        LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
+        LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement
+        LidarPWM.reset();
+    }
+
+    @Override
+    public void updateInputs(LidarState state) {
+        
+        if(LidarPWM.get() < 1)
+            state.distance = -1;
+        else
+            state.distance = (LidarPWM.getPeriod() * LiDARConstants.SECONDS_TO_MICROS) / LiDARConstants.LIDAR_MICROS_TO_CM;
+    }
+}

src/main/java/frc4388/robot/subsystems/vision/Vision.java

@@ -0,0 +1,94 @@
+package frc4388.robot.subsystems.vision;
+
+import java.util.Optional;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.FaultReporter;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status;
+
+public class Vision extends SubsystemBase implements Queryable {
+    VisionIO[] io;
+    VisionStateAutoLogged[] state;
+
+    
+    public Pose2d lastVisionPose = new Pose2d();
+    public Pose2d lastPhysOdomPose = new Pose2d();
+
+    public Vision(VisionIO... devices) {
+        FaultReporter.register(this);
+        io = devices;
+        state = new VisionStateAutoLogged[io.length];
+
+        for(int i = 0; i < io.length; i++) {
+            state[i] = new VisionStateAutoLogged();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < io.length; i++) {
+            io[i].updateInputs(state[i]);
+            Logger.processInputs("Vision/Camera" + i , state[i]);
+        }
+    }
+
+    public void addVisionMeasurement(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
+        // for(EstimatedRobotPose pose : poses){
+        //     
+        // }
+        for(int i = 0; i < state.length; i++) {
+            if(state[i].lastEstimatedPose != null) {
+                PoseObservation pose = state[i].lastEstimatedPose;
+                drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
+            }
+        }
+    }
+
+    public void setLastOdomPose(Optional<Pose2d> pose){
+        if(pose.isPresent())
+            lastPhysOdomPose = pose.get();
+    }
+
+    public boolean isTag(){
+        for(int i = 0; i < state.length; i++){
+            if(state[i].isTagDetected && state[i].isTagProcessed)
+                return true;
+        }
+        return false;
+    }
+
+    @AutoLogOutput
+    public Pose2d getPose2d() {
+        if(lastPhysOdomPose != null)
+            return lastPhysOdomPose;
+
+        // if(lastVisionPose != null)
+        //     return lastVisionPose;
+        return new Pose2d();
+
+    }
+
+    public static double getTime() {
+        return Utils.getCurrentTimeSeconds();
+    }
+
+
+    @Override
+    public Status diagnosticStatus() {
+        return new Status();
+        // // TODO Auto-generated method stub
+        // throw new UnsupportedOperationException("Unimplemented method 'diagnosticStatus'");
+    }
+    
+}

src/main/java/frc4388/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,22 @@
+package frc4388.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public interface VisionIO {
+    @AutoLog
+    public class VisionState {
+        public boolean isTagDetected = false;
+        public boolean isTagProcessed = false;
+        // public double latency = 0;
+        public PoseObservation lastEstimatedPose = null;
+    }
+
+    public static record PoseObservation(
+        Pose2d pose,
+        double timestamp
+    ) {}
+
+    public default void updateInputs(VisionState state) {}
+}

src/main/java/frc4388/robot/subsystems/vision/VisionPhotonvision.java

@@ -0,0 +1,158 @@
+package frc4388.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import frc4388.robot.constants.Constants.FieldConstants;
+import frc4388.robot.constants.Constants.VisionConstants;
+
+public class VisionPhotonvision implements VisionIO {
+    // private PhotonCamera[] cameras;
+    // private PhotonPoseEstimator[] estimators;
+    
+    private PhotonCamera camera;
+    private PhotonPoseEstimator estimator;
+
+    // public List<EstimatedRobotPose> poses = new ArrayList<>();
+
+    
+    public VisionPhotonvision(PhotonCamera camera, Transform3d position){
+        this.camera = camera;
+        estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
+        estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+    }
+
+    // private Instant lastVisionTime = null;
+
+
+    public void updateInputs(VisionState state) {
+        state.isTagProcessed = false;
+        state.isTagDetected = false;
+
+        state.lastEstimatedPose = null;
+
+        var results = camera.getAllUnreadResults();
+
+        // If there are no more updates from the camera
+        if (results.size() == 0) 
+            return;
+
+        
+        var result = results.get(results.size()-1);
+
+        state.isTagDetected = state.isTagDetected | result.hasTargets();
+
+        // If there are no tags
+        if(!result.hasTargets())
+            return;
+
+        Optional<EstimatedRobotPose> estimatedRobotPose = getEstimatedGlobalPose(result, estimator);
+
+        // If the tag was failed to be processed
+        if(estimatedRobotPose.isEmpty())
+            return;
+        
+        EstimatedRobotPose pose = estimatedRobotPose.get();
+
+        state.lastEstimatedPose = new PoseObservation(pose.estimatedPose.toPose2d(), pose.timestampSeconds);
+
+        state.isTagProcessed = true;
+    }
+
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should
+     * only be called once per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link getEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+     *     used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+
+        var targets = change.getTargets();
+        for(int i = targets.size()-1; i >= 0; i--){
+            Transform3d pos = targets.get(i).getBestCameraToTarget();
+            double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2));
+            if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) {
+                change.targets.remove(i);
+            }
+        }
+
+        if(targets.size() <= 0)
+            return visionEst; // Will be empty
+
+        visionEst = estimator.update(change);
+        // updateEstimationStdDevs(visionEst, change.getTargets(), estimator);
+
+        return visionEst;
+    }
+
+    public String getName() {
+        return camera.getName();
+    }
+
+
+    // /**
+    //  * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+    //  * deviations based on number of tags, estimation strategy, and distance from the tags.
+    //  *
+    //  * @param estimatedPose The estimated pose to guess standard deviations for.
+    //  * @param targets All targets in this camera frame
+    //  */
+    // private void updateEstimationStdDevs(
+    //         Optional<EstimatedRobotPose> estimatedPose, 
+    //         List<PhotonTrackedTarget> targets,
+    //         PhotonPoseEstimator estimator) {
+    //     if (estimatedPose.isEmpty()) {
+    //         // No pose input. Default to single-tag std devs
+    //         curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+    //     } else {
+    //         // Pose present. Start running Heuristic
+    //         var estStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         int numTags = 0;
+    //         double avgDist = 0;
+
+    //         // Precalculation - see how many tags we found, and calculate an average-distance metric
+    //         for (var tgt : targets) {
+    //             var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId());
+    //             if (tagPose.isEmpty()) continue;
+                
+    //             double distance = tagPose
+    //             .get()
+    //             .toPose2d()
+    //             .getTranslation()
+    //             .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+                
+    //             if (distance < VisionConstants.MIN_ESTIMATION_DISTANCE) {
+    //                 numTags++;
+    //                 avgDist += distance;
+    //             }
+    //         }
+
+    //         if (numTags == 0) {
+    //             // No tags visible. Default to single-tag std devs
+    //             curStdDevs = VisionConstants.kSingleTagStdDevs;
+    //         } else {
+    //             // One or more tags visible, run the full heuristic.
+    //             avgDist /= numTags;
+    //             // Decrease std devs if multiple targets are visible
+    //             if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+    //             // Increase std devs based on (average) distance
+    //             if (numTags == 1 && avgDist > 4)
+    //                 estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    //             else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+    //             curStdDevs = estStdDevs;
+    //         }
+    //     }
+    // }
+}

src/main/java/frc4388/utility/status/Alerts.java

@@ -6,9 +6,5 @@ import frc4388.robot.RobotContainer;
 
 // Class to update a series of WPILIB Alerts
 public class Alerts {
-  private static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
+  public static Alert no_auto = new Alert("No auto has been selected!", AlertType.kError);
-
-  public static void UpdateAlerts(RobotContainer m_RobotContainer) {
-    no_auto.set(!m_RobotContainer.autoChooserUpdated);
-  }
 }