Attempt to implement advantagekit for CTRE swerve

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

@@ -205,13 +205,13 @@ public class Robot extends LoggedRobot {
   // }
 
 
-  @Override
+  // @Override
-  public void simulationPeriodic() {
+  // public void simulationPeriodic() {
-    m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
+  //   m_robotContainer.m_robotSwerveDrive.updateSim(RobotController.getBatteryVoltage());
-    // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
+  //   // visionSim.update(m_robotContainer.m_robotSwerveDrive.getPose2d());
 
-    // m_robotContainer.m_robotSwerveDrive.
+  //   // m_robotContainer.m_robotSwerveDrive.
-  }
+  // }
 
 
 

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

@@ -48,6 +48,7 @@ import frc4388.robot.constants.Constants;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.constants.Constants.LiDARConstants;
 import frc4388.robot.constants.Constants.OIConstants;
+import frc4388.robot.constants.Constants.SimConstants.Mode;
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -56,8 +57,8 @@ import frc4388.robot.subsystems.Elevator;
 // Subsystems
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
-// import frc4388.robot.subsystems.Endeffector;
+import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 // Utilites
 import frc4388.utility.DeferredBlock;
@@ -74,7 +75,7 @@ import frc4388.utility.compute.ReefPositionHelper.Side;
 public class RobotContainer {
     /* RobotMap */
     
-    public final RobotMap m_robotMap = new RobotMap();
+    public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
     
     /* Subsystems */
     public final LED m_robotLED = new LED();
@@ -83,6 +84,9 @@ public class RobotContainer {
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
+    public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
+    public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
+
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -129,7 +133,7 @@ public class RobotContainer {
 
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new ParallelRaceGroup(
             new WaitCommand(1),
@@ -292,7 +296,7 @@ public class RobotContainer {
 
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new ParallelRaceGroup(
             new WaitCommand(1),
@@ -339,7 +343,7 @@ public class RobotContainer {
 
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L4_DISTANCE_SCORE, Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         // waitDebuger.asProxy(),
         // new ParallelRaceGroup(
         //     new WaitCommand(1),
@@ -380,7 +384,7 @@ public class RobotContainer {
 
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE-Units.inchesToMeters(1), Side.LEFT, true),
         waitDebuger.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -409,7 +413,7 @@ public class RobotContainer {
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L3_DISTANCE_SCORE, Side.RIGHT, true),
         waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
 
         // new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator),
@@ -429,7 +433,7 @@ public class RobotContainer {
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.LEFT, true),
         waitDebuger.asProxy(),
         
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -450,7 +454,7 @@ public class RobotContainer {
         new DriveToReef(m_robotSwerveDrive, m_vision, AutoConstants.L2_SCORE_DISTANCE, Side.RIGHT, true),
         waitDebuger.asProxy(),
 
-        new LidarAlign(m_robotSwerveDrive, m_robotMap.reefLidar),
+        new LidarAlign(m_robotSwerveDrive, reefLidar),
         waitDebuger.asProxy(),
         new InstantCommand(() -> {m_robotElevator.transitionState(CoordinationState.L2Score);}, m_robotElevator),
         new waitEndefectorRefrence(m_robotElevator),
@@ -558,7 +562,7 @@ public class RobotContainer {
         NamedCommands.registerCommand("await-coral", new waitFeedCoral(m_robotElevator));
 
         NamedCommands.registerCommand("feed-driveback", new DriveUntilLiDAR(m_robotSwerveDrive, 
-        new Translation2d(-1,0), new Translation2d(), m_robotMap.reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
+        new Translation2d(-1,0), new Translation2d(), reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE));
         // NamedCommands.registerCommand("feed-driveback", Commands.none());
         NamedCommands.registerCommand("stop", new waitSupplier(() -> m_robotSwerveDrive.isStopped()));
 
@@ -722,7 +726,7 @@ public class RobotContainer {
             .onTrue(thrustIntake.asProxy());
           
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, m_robotMap.reefLidar)); 
+            .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive, reefLidar)); 
         
             
         // ?  /* Operator Buttons */

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

@@ -19,14 +19,18 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.LiDARConstants;
+import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
-import frc4388.robot.constants.DriveConstants;
 import frc4388.robot.subsystems.lidar.LiDAR;
 import frc4388.robot.subsystems.lidar.LidarIO;
 import frc4388.robot.subsystems.lidar.LidarLiteV2;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveIO;
+import frc4388.robot.subsystems.swerve.SwervePhoenix;
 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;
 
@@ -38,24 +42,20 @@ public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
 
-    public final VisionIO leftCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME), VisionConstants.LEFT_CAMERA_POS);
+    public final VisionIO leftCamera;
-    public final VisionIO rightCamera = new VisionPhotonvision(new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME), VisionConstants.RIGHT_CAMERA_POS);
+    public final VisionIO rightCamera;
 
     // public final LiDAR lidar = new 
 
-    public final LiDAR reefLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL), "Reef");
+    public final LidarIO reefLidar;
-    public final LiDAR reverseLidar = new LiDAR((LidarIO) new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL), "Reverse");
+    public final LidarIO reverseLidar;
 
 
     /* LED Subsystem */
     // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
     
     /* Swreve Drive Subsystem */
-    public final SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrain = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
+    public final SwerveIO swerveDrivetrain;
-        DriveConstants.DrivetrainConstants, 
-        DriveConstants.FRONT_LEFT, DriveConstants.FRONT_RIGHT,
-        DriveConstants.BACK_LEFT, DriveConstants.BACK_RIGHT
-    );
 
     /* Elevator Subsystem */
     public final TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
@@ -66,71 +66,101 @@ public class RobotMap {
     public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
     public final DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
 
-    void configureDriveMotorControllers() {
+    public RobotMap(SimConstants.Mode mode) {
-        // endeffector.saf
+        switch (mode) {
+            case REAL:
+                // Configure cameras
+                PhotonCamera leftCameraReal = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME);
+                PhotonCamera rightCameraReal = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME);
+
+                leftCamera =  new VisionPhotonvision(leftCameraReal, VisionConstants.LEFT_CAMERA_POS);                ;
+                rightCamera = new VisionPhotonvision(rightCameraReal, VisionConstants.RIGHT_CAMERA_POS);
+
+                FaultPhotonCamera.addDevice(leftCameraReal, "Left Camera");
+                FaultPhotonCamera.addDevice(rightCameraReal , "Right Camera");
+
+                // Configure LiDAR
+                reefLidar = new LidarLiteV2(LiDARConstants.REEF_LIDAR_DIO_CHANNEL);
+                reverseLidar = new LidarLiteV2(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
+
+                // Configure swerve drive train
+
+                SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new, 
+                    SwerveDriveConstants.DrivetrainConstants, 
+                    SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
+                    SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
+                );
+
+                swerveDrivetrain = new SwervePhoenix(swerveDrivetrainReal);
+
+
+
+                FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
+
+                FaultTalonFX.addDevice(elevator, "Elevator");
+                FaultTalonFX.addDevice(endeffector, "Endeffector");
+                
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
+                FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
+                FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
+
+                break;
+            // case SIM:
+            //     break;
+            default:
+                leftCamera = new VisionIO() {};
+                rightCamera = new VisionIO() {};
+                reefLidar = new LidarIO() {};
+                reverseLidar = new LidarIO() {};
+                swerveDrivetrain = new SwerveIO() {};
+                break;
+        }
     }
 
+    // public class RobotMapSim {
+    //     public PhotonCameraSim leftCamera;
+    //     public PhotonCameraSim rightCamera;
+    // }
 
-    public RobotMap() {
+    // public RobotMapSim configureSim() {
-        configureDriveMotorControllers();
+    //     RobotMapSim sim = new RobotMapSim();
 
-        // FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
+    //     // The simulated camera properties
-        // FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
+    //     SimCameraProperties cameraProp = new SimCameraProperties();
+    //     // A 640 x 480 camera with a 100 degree diagonal FOV.
+    //     cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
+    //     // Approximate detection noise with average and standard deviation error in pixels.
+    //     cameraProp.setCalibError(0.25, 0.08);
+    //     // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+    //     cameraProp.setFPS(20);
+    //     // The average and standard deviation in milliseconds of image data latency.
+    //     cameraProp.setAvgLatencyMs(35);
+    //     cameraProp.setLatencyStdDevMs(5);
 
-        FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro");
+    //     // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
-
+    //     // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
-        FaultTalonFX.addDevice(elevator, "Elevator");
-        FaultTalonFX.addDevice(endeffector, "Endeffector");
-        
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getDriveMotor(), "Module 0 Drive");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getSteerMotor(), "Module 0 Steer");
-        FaultCANCoder.addDevice(swerveDrivetrain.getModule(0).getEncoder(), "Module 0 CANCoder");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getDriveMotor(), "Module 1 Drive");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getSteerMotor(), "Module 1 Steer");
-        FaultCANCoder.addDevice(swerveDrivetrain.getModule(1).getEncoder(), "Module 1 CANCoder");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getDriveMotor(), "Module 2 Drive");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getSteerMotor(), "Module 2 Steer");
-        FaultCANCoder.addDevice(swerveDrivetrain.getModule(2).getEncoder(), "Module 2 CANCoder");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getDriveMotor(), "Module 3 Drive");
-        FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getSteerMotor(), "Module 3 Steer");
-        FaultCANCoder.addDevice(swerveDrivetrain.getModule(3).getEncoder(), "Module 3 CANCoder");
-    }
-
-    public class RobotMapSim {
-        public PhotonCameraSim leftCamera;
-        public PhotonCameraSim rightCamera;
-    }
-
-    public RobotMapSim configureSim() {
-        RobotMapSim sim = new RobotMapSim();
-
-        // The simulated camera properties
-        SimCameraProperties cameraProp = new SimCameraProperties();
-        // A 640 x 480 camera with a 100 degree diagonal FOV.
-        cameraProp.setCalibration(640, 480, Rotation2d.fromDegrees(100));
-        // Approximate detection noise with average and standard deviation error in pixels.
-        cameraProp.setCalibError(0.25, 0.08);
-        // Set the camera image capture framerate (Note: this is limited by robot loop rate).
-        cameraProp.setFPS(20);
-        // The average and standard deviation in milliseconds of image data latency.
-        cameraProp.setAvgLatencyMs(35);
-        cameraProp.setLatencyStdDevMs(5);
-
-        // sim.leftCamera = new PhotonCameraSim(leftCamera, cameraProp);
-        // sim.rightCamera = new PhotonCameraSim(rightCamera, cameraProp);
 
         
-        sim.leftCamera.enableRawStream(true);
+    //     sim.leftCamera.enableRawStream(true);
-        sim.leftCamera.enableProcessedStream(true);
+    //     sim.leftCamera.enableProcessedStream(true);
-        sim.leftCamera.enableDrawWireframe(true);
+    //     sim.leftCamera.enableDrawWireframe(true);
 
 
-        sim.rightCamera.enableRawStream(true);
+    //     sim.rightCamera.enableRawStream(true);
-        sim.rightCamera.enableProcessedStream(true);
+    //     sim.rightCamera.enableProcessedStream(true);
-        sim.rightCamera.enableDrawWireframe(true);
+    //     sim.rightCamera.enableDrawWireframe(true);
 
-        return sim;
+    //     return sim;
 
-    }
+    // }
    
 }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -4,7 +4,7 @@ import java.time.Instant;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 // Command to repeat a joystick movement for a specific time.
 public class MoveForTimeCommand extends Command {

src/main/java/frc4388/robot/commands/MoveUntilSuply.java

@@ -4,7 +4,7 @@ import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 // Command to repeat a joystick movement for a specific time.
 public class MoveUntilSuply extends Command {

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

@@ -6,7 +6,7 @@ import edu.wpi.first.math.util.Units;
 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.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.ReefPositionHelper;
 import frc4388.utility.compute.TimesNegativeOne;

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

@@ -2,8 +2,8 @@ package frc4388.robot.commands.alignment;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 // Command to repeat a joystick movement for a specific time.
 public class DriveUntilLiDAR extends Command {

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

@@ -8,8 +8,8 @@ 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.SwerveDrive;
 import frc4388.robot.subsystems.lidar.LiDAR;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 /* 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 {

src/main/java/frc4388/robot/commands/swerve/RotateToAngle.java

@@ -6,7 +6,7 @@ package frc4388.robot.commands.swerve;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 
 public class RotateToAngle extends PID {
 

src/main/java/frc4388/robot/commands/swerve/neoJoystickPlayback.java

@@ -6,7 +6,7 @@ import java.util.function.Supplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.utility.compute.DataUtils;
 import frc4388.utility.controller.VirtualController;
 import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;

src/main/java/frc4388/robot/commands/swerve/neoJoystickRecorder.java

@@ -7,7 +7,7 @@ import java.util.function.Supplier;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.utility.compute.DataUtils;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;

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 = 171;
+  public static final int GIT_REVISION = 172;
-  public static final String GIT_SHA = "3130f647c83cc82b45a5299e19108f9eec45e6f6";
+  public static final String GIT_SHA = "8e34bfe35427e1fb86ab2af21dd706d387bf4874";
-  public static final String GIT_DATE = "2025-07-15 11:07:01 MDT";
+  public static final String GIT_DATE = "2025-07-15 13:42:25 MDT";
   public static final String GIT_BRANCH = "advantagekit";
-  public static final String BUILD_DATE = "2025-07-15 13:40:35 MDT";
+  public static final String BUILD_DATE = "2025-07-16 16:20:33 MDT";
-  public static final long BUILD_UNIX_TIME = 1752608435113L;
+  public static final long BUILD_UNIX_TIME = 1752704433011L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -13,7 +13,7 @@ import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.constants.DriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
 // import frc4388.utility.RobotGyro;
 import frc4388.utility.compute.RobotTime;
 import frc4388.utility.status.Status;
@@ -56,7 +56,7 @@ public class DiffDrive extends SubsystemBase implements Queryable {
 
   @Override
   public void periodic() {
-    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+    if (m_robotTime.m_frameNumber % SwerveDriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
       updateSmartDashboard();
     }
   }

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

@@ -1,5 +1,7 @@
 package frc4388.robot.subsystems.lidar;
 
+import org.littletonrobotics.junction.Logger;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.LiDARConstants;
 import frc4388.utility.status.Status;
@@ -23,6 +25,7 @@ public class LiDAR extends SubsystemBase implements Queryable {
     @Override
     public void periodic() {
         io.updateInputs(state);
+        Logger.processInputs("LiDAR/"+name, state);
     }
 
     // @AutoLogOutput(key = "Lidar/{name}")

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

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.swerve;
 
 import java.util.Optional;
 
@@ -22,7 +22,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 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.swerve.SwerveIO.SwerveState;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.TimesNegativeOne;
 import frc4388.utility.status.Status;
@@ -36,18 +36,22 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.RobotConfig;
 
 public class SwerveDrive extends SubsystemBase implements Queryable {
-    private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+    // private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+    private SwerveIO io;
+    private SwerveStateAutoLogged state;
+
     private Vision vision;
 
-    // @AutoLog
+
-    // public class SwerveDriveState {
+
-    public int gear_index = DriveConstants.STARTING_GEAR;
+    public int gear_index = SwerveDriveConstants.STARTING_GEAR;
     public boolean stopped = false;
     public boolean robotKnowsWhereItIs = false;
 
-    public double speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * DriveConstants.GEARS[gear_index];
+    public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
-    public double rotSpeedAdjust = DriveConstants.MAX_ROT_SPEED;
+    public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
-    public double autoSpeedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
+    public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to
                                                                                             // 25%
 
     public double lastOdomSpeed;
@@ -58,17 +62,16 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public double rotTarget = 0.0;
     public Rotation2d orientRotTarget = new Rotation2d();
     public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
-    // }
-
-    // public SwerveDriveState state = new SwerveDriveState();
 
     /** Creates a new SwerveDrive. */
-    public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+    public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
         // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
         // swerveDriveTrain) {
         FaultReporter.register(this);
 
-        this.swerveDriveTrain = swerveDriveTrain;
+        this.io = swerveDriveTrain;
+        this.state = new SwerveStateAutoLogged();
+
         this.vision = vision;
 
         RobotConfig config;
@@ -81,12 +84,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
-                    return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d);
+                    return getPose2d();
                 }, // Robot pose supplier
                 this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting
                                              // pose)
-                () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
+                () -> state.speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
-                (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds()
+                (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
                         .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
                                               // Also optionally outputs individual module feedforwards
                 new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
@@ -139,7 +142,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void setOdoPose(Pose2d pose) {
         if (pose == null) return;
         initalPose2d = pose;
-        swerveDriveTrain.resetPose(pose);
+        io.resetPose(pose);
     }
 
     // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -172,12 +175,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             rightStick = TimesNegativeOne.invert(rightStick, TimesNegativeOne.RotAxis);    
 
             // ! drift correction
-            if (rightStick.getNorm() > 0.05 || !DriveConstants.DRIFT_CORRECTION_ENABLED) {
+            if (rightStick.getNorm() > 0.05 || !SwerveDriveConstants.DRIFT_CORRECTION_ENABLED) {
-                rotTarget = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()).getRotation().getDegrees();
+
-                swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+                rotTarget = state.currentPose.getRotation().getDegrees();
+
+                io.setControl(new SwerveRequest.FieldCentric()
                     .withVelocityX(leftStick.getX() * speedAdjust)
                     .withVelocityY(leftStick.getY() * speedAdjust)
                     .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+
                     // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective));
                 SmartDashboard.putBoolean("drift correction", false);
             } else {
@@ -186,17 +192,17 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                     .withVelocityY(leftStick.getY() * speedAdjust)
                     .withTargetDirection(Rotation2d.fromDegrees(rotTarget));
                 ctrl.HeadingController.setPID(
-                    DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kP,
-                    DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kI,
-                    DriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
+                    SwerveDriveConstants.PIDConstants.DRIFT_CORRECTION_GAINS.kD
                 );
-                swerveDriveTrain.setControl(ctrl);
+                io.setControl(ctrl);
                 SmartDashboard.putBoolean("drift correction", true);
             }
 
            
         } else { // Create robot-relative speeds.
-            swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+            io.setControl(new SwerveRequest.RobotCentric()
                     .withVelocityX(leftStick.getX() * speedAdjust)
                     .withVelocityY(-leftStick.getY() * speedAdjust)
                     .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
@@ -207,9 +213,9 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         stopped = false;
         // Create robot-relative speeds.
         if (rightStick.getNorm() > 0.1) rightStick = rightStick.times(0);
-        swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+        io.setControl(new SwerveRequest.RobotCentric()
-            .withVelocityX(leftStick.getX() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
-            .withVelocityY(-leftStick.getY() * DriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
             .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
         
     }
@@ -229,7 +235,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rightStick.getAngle()));
@@ -243,11 +249,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             .withVelocityY(leftStick.getY() * speedAdjust)
             .withTargetDirection(heading);
         ctrl.HeadingController.setPID(
-            DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP,
-            DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
-            DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
+            SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
         );
-        swerveDriveTrain.setControl(ctrl);
+        io.setControl(ctrl);
     }
 
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
@@ -262,31 +268,19 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI,
         //     DriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD
         // );
-        swerveDriveTrain.setControl(ctrl);
+        io.setControl(ctrl);
-    }
-
-    public void setLimits(double limitInAmps) {
-        for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
-            var talonFXConfigurator = module.getDriveMotor().getConfigurator();
-            var talonFXConfigs = new TalonFXConfiguration();
-
-            talonFXConfigurator.refresh(talonFXConfigs);
-            talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
-            talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
-            talonFXConfigurator.apply(talonFXConfigs);
-        }
     }
 
     public void activateLuigiMode() {
-        setLimits(20);
+        io.setLimits(20);
     }
 
     public void deactivateLuigiMode() {
-        setLimits(DriveConstants.Configurations.SLIP_CURRENT);
+        io.setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT);
     }
 
     public boolean rotateToTarget(double angle) {
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(0)
                 .withVelocityY(0)
                 .withTargetDirection(Rotation2d.fromDegrees(angle)));
@@ -312,7 +306,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         leftStick = leftStick.rotateBy(TimesNegativeOne.ForwardOffset);
 
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+        io.setControl(new SwerveRequest.FieldCentricFacingAngle()
                 .withVelocityX(leftStick.getX() * -speedAdjust)
                 .withVelocityY(leftStick.getY() * speedAdjust)
                 .withTargetDirection(rot));
@@ -324,11 +318,13 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public Pose2d getPose2d() {
-        return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d);
+        if(state.currentPose == null)
+            return initalPose2d;
+        return state.currentPose;
     }
 
     public void resetGyro() {
-        swerveDriveTrain.tareEverything();
+        io.tareEverything();
         robotKnowsWhereItIs = false;
         rotTarget = 0;
         // vision.resetRotations();
@@ -337,7 +333,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public void softStop() {
         stopped = true;
-        swerveDriveTrain.setControl(new SwerveRequest.FieldCentric()
+        io.setControl(new SwerveRequest.FieldCentric()
             .withVelocityX(0)
             .withVelocityY(0)
             .withRotationalRate(0)
@@ -356,14 +352,10 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI);
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
-        double time = Vision.getTime();
+        io.updateInputs(state);
-        double freq =  swerveDriveTrain.getOdometryFrequency();
         
-        Optional<Pose2d> curpose = swerveDriveTrain.samplePoseAt(time);
+        vision.setLastOdomPose(state.currentPose);
-        Optional<Pose2d> lastPose = swerveDriveTrain.samplePoseAt(time - freq);
+        setLastOdomSpeed(state.currentPose, state.lastPose, state.frequency);
-        
-        vision.setLastOdomPose(curpose);
-        setLastOdomSpeed(curpose, lastPose, freq);
 
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
@@ -373,66 +365,66 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                 rotTarget += pose.getRotation().getDegrees() - curPose.getRotation().getDegrees();
             }
 
-            vision.addVisionMeasurement(swerveDriveTrain);
+            io.addVisionMeasurement(vision.getPosesToAdd());
         }
 
         // if(e.isPresent())
     }
 
     private void reset_index() {
-        gear_index = DriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
+        gear_index = SwerveDriveConstants.STARTING_GEAR; // however we wish to initialize the gear (What gear does the
                                                          // robot start in?)
     }
 
     public void shiftDown() {
-        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
+        if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
         int i = gear_index - 1;
         if (i == -1)
             i = 0;
-        setPercentOutput(DriveConstants.GEARS[i]);
+        setPercentOutput(SwerveDriveConstants.GEARS[i]);
         gear_index = i;
     }
 
     public void shiftUp() {
-        if (gear_index == -1 || gear_index >= DriveConstants.GEARS.length)
+        if (gear_index == -1 || gear_index >= SwerveDriveConstants.GEARS.length)
             reset_index(); // If outof bounds: reset index
         int i = gear_index + 1;
-        if (i == DriveConstants.GEARS.length)
+        if (i == SwerveDriveConstants.GEARS.length)
-            i = DriveConstants.GEARS.length - 1;
+            i = SwerveDriveConstants.GEARS.length - 1;
-        setPercentOutput(DriveConstants.GEARS[i]);
+        setPercentOutput(SwerveDriveConstants.GEARS[i]);
         gear_index = i;
     }
 
     public void setPercentOutput(double speed) {
-        speedAdjust = DriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
+        speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
         gear_index = -1;
     }
 
     public void setToSlow() {
-        setPercentOutput(DriveConstants.SLOW_SPEED);
+        setPercentOutput(SwerveDriveConstants.SLOW_SPEED);
         gear_index = 0;
     }
 
     public void setToFast() {
-        setPercentOutput(DriveConstants.FAST_SPEED);
+        setPercentOutput(SwerveDriveConstants.FAST_SPEED);
         gear_index = 1;
     }
 
     public void setToTurbo() {
-        setPercentOutput(DriveConstants.TURBO_SPEED);
+        setPercentOutput(SwerveDriveConstants.TURBO_SPEED);
         gear_index = 2;
     }
 
     public void shiftUpRot() {
-        rotSpeedAdjust = DriveConstants.ROTATION_SPEED;
+        rotSpeedAdjust = SwerveDriveConstants.ROTATION_SPEED;
     }
 
     public void shiftDownRot() {
-        rotSpeedAdjust = DriveConstants.MIN_ROT_SPEED;
+        rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
     }
 
-    private int tmp_gear_index = DriveConstants.STARTING_GEAR;
+    private int tmp_gear_index = SwerveDriveConstants.STARTING_GEAR;
 
     public void startSlowPeriod() {
         tmp_gear_index = gear_index;
@@ -445,15 +437,15 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     }
 
     public void endSlowPeriod() {
-        setPercentOutput(DriveConstants.GEARS[tmp_gear_index]);
+        setPercentOutput(SwerveDriveConstants.GEARS[tmp_gear_index]);
         gear_index = tmp_gear_index;
     }
 
 
 
-    public void setLastOdomSpeed(Optional<Pose2d> curPose, Optional<Pose2d> lastPose, double freq){
+    public void setLastOdomSpeed(Pose2d curPose, Pose2d lastPose, double freq){
-        if(curPose.isPresent() && lastPose.isPresent()){
+        if(curPose != null && lastPose != null){
-            lastOdomSpeed = curPose.get().getTranslation().getDistance(lastPose.get().getTranslation())/freq;
+            lastOdomSpeed = curPose.getTranslation().getDistance(lastPose.getTranslation())/freq;
         }
     }
     
@@ -473,11 +465,5 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         return status;
     }
-
-
-    // Update CTRE simulation, if used.
-    public void updateSim(double voltage) {
-        swerveDriveTrain.updateSimState(0.02, voltage);
-    }
 }
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -1,4 +1,4 @@
-package frc4388.robot.constants;
+package frc4388.robot.subsystems.swerve;
 
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Rotations;
@@ -28,7 +28,7 @@ import frc4388.utility.structs.Gains;
 
 // No mans land
 // Beware, there be dragons.
-public final class DriveConstants {
+public final class SwerveDriveConstants {
     public static final double MAX_ROT_SPEED        = Math.PI * 2;
     public static final double AUTO_MAX_ROT_SPEED = 1.5;
     public static final double MIN_ROT_SPEED        = 1.0;

src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java

@@ -0,0 +1,33 @@
+package frc4388.robot.subsystems.swerve;
+
+import java.util.List;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+
+public interface SwerveIO {
+    @AutoLog
+    public class SwerveState {
+        public Pose2d currentPose = null;
+        public Pose2d lastPose = null;
+        public ChassisSpeeds speeds = null;
+        public double frequency = 1;
+    }
+
+    public default void setControl(SwerveRequest ctrl) {}
+
+    public default void setLimits(double limitInAmps) {}
+
+    public default void tareEverything() {}
+
+    public default void resetPose(Pose2d pose) {}
+
+    public default void addVisionMeasurement(List<PoseObservation> poses) {}
+
+    public default void updateInputs(SwerveState state) {}
+}

src/main/java/frc4388/robot/subsystems/swerve/SwervePhoenix.java

@@ -0,0 +1,65 @@
+package frc4388.robot.subsystems.swerve;
+
+import java.util.List;
+
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain;
+import com.ctre.phoenix6.swerve.SwerveModule;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc4388.robot.subsystems.lidar.LidarIO.LidarState;
+import frc4388.robot.subsystems.vision.Vision;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+
+public class SwervePhoenix implements SwerveIO {
+    SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+    public SwervePhoenix(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+        this.swerveDriveTrain = swerveDriveTrain;
+        swerveDriveTrain.getOdometryFrequency();
+    }
+
+    @Override
+    public void updateInputs(SwerveState state) {
+        double time = Vision.getTime();
+        state.frequency = swerveDriveTrain.getOdometryFrequency();
+        state.currentPose = swerveDriveTrain.samplePoseAt(time).orElse(null);
+        state.lastPose = swerveDriveTrain.samplePoseAt(time - state.frequency).orElse(null);
+        state.speeds = swerveDriveTrain.getState().Speeds;
+    }
+
+    @Override
+    public void setControl(SwerveRequest ctrl) {
+        swerveDriveTrain.setControl(ctrl);
+    }
+
+    @Override
+    public void tareEverything() {
+        swerveDriveTrain.tareEverything();
+    }
+
+    @Override
+    public void setLimits(double limitInAmps) {
+        for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {
+            var talonFXConfigurator = module.getDriveMotor().getConfigurator();
+            var talonFXConfigs = new TalonFXConfiguration();
+
+            talonFXConfigurator.refresh(talonFXConfigs);
+            talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps;
+            talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10;
+            talonFXConfigurator.apply(talonFXConfigs);
+        }
+    }
+
+    @Override
+    public void addVisionMeasurement(List<PoseObservation> poses) {
+        for(PoseObservation pose : poses) {
+            swerveDriveTrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
+        }
+    }
+
+}

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

@@ -1,5 +1,7 @@
 package frc4388.robot.subsystems.vision;
 
+import java.util.ArrayList;
+import java.util.List;
 import java.util.Optional;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -43,21 +45,20 @@ public class Vision extends SubsystemBase implements Queryable {
         }
     }
 
-    public void addVisionMeasurement(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> drivetrain){
+    public List<PoseObservation> getPosesToAdd(){
-        // for(EstimatedRobotPose pose : poses){
+        List<PoseObservation> poses = new ArrayList<>();
-        //     
-        // }
         for(int i = 0; i < state.length; i++) {
             if(state[i].lastEstimatedPose != null) {
-                PoseObservation pose = state[i].lastEstimatedPose;
+                poses.add(state[i].lastEstimatedPose);
-                drivetrain.addVisionMeasurement(pose.pose(), Utils.fpgaToCurrentTime(pose.timestamp()));
             }
         }
+
+        return poses;
     }
 
-    public void setLastOdomPose(Optional<Pose2d> pose){
+    public void setLastOdomPose(Pose2d pose){
-        if(pose.isPresent())
+        if(pose != null)
-            lastPhysOdomPose = pose.get();
+            lastPhysOdomPose = pose;
     }
 
     public boolean isTag(){

src/main/java/frc4388/utility/compute/TimesNegativeOne.java

@@ -7,16 +7,16 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc4388.robot.constants.DriveConstants;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
 
 // Class that holds weather the drivers sticks should be inverted
 public class TimesNegativeOne {
 
-    public static boolean XAxis = DriveConstants.INVERT_X;
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
-    public static boolean YAxis = DriveConstants.INVERT_Y;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
-    public static boolean RotAxis = DriveConstants.INVERT_ROTATION;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
     public static boolean isRed = false;
-    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(DriveConstants.FORWARD_OFFSET);
+    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
 
     private static boolean isRed() {
         Optional<Alliance> alliance = DriverStation.getAlliance();
@@ -26,10 +26,10 @@ public class TimesNegativeOne {
 
     public static void update(){
         isRed = isRed();
-        XAxis = DriveConstants.INVERT_X ^ isRed;
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
-        YAxis = DriveConstants.INVERT_Y ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
-        RotAxis = DriveConstants.INVERT_ROTATION;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
-        ForwardOffset = Rotation2d.fromDegrees((DriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
+        ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
         SmartDashboard.putBoolean("Is red alliance", isRed);
     }