From b6f34bc22a194febf80c375e6c80537fea70f3bd Mon Sep 17 00:00:00 2001
From: Alex Resnick <adr8292@gmail.com>
Date: Sat, 27 Jan 2024 14:59:29 -0600
Subject: [PATCH] Add Advantage kit to Swerve and Update to 2024 Phoenix 6
 (#205)

---
 .gitignore                                    |   2 +
 build.gradle                                  |  35 ++
 src/main/java/frc/lib/math/Conversions.java   |  96 ++----
 .../java/frc/lib/util/ctre/CTREConfigs.java   |  65 ----
 .../frc/lib/util/ctre/CTREModuleState.java    |  61 ----
 .../frc/lib/util/swerve/SwerveModule.java     | 213 +++++-------
 .../util/swerve/SwerveModuleConstants.java    |  27 --
 .../frc/lib/util/swerve/SwerveModuleIO.java   |  28 ++
 .../frc/lib/util/swerve/SwerveModuleReal.java | 139 ++++++++
 src/main/java/frc/robot/Constants.java        |  44 ++-
 src/main/java/frc/robot/Robot.java            | 119 ++++++-
 src/main/java/frc/robot/RobotContainer.java   |  30 +-
 src/main/java/frc/robot/autos/PPExample.java  |   2 +-
 .../java/frc/robot/autos/TrajectoryBase.java  |   2 +-
 .../robot/commands/drive/ClimbPlatform.java   |   2 +-
 .../robot/commands/drive/MoveToEngage.java    |   2 +-
 .../frc/robot/commands/drive/MoveToPos.java   |   2 +-
 .../frc/robot/commands/drive/MoveToScore.java |   2 +-
 .../robot/commands/drive/TeleopSwerve.java    |   4 +-
 .../frc/robot/commands/drive/TurnToAngle.java |   2 +-
 .../robot/commands/test/TestTransform.java    |   2 +-
 .../java/frc/robot/subsystems/Swerve.java     | 305 ------------------
 .../frc/robot/subsystems/WristIntake.java     |  24 +-
 .../frc/robot/subsystems/swerve/Swerve.java   | 276 ++++++++++++++++
 .../frc/robot/subsystems/swerve/SwerveIO.java |  39 +++
 .../robot/subsystems/swerve/SwerveReal.java   |  42 +++
 vendordeps/AdvantageKit.json                  |  42 +++
 vendordeps/Phoenix5.json                      | 151 ---------
 28 files changed, 890 insertions(+), 868 deletions(-)
 delete mode 100644 src/main/java/frc/lib/util/ctre/CTREConfigs.java
 delete mode 100644 src/main/java/frc/lib/util/ctre/CTREModuleState.java
 delete mode 100644 src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java
 create mode 100644 src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
 create mode 100644 src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
 delete mode 100644 src/main/java/frc/robot/subsystems/Swerve.java
 create mode 100644 src/main/java/frc/robot/subsystems/swerve/Swerve.java
 create mode 100644 src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
 create mode 100644 src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
 create mode 100644 vendordeps/AdvantageKit.json
 delete mode 100644 vendordeps/Phoenix5.json

diff --git a/.gitignore b/.gitignore
index be238f3b..6e18ae02 100644
--- a/.gitignore
+++ b/.gitignore
@@ -177,3 +177,5 @@ logs/
 
 # Folder that has CTRE Phoenix Sim device config storage
 ctre_sim/
+
+BuildConstants.java
diff --git a/build.gradle b/build.gradle
index 0527f592..d2b7ffeb 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,7 @@
 plugins {
     id "java"
     id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "com.peterabeles.gversion" version "1.10"
 }
 
 java {
@@ -14,6 +15,27 @@ repositories {
 
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
+repositories {
+    maven {
+        url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
+        credentials {
+            username = "Mechanical-Advantage-Bot"
+            password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
+        }
+    }
+    mavenLocal()
+}
+
+configurations.all {
+    exclude group: "edu.wpi.first.wpilibj"
+}
+
+task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.CheckInstall"
+    classpath = sourceSets.main.runtimeClasspath
+}
+compileJava.finalizedBy checkAkitInstall
+
 // Define my targets (RoboRIO) and artifacts (deployable files)
 // This is added by GradleRIO's backing project DeployUtils.
 deploy {
@@ -74,6 +96,8 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     annotationProcessor 'com.github.Frc5572:RobotTools:main-SNAPSHOT'
 
+    def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
+    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
 }
 
 test {
@@ -119,3 +143,14 @@ javadoc {
             "</script>"
     }
 }
+
+// Create version file
+project.compileJava.dependsOn(createVersionFile)
+gversion {
+    srcDir       = "src/main/java/"
+    classPackage = "frc.robot"
+    className    = "BuildConstants"
+    dateFormat   = "yyyy-MM-dd HH:mm:ss z"
+    timeZone     = "America/New_York"
+    indent       = "  "
+}
diff --git a/src/main/java/frc/lib/math/Conversions.java b/src/main/java/frc/lib/math/Conversions.java
index 57c8e097..6fa180ae 100644
--- a/src/main/java/frc/lib/math/Conversions.java
+++ b/src/main/java/frc/lib/math/Conversions.java
@@ -3,93 +3,48 @@
 import edu.wpi.first.math.MathUtil;
 
 /**
- * Mathematical conversions for swerve calculations
+ * Static Conversion functions
  */
 public class Conversions {
 
     /**
-     * @param counts Falcon Counts
-     * @param gearRatio Gear Ratio between Falcon and Mechanism
-     * @return Degrees of Rotation of Mechanism falconToDegrees
+     * @param wheelRPS Wheel Velocity: (in Rotations per Second)
+     * @param circumference Wheel Circumference: (in Meters)
+     * @return Wheel Velocity: (in Meters per Second)
      */
-    public static double falconToDegrees(double counts, double gearRatio) {
-        return counts * (360.0 / (gearRatio * 2048.0));
-    }
-
-    /**
-     * @param degrees Degrees of rotation of Mechanism
-     * @param gearRatio Gear Ratio between Falcon and Mechanism
-     * @return Falcon Counts degreesToFalcon
-     */
-    public static double degreesToFalcon(double degrees, double gearRatio) {
-        double ticks = degrees / (360.0 / (gearRatio * 2048.0));
-        return ticks;
-    }
-
-    /**
-     * @param velocityCounts Falcon Velocity Counts
-     * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
-     * @return RPM of Mechanism
-     */
-    public static double falconToRPM(double velocityCounts, double gearRatio) {
-        double motorRPM = velocityCounts * (600.0 / 2048.0);
-        double mechRPM = motorRPM / gearRatio;
-        return mechRPM;
-    }
-
-    /**
-     * @param rpm RPM of mechanism
-     * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
-     * @return RPM of Mechanism
-     */
-    public static double rpmToFalcon(double rpm, double gearRatio) {
-        double motorRPM = rpm * gearRatio;
-        double sensorCounts = motorRPM * (2048.0 / 600.0);
-        return sensorCounts;
-    }
-
-    /**
-     * @param counts Falcon Counts
-     * @param gearRatio Gear Ratio between Falcon and Mechanism
-     * @return Degrees of Rotation of Mechanism falconToDegrees
-     */
-    public static double falconToMeters(double counts, double gearRatio, double circumference) {
-        return counts * circumference / (gearRatio * 2048.0);
+    public static double rpsToMPS(double wheelRPS, double circumference) {
+        double wheelMPS = wheelRPS * circumference;
+        return wheelMPS;
     }
 
     /**
-     * @param velocitycounts Falcon Velocity Counts
-     * @param circumference Circumference of Wheel
-     * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
-     * @return Falcon Velocity Counts
+     * @param wheelMPS Wheel Velocity: (in Meters per Second)
+     * @param circumference Wheel Circumference: (in Meters)
+     * @return Wheel Velocity: (in Rotations per Second)
      */
-    public static double falconToMPS(double velocitycounts, double circumference,
-        double gearRatio) {
-        double wheelRPM = falconToRPM(velocitycounts, gearRatio);
-        double wheelMPS = (wheelRPM * circumference) / 60;
-        return wheelMPS;
+    public static double mpsToRPS(double wheelMPS, double circumference) {
+        double wheelRPS = wheelMPS / circumference;
+        return wheelRPS;
     }
 
     /**
-     * @param velocity Velocity MPS
-     * @param circumference Circumference of Wheel
-     * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
-     * @return Falcon Velocity Counts
+     * @param wheelRotations Wheel Position: (in Rotations)
+     * @param circumference Wheel Circumference: (in Meters)
+     * @return Wheel Distance: (in Meters)
      */
-    public static double mpsToFalcon(double velocity, double circumference, double gearRatio) {
-        double wheelRPM = ((velocity * 60) / circumference);
-        double wheelVelocity = rpmToFalcon(wheelRPM, gearRatio);
-        return wheelVelocity;
+    public static double rotationsToMeters(double wheelRotations, double circumference) {
+        double wheelMeters = wheelRotations * circumference;
+        return wheelMeters;
     }
 
     /**
-     * Normalize angle to between 0 to 360
-     *
-     * @param goal initial angle
-     * @return normalized angle
+     * @param wheelMeters Wheel Distance: (in Meters)
+     * @param circumference Wheel Circumference: (in Meters)
+     * @return Wheel Position: (in Rotations)
      */
-    public static double reduceTo0_360(double goal) {
-        return goal % 360;
+    public static double metersToRotations(double wheelMeters, double circumference) {
+        double wheelRotations = wheelMeters / circumference;
+        return wheelRotations;
     }
 
     /**
@@ -113,7 +68,6 @@ public static double applySwerveCurve(double input, double deadband) {
             processedInput = Math.copySign(processedInput, input);
         }
         return MathUtil.clamp(processedInput, -1, 1);
-
     }
 
     /**
diff --git a/src/main/java/frc/lib/util/ctre/CTREConfigs.java b/src/main/java/frc/lib/util/ctre/CTREConfigs.java
deleted file mode 100644
index c7b1b38b..00000000
--- a/src/main/java/frc/lib/util/ctre/CTREConfigs.java
+++ /dev/null
@@ -1,65 +0,0 @@
-package frc.lib.util.ctre;
-
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.sensors.AbsoluteSensorRange;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
-import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-import com.ctre.phoenix.sensors.SensorTimeBase;
-import frc.robot.Constants;
-
-/**
- * CTRE config constants
- */
-public final class CTREConfigs {
-    public TalonFXConfiguration swerveAngleFXConfig;
-    public TalonFXConfiguration swerveDriveFXConfig;
-    public CANCoderConfiguration swerveCanCoderConfig;
-
-    /**
-     * CTRE config constants
-     */
-    public CTREConfigs() {
-        swerveAngleFXConfig = new TalonFXConfiguration();
-        swerveDriveFXConfig = new TalonFXConfiguration();
-        swerveCanCoderConfig = new CANCoderConfiguration();
-
-        /* Swerve Angle Motor Configurations */
-        SupplyCurrentLimitConfiguration angleSupplyLimit =
-            new SupplyCurrentLimitConfiguration(Constants.Swerve.ANGLE_ENABLE_CURRENT_LIMIT,
-                Constants.Swerve.ANGLE_CONTINOUS_CURRENT_LIMIT,
-                Constants.Swerve.ANGLE_PEAK_CURRENT_LIMIT,
-                Constants.Swerve.ANGLE_PEAK_CURRENT_DURATION);
-
-        swerveAngleFXConfig.slot0.kP = Constants.Swerve.ANGLE_KP;
-        swerveAngleFXConfig.slot0.kI = Constants.Swerve.ANGLE_KI;
-        swerveAngleFXConfig.slot0.kD = Constants.Swerve.ANGLE_KD;
-        swerveAngleFXConfig.slot0.kF = Constants.Swerve.ANGLE_KF;
-        swerveAngleFXConfig.supplyCurrLimit = angleSupplyLimit;
-        swerveAngleFXConfig.initializationStrategy = SensorInitializationStrategy.BootToZero;
-
-        /* Swerve Drive Motor Configuration */
-        SupplyCurrentLimitConfiguration driveSupplyLimit =
-            new SupplyCurrentLimitConfiguration(Constants.Swerve.DRIVE_ENABLE_CURRENT_LIMIT,
-                Constants.Swerve.DRIVE_CONTINOUS_CURRENT_LIMIT,
-                Constants.Swerve.DRIVE_PEAK_CURRENT_LIMIT,
-                Constants.Swerve.DRIVE_PEAK_CURRENT_DURATION);
-
-        swerveDriveFXConfig.slot0.kP = Constants.Swerve.DRIVE_KP;
-        swerveDriveFXConfig.slot0.kI = Constants.Swerve.DRIVE_KI;
-        swerveDriveFXConfig.slot0.kD = Constants.Swerve.DRIVE_KD;
-        swerveDriveFXConfig.slot0.kF = Constants.Swerve.DRIVE_KF;
-        swerveDriveFXConfig.supplyCurrLimit = driveSupplyLimit;
-        swerveDriveFXConfig.initializationStrategy = SensorInitializationStrategy.BootToZero;
-        swerveDriveFXConfig.openloopRamp = Constants.Swerve.OPEN_LOOP_RAMP;
-        swerveDriveFXConfig.closedloopRamp = Constants.Swerve.CLOSED_LOOP_RAMP;
-
-        /* Swerve CANCoder Configuration */
-        swerveCanCoderConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
-        swerveCanCoderConfig.sensorDirection = Constants.Swerve.CAN_CODER_INVERT;
-        swerveCanCoderConfig.initializationStrategy =
-            SensorInitializationStrategy.BootToAbsolutePosition;
-        swerveCanCoderConfig.sensorTimeBase = SensorTimeBase.PerSecond;
-    }
-
-}
diff --git a/src/main/java/frc/lib/util/ctre/CTREModuleState.java b/src/main/java/frc/lib/util/ctre/CTREModuleState.java
deleted file mode 100644
index 9c844d97..00000000
--- a/src/main/java/frc/lib/util/ctre/CTREModuleState.java
+++ /dev/null
@@ -1,61 +0,0 @@
-package frc.lib.util.ctre;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-
-/**
- * Retrieve and set module states on CTRE devices
- */
-public class CTREModuleState {
-
-    /**
-     * Minimize the change in heading the desired swerve module state would require by potentially
-     * reversing the direction the wheel spins. Customized from WPILib's version to include placing
-     * in appropriate scope for CTRE onboard control.
-     *
-     * @param desiredState The desired state.
-     * @param currentAngle The current module angle.
-     */
-    public static SwerveModuleState optimize(SwerveModuleState desiredState,
-        Rotation2d currentAngle) {
-        double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(),
-            desiredState.angle.getDegrees());
-        double targetSpeed = desiredState.speedMetersPerSecond;
-        double delta = targetAngle - currentAngle.getDegrees();
-        if (Math.abs(delta) > 90) {
-            targetSpeed = -targetSpeed;
-            targetAngle = delta > 90 ? (targetAngle -= 180) : (targetAngle += 180);
-        }
-        return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
-    }
-
-    /**
-     * @param scopeReference Current Angle
-     * @param newAngle Target Angle
-     * @return Closest angle within scope
-     */
-    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
-        double lowerBound;
-        double upperBound;
-        double lowerOffset = scopeReference % 360;
-        if (lowerOffset >= 0) {
-            lowerBound = scopeReference - lowerOffset;
-            upperBound = scopeReference + (360 - lowerOffset);
-        } else {
-            upperBound = scopeReference - lowerOffset;
-            lowerBound = scopeReference - (360 + lowerOffset);
-        }
-        while (newAngle < lowerBound) {
-            newAngle += 360;
-        }
-        while (newAngle > upperBound) {
-            newAngle -= 360;
-        }
-        if (newAngle - scopeReference > 180) {
-            newAngle -= 360;
-        } else if (newAngle - scopeReference < -180) {
-            newAngle += 360;
-        }
-        return newAngle;
-    }
-}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModule.java b/src/main/java/frc/lib/util/swerve/SwerveModule.java
index ad96c387..3c083b5b 100644
--- a/src/main/java/frc/lib/util/swerve/SwerveModule.java
+++ b/src/main/java/frc/lib/util/swerve/SwerveModule.java
@@ -1,181 +1,142 @@
 package frc.lib.util.swerve;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-import com.ctre.phoenix.sensors.CANCoder;
+import org.littletonrobotics.junction.Logger;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import frc.lib.math.Conversions;
-import frc.lib.util.ctre.CTREModuleState;
 import frc.robot.Constants;
-import frc.robot.Robot;
 
 /**
- * Base Swerve Module Class. Creates an instance of the swerve module
+ * Swerve Module
  */
 public class SwerveModule {
     public int moduleNumber;
-    private double angleOffset;
-    private TalonFX angleMotor;
-    private TalonFX driveMotor;
-    private CANCoder angleEncoder;
-    private double lastAngle;
-    SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(Constants.Swerve.DRIVE_KS,
-        Constants.Swerve.DRIVE_KV, Constants.Swerve.DRIVE_KA);
+    private Rotation2d angleOffset;
+    // private double lastAngle;
 
-    /**
-     * Creates an instance of a Swerve Module
-     *
-     * @param moduleNumber Swerve Module ID. Must be unique
-     * @param constants Constants specific to the swerve module
-     */
-    public SwerveModule(int moduleNumber, frc.lib.util.swerve.SwerveModuleConstants constants) {
-        this.moduleNumber = moduleNumber;
-        angleOffset = constants.angleOffset;
-
-        /* Angle Encoder Config */
-        angleEncoder = new CANCoder(constants.cancoderID, "canivore");
-        configAngleEncoder();
+    private SwerveModuleIO io;
+    private SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged();
 
-        /* Angle Motor Config */
-        angleMotor = new TalonFX(constants.angleMotorID, "canivore");
-        configAngleMotor();
+    private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(
+        Constants.Swerve.DRIVE_KS, Constants.Swerve.DRIVE_KV, Constants.Swerve.DRIVE_KA);
 
-        /* Drive Motor Config */
-        driveMotor = new TalonFX(constants.driveMotorID, "canivore");
-        configDriveMotor();
+    /* drive motor control requests */
+    private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
+    private final VelocityVoltage driveVelocity = new VelocityVoltage(0);
 
-
-
-        lastAngle = getState().angle.getDegrees();
-    }
+    /* angle motor control requests */
+    private final PositionVoltage anglePosition = new PositionVoltage(0);
 
     /**
-     * Sets the desired state for the swerve module for speed and angle
+     * Swerve Module
      *
-     * @param desiredState The desired state (speed and angle)
-     * @param isOpenLoop Whether to use open or closed loop formula
+     * @param moduleNumber Module Number
+     * @param driveMotorID CAN ID of the Drive Motor
+     * @param angleMotorID CAN ID of the Angle Motor
+     * @param cancoderID CAN ID of the CANCoder
+     * @param angleOffset Angle Offset of the CANCoder to align the wheels
      */
-    public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
-        desiredState = CTREModuleState.optimize(desiredState, getState().angle);
-        // Custom optimize
-        // command, since
-        // default WPILib
-        // optimize assumes
-        // continuous
-        // controller which
-        // CTRE is
-        // not
+    public SwerveModule(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
+        Rotation2d angleOffset, SwerveModuleIO io) {
+        this.io = io;
 
-        if (isOpenLoop) {
-            double percentOutput = desiredState.speedMetersPerSecond / Constants.Swerve.MAX_SPEED;
-            driveMotor.set(ControlMode.PercentOutput, percentOutput);
-        } else {
-            double velocity = Conversions.mpsToFalcon(desiredState.speedMetersPerSecond,
-                Constants.Swerve.WHEEL_CIRCUMFERENCE, Constants.Swerve.DRIVE_GEAR_RATIO);
-            driveMotor.set(ControlMode.Velocity, velocity, DemandType.ArbitraryFeedForward,
-                feedforward.calculate(desiredState.speedMetersPerSecond));
-        }
+        this.moduleNumber = moduleNumber;
 
-        double angle =
-            (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.MAX_SPEED * 0.01))
-                ? lastAngle
-                : desiredState.angle.getDegrees(); // Prevent rotating module if speed is
-        // less then 1%. Prevents
-        // Jittering.
-        angleMotor.set(ControlMode.Position,
-            Conversions.degreesToFalcon(angle, Constants.Swerve.ANGLE_GEAR_RATIO));
-        lastAngle = angle;
-    }
+        this.angleOffset = angleOffset;
 
-    /**
-     *
-     */
-    private void resetToAbsolute() {
-        double absolutePosition = Conversions.degreesToFalcon(
-            getCanCoder().getDegrees() - angleOffset, Constants.Swerve.ANGLE_GEAR_RATIO);
-        angleMotor.setSelectedSensorPosition(absolutePosition);
+        // lastAngle = getState().angle.getDegrees();
+        io.updateInputs(inputs);
+        resetToAbsolute();
+        Logger.processInputs("SwerveModule" + moduleNumber, inputs);
     }
 
-    /**
-     *
-     *
-     * @param rotationSpeed Drive motor speed (-1 &lt;= value &lt;= 1)
-     */
-    public void setTurnAngle(double rotationSpeed) {
-        double absolutePosition = Conversions.degreesToFalcon(
-            getCanCoder().getDegrees() - angleOffset, Constants.Swerve.ANGLE_GEAR_RATIO);
-        angleMotor.setSelectedSensorPosition(absolutePosition);
-        driveMotor.set(ControlMode.PercentOutput, rotationSpeed);
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("SwerveModule" + moduleNumber, inputs);
     }
 
     /**
-     * Gets the Swerve module position
+     * Set the desired state of the Swerve Module
      *
-     * @return Swerve module position
+     * @param desiredState The desired {@link SwerveModuleState} for the module
+     * @param isOpenLoop Whether the state should be open or closed loop controlled
      */
-    public SwerveModulePosition getPosition() {
-        double position = Conversions.falconToMeters(driveMotor.getSelectedSensorPosition(),
-            Constants.Swerve.DRIVE_GEAR_RATIO, Constants.Swerve.WHEEL_CIRCUMFERENCE);
-
-        Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees(
-            angleMotor.getSelectedSensorPosition(), Constants.Swerve.ANGLE_GEAR_RATIO));
-        return new SwerveModulePosition(position, angle);
+    public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
+        desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
+        Logger.recordOutput("/ServeModule" + moduleNumber + "/DesiredState/Angle",
+            desiredState.angle);
+        Logger.recordOutput("/ServeModule" + moduleNumber + "/DesiredState/Speed",
+            desiredState.speedMetersPerSecond);
+        io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations()));
+        setSpeed(desiredState, isOpenLoop);
     }
 
     /**
-     * Configure the Angle motor CANCoder
+     * Set the velocity or power of the drive motor
+     *
+     * @param desiredState The desired {@link SwerveModuleState} of the module
+     * @param isOpenLoop Whether the state should be open or closed loop controlled
      */
-    private void configAngleEncoder() {
-        angleEncoder.configFactoryDefault();
-        angleEncoder.configAllSettings(Robot.ctreConfigs.swerveCanCoderConfig);
+    private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
+        if (isOpenLoop) {
+            driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.MAX_SPEED;
+            io.setDriveMotor(driveDutyCycle);
+            inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output;
+        } else {
+            driveVelocity.Velocity = Conversions.mpsToRPS(desiredState.speedMetersPerSecond,
+                Constants.Swerve.WHEEL_CIRCUMFERENCE);
+            driveVelocity.FeedForward =
+                driveFeedForward.calculate(desiredState.speedMetersPerSecond);
+            io.setDriveMotor(driveVelocity);
+            inputs.driveMotorSelectedSensorVelocity = driveVelocity.Velocity;
+        }
     }
 
     /**
-     * Configure the Angle motor
+     * Get the rotation of the CANCoder
+     *
+     * @return The rotation of the CANCoder in {@link Rotation2d}
      */
-    private void configAngleMotor() {
-        angleMotor.configFactoryDefault();
-        angleMotor.configAllSettings(Robot.ctreConfigs.swerveAngleFXConfig);
-        angleMotor.setInverted(Constants.Swerve.ANGLE_MOTOT_INVERT);
-        angleMotor.setNeutralMode(Constants.Swerve.ANGLE_NEUTRAL_MODE);
-        resetToAbsolute();
+    public Rotation2d getCANcoder() {
+        return Rotation2d.fromRotations(inputs.absolutePositionAngleEncoder);
     }
 
     /**
-     * Configure the Drive motor
+     * Reset the Swerve Module angle to face forward
      */
-    private void configDriveMotor() {
-        driveMotor.configFactoryDefault();
-        driveMotor.configAllSettings(Robot.ctreConfigs.swerveDriveFXConfig);
-        driveMotor.setInverted(Constants.Swerve.DRIVE_MOTOR_INVERT);
-        driveMotor.setNeutralMode(Constants.Swerve.DRIVE_NEUTRAL_MODE);
-        driveMotor.setSelectedSensorPosition(0);
+    public void resetToAbsolute() {
+        double absolutePosition = getCANcoder().getRotations() - angleOffset.getRotations();
+        io.setPositionAngleMotor(absolutePosition);
+        inputs.angleMotorSelectedPosition = absolutePosition;
     }
 
     /**
-     * Get the 2d rotation of the module
+     * Get the current Swerve Module State
      *
-     * @return 2d rotation of the module
+     * @return The current {@link SwerveModuleState}
      */
-    public Rotation2d getCanCoder() {
-        return Rotation2d.fromDegrees(angleEncoder.getAbsolutePosition());
+    public SwerveModuleState getState() {
+        return new SwerveModuleState(
+            Conversions.rpsToMPS(inputs.driveMotorSelectedSensorVelocity,
+                Constants.Swerve.WHEEL_CIRCUMFERENCE),
+            Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
     }
 
     /**
-     * Gets the Swerve module state
+     * Get the current Swerve Module Position
      *
-     * @return The current Swerve Module State {@link SwerveModuleState}
+     * @return The current {@link SwerveModulePosition}
      */
-    public SwerveModuleState getState() {
-        double velocity = Conversions.falconToMPS(driveMotor.getSelectedSensorVelocity(),
-            Constants.Swerve.WHEEL_CIRCUMFERENCE, Constants.Swerve.DRIVE_GEAR_RATIO);
-        Rotation2d angle = Rotation2d.fromDegrees(Conversions.falconToDegrees(
-            angleMotor.getSelectedSensorPosition(), Constants.Swerve.ANGLE_GEAR_RATIO));
-        return new SwerveModuleState(velocity, angle);
-    }
+    public SwerveModulePosition getPosition() {
+        return new SwerveModulePosition(
+            Conversions.rotationsToMeters(inputs.driveMotorSelectedPosition,
+                Constants.Swerve.WHEEL_CIRCUMFERENCE),
+            Rotation2d.fromRotations(inputs.angleMotorSelectedPosition));
 
+    }
 }
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java b/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java
deleted file mode 100644
index d6402e41..00000000
--- a/src/main/java/frc/lib/util/swerve/SwerveModuleConstants.java
+++ /dev/null
@@ -1,27 +0,0 @@
-package frc.lib.util.swerve;
-
-/**
- * Constants file used when creating swerve modules
- */
-public class SwerveModuleConstants {
-    public final int driveMotorID;
-    public final int angleMotorID;
-    public final int cancoderID;
-    public final double angleOffset;
-
-    /**
-     * Swerve Module Constants to be used when creating swerve modules.
-     *
-     * @param driveMotorID ID of the drive motor
-     * @param angleMotorID ID of the angle motor
-     * @param canCoderID ID of the canCoder
-     * @param angleOffset offset of the canCoder angle
-     */
-    public SwerveModuleConstants(int driveMotorID, int angleMotorID, int canCoderID,
-        double angleOffset) {
-        this.driveMotorID = driveMotorID;
-        this.angleMotorID = angleMotorID;
-        this.cancoderID = canCoderID;
-        this.angleOffset = angleOffset;
-    }
-}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
new file mode 100644
index 00000000..282d03b6
--- /dev/null
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
@@ -0,0 +1,28 @@
+package frc.lib.util.swerve;
+
+import org.littletonrobotics.junction.AutoLog;
+import com.ctre.phoenix6.controls.ControlRequest;
+
+/** IO Class for SwerveModule */
+public interface SwerveModuleIO {
+
+    /** Inputs Class for SwerveModule */
+    @AutoLog
+    public static class SwerveModuleInputs {
+        public double driveMotorSelectedPosition;
+        public double driveMotorSelectedSensorVelocity;
+        public double angleMotorSelectedPosition;
+        public double absolutePositionAngleEncoder;
+    }
+
+    public default void updateInputs(SwerveModuleInputs inputs) {}
+
+    public default void setDriveMotor(ControlRequest request) {}
+
+    public default void setAngleMotor(ControlRequest request) {}
+
+    public default void setAngleSelectedSensorPosition(double angle) {}
+
+    public default void setPositionAngleMotor(double absolutePosition) {}
+
+}
diff --git a/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
new file mode 100644
index 00000000..f805e488
--- /dev/null
+++ b/src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
@@ -0,0 +1,139 @@
+package frc.lib.util.swerve;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.Constants;
+
+/**
+ * Swerve Module with real motors and sensors
+ */
+public class SwerveModuleReal implements SwerveModuleIO {
+    private TalonFX mAngleMotor;
+    private TalonFX mDriveMotor;
+    private CANcoder angleEncoder;
+    private TalonFXConfiguration swerveAngleFXConfig = new TalonFXConfiguration();
+    private TalonFXConfiguration swerveDriveFXConfig = new TalonFXConfiguration();
+    private CANcoderConfiguration swerveCANcoderConfig = new CANcoderConfiguration();
+
+    /**
+     * A real Swerve Module with motors and sensors
+     *
+     * @param moduleNumber The Module Number
+     * @param driveMotorID The Drive Motor CAN ID
+     * @param angleMotorID The Angle Motor CAN ID
+     * @param cancoderID The CANCode CAN ID
+     * @param angleOffset The Angle Offset in {@link Rotation2d}
+     */
+    public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
+        Rotation2d angleOffset) {
+
+        angleEncoder = new CANcoder(cancoderID, "canivore");
+        mDriveMotor = new TalonFX(driveMotorID, "canivore");
+        mAngleMotor = new TalonFX(angleMotorID, "canivore");
+
+        configAngleEncoder();
+        configAngleMotor();
+        configDriveMotor();
+    }
+
+    private void configAngleMotor() {
+        /* Angle Motor Config */
+        /* Motor Inverts and Neutral Mode */
+        swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode;
+
+        /* Gear Ratio and Wrapping Config */
+        swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.ANGLE_GEAR_RATIO;
+        swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true;
+
+        /* Current Limiting */
+        swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
+            Constants.Swerve.ANGLE_ENABLE_CURRENT_LIMIT;
+        swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit =
+            Constants.Swerve.ANGLE_CONTINOUS_CURRENT_LIMIT;
+        swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold =
+            Constants.Swerve.ANGLE_PEAK_CURRENT_LIMIT;
+        swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold =
+            Constants.Swerve.ANGLE_PEAK_CURRENT_DURATION;
+
+        /* PID Config */
+        swerveAngleFXConfig.Slot0.kP = Constants.Swerve.ANGLE_KP;
+        swerveAngleFXConfig.Slot0.kI = Constants.Swerve.ANGLE_KI;
+        swerveAngleFXConfig.Slot0.kD = Constants.Swerve.ANGLE_KD;
+
+        mAngleMotor.getConfigurator().apply(swerveAngleFXConfig);
+        mAngleMotor.setInverted(Constants.Swerve.ANGLE_MOTOR_INVERT);
+    }
+
+    private void configDriveMotor() {
+        /* Drive Motor Config */
+        /* Motor Inverts and Neutral Mode */
+        swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode;
+
+        /* Gear Ratio Config */
+        swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.DRIVE_GEAR_RATIO;
+
+        /* Current Limiting */
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable =
+            Constants.Swerve.DRIVE_ENABLE_CURRENT_LIMIT;
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit =
+            Constants.Swerve.DRIVE_CONTINOUS_CURRENT_LIMIT;
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold =
+            Constants.Swerve.DRIVE_PEAK_CURRENT_LIMIT;
+        swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold =
+            Constants.Swerve.DRIVE_PEAK_CURRENT_DURATION;
+
+        /* PID Config */
+        swerveDriveFXConfig.Slot0.kP = Constants.Swerve.DRIVE_KP;
+        swerveDriveFXConfig.Slot0.kI = Constants.Swerve.DRIVE_KI;
+        swerveDriveFXConfig.Slot0.kD = Constants.Swerve.DRIVE_KD;
+
+        /* Open and Closed Loop Ramping */
+        swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod =
+            Constants.Swerve.OPEN_LOOP_RAMP;
+        swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod =
+            Constants.Swerve.OPEN_LOOP_RAMP;
+
+        swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod =
+            Constants.Swerve.CLOSED_LOOP_RAMP;
+        swerveDriveFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod =
+            Constants.Swerve.CLOSED_LOOP_RAMP;
+
+        mDriveMotor.getConfigurator().apply(swerveDriveFXConfig);
+        mDriveMotor.getConfigurator().setPosition(0.0);
+        mDriveMotor.setInverted(Constants.Swerve.DRIVE_MOTOR_INVERT);
+    }
+
+    private void configAngleEncoder() {
+        /* Angle Encoder Config */
+        swerveCANcoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.CAN_CODER_INVERT;
+        angleEncoder.getConfigurator().apply(swerveCANcoderConfig);
+    }
+
+    @Override
+    public void setAngleMotor(ControlRequest request) {
+        mAngleMotor.setControl(request);
+    }
+
+    @Override
+    public void setDriveMotor(ControlRequest request) {
+        mDriveMotor.setControl(request);
+    }
+
+    @Override
+    public void updateInputs(SwerveModuleInputs inputs) {
+        inputs.driveMotorSelectedPosition = mDriveMotor.getPosition().getValueAsDouble();
+        inputs.driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity().getValueAsDouble();
+        inputs.angleMotorSelectedPosition = mAngleMotor.getPosition().getValueAsDouble();
+        inputs.absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition().getValueAsDouble();
+    }
+
+    @Override
+    public void setPositionAngleMotor(double absolutePosition) {
+        mAngleMotor.setPosition(absolutePosition);
+    }
+
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index fd07ec94..e55b1369 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,9 +1,11 @@
 package frc.robot;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -13,7 +15,6 @@
 import edu.wpi.first.math.util.Units;
 import frc.lib.math.DoubleJointedArmFeedforward;
 import frc.lib.math.DoubleJointedArmFeedforward.JointConfig;
-import frc.lib.util.swerve.SwerveModuleConstants;
 
 /**
  * Constants file.
@@ -106,37 +107,38 @@ public static final class Swerve {
         public static final boolean DRIVE_ENABLE_CURRENT_LIMIT = true;
 
         /* Angle Motor PID Values */
-        public static final double ANGLE_KP = 0.6;
+        public static final double ANGLE_KP = 100;
         public static final double ANGLE_KI = 0.0;
-        public static final double ANGLE_KD = 12.0;
+        public static final double ANGLE_KD = 0.0;
         public static final double ANGLE_KF = 0.0;
 
         /* Drive Motor PID Values */
-        public static final double DRIVE_KP = 0.10;
+        public static final double DRIVE_KP = 0.12;
         public static final double DRIVE_KI = 0.0;
         public static final double DRIVE_KD = 0.0;
         public static final double DRIVE_KF = 0.0;
 
         /* Drive Motor Characterization Values */
-        public static final double DRIVE_KS = (0.667 / 12);
+        public static final double DRIVE_KS = (0.667);
         // divide by 12 to convert from volts to percent output for CTRE
-        public static final double DRIVE_KV = (2.44 / 12);
-        public static final double DRIVE_KA = (0.27 / 12);
+        public static final double DRIVE_KV = (2.44);
+        public static final double DRIVE_KA = (0.27);
 
         /* Swerve Profiling Values */
         public static final double MAX_SPEED = 4; // meters per second
-        public static final double MAX_ANGULAR_VELOCITY = 4.0;
+        public static final double MAX_ANGULAR_VELOCITY = 10.0;
 
         /* Neutral Modes */
-        public static final NeutralMode ANGLE_NEUTRAL_MODE = NeutralMode.Coast;
-        public static final NeutralMode DRIVE_NEUTRAL_MODE = NeutralMode.Brake;
+        public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast;
+        public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake;
 
         /* Motor Inverts */
         public static final boolean DRIVE_MOTOR_INVERT = false;
-        public static final boolean ANGLE_MOTOT_INVERT = false;
+        public static final boolean ANGLE_MOTOR_INVERT = false;
 
         /* Angle Encoder Invert */
-        public static final boolean CAN_CODER_INVERT = false;
+        public static final SensorDirectionValue CAN_CODER_INVERT =
+            SensorDirectionValue.CounterClockwise_Positive;
 
         /* Module Specific Constants */
         /**
@@ -146,9 +148,7 @@ public static final class Mod0 {
             public static final int DRIVE_MOTOR_ID = 6;
             public static final int ANGLE_MOTOR_ID = 8;
             public static final int CAN_CODER_ID = 4;
-            public static final double ANGLE_OFFSET = 138.604;
-            public static final SwerveModuleConstants constants = new SwerveModuleConstants(
-                DRIVE_MOTOR_ID, ANGLE_MOTOR_ID, CAN_CODER_ID, ANGLE_OFFSET);
+            public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(138.604);
         }
 
         /**
@@ -158,9 +158,7 @@ public static final class Mod1 {
             public static final int DRIVE_MOTOR_ID = 1;
             public static final int ANGLE_MOTOR_ID = 4;
             public static final int CAN_CODER_ID = 1;
-            public static final double ANGLE_OFFSET = 280.107;
-            public static final SwerveModuleConstants constants = new SwerveModuleConstants(
-                DRIVE_MOTOR_ID, ANGLE_MOTOR_ID, CAN_CODER_ID, ANGLE_OFFSET);
+            public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(280.107);
         }
 
         /**
@@ -170,9 +168,7 @@ public static final class Mod2 {
             public static final int DRIVE_MOTOR_ID = 3;
             public static final int ANGLE_MOTOR_ID = 2;
             public static final int CAN_CODER_ID = 2;
-            public static final double ANGLE_OFFSET = 121.553;
-            public static final SwerveModuleConstants constants = new SwerveModuleConstants(
-                DRIVE_MOTOR_ID, ANGLE_MOTOR_ID, CAN_CODER_ID, ANGLE_OFFSET);
+            public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(121.553);
         }
 
         /**
@@ -182,9 +178,7 @@ public static final class Mod3 {
             public static final int DRIVE_MOTOR_ID = 7;
             public static final int ANGLE_MOTOR_ID = 5;
             public static final int CAN_CODER_ID = 3;
-            public static final double ANGLE_OFFSET = 248.027;
-            public static final SwerveModuleConstants constants = new SwerveModuleConstants(
-                DRIVE_MOTOR_ID, ANGLE_MOTOR_ID, CAN_CODER_ID, ANGLE_OFFSET);
+            public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(248.027);
         }
 
         public static final HolonomicPathFollowerConfig pathFollowerConfig =
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7e598044..2fe58af2 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,12 +4,20 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.TimedRobot;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+import java.util.Scanner;
+import org.littletonrobotics.junction.LogFileUtil;
+import org.littletonrobotics.junction.LoggedRobot;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.wpilog.WPILOGReader;
+import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.lib.util.Scoring;
 import frc.lib.util.Scoring.GamePiece;
-import frc.lib.util.ctre.CTREConfigs;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -17,13 +25,24 @@
  * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
-public class Robot extends TimedRobot {
-    public static CTREConfigs ctreConfigs;
-
+public class Robot extends LoggedRobot {
     private Command m_autonomousCommand;
-
     private RobotContainer m_robotContainer;
 
+    /**
+     * Robnot Run type
+     */
+    public static enum RobotRunType {
+        /** Real Robot. */
+        kReal,
+        /** Simulation runtime. */
+        kSimulation,
+        /** Replay runtime. */
+        kReplay;
+    }
+
+    public RobotRunType robotRunType = RobotRunType.kReal;
+
     public static int level = 0;
     public static int column = 0;
     // public static UsbCamera camera = CameraServer.startAutomaticCapture("Magazine Camera", 0);
@@ -35,10 +54,56 @@ public class Robot extends TimedRobot {
      */
     @Override
     public void robotInit() {
-        ctreConfigs = new CTREConfigs();
-        // Instantiate our RobotContainer. This will perform all our button bindings, and put our
-        // autonomous chooser on the dashboard.
-        m_robotContainer = new RobotContainer();
+        // Record metadata
+        Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME);
+        Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE);
+        Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
+        Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
+        Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
+        switch (BuildConstants.DIRTY) {
+            case 0:
+                Logger.recordMetadata("GitDirty", "All changes committed");
+                break;
+            case 1:
+                Logger.recordMetadata("GitDirty", "Uncommitted changes");
+                break;
+            default:
+                Logger.recordMetadata("GitDirty", "Unknown");
+                break;
+        }
+
+
+
+        if (isReal()) {
+            Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); // Log to a USB stick
+            Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+            // new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
+            setUseTiming(true);
+            robotRunType = RobotRunType.kReal;
+        } else {
+            String logPath = findReplayLog();
+            if (logPath == null) {
+                Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+                setUseTiming(true);
+                robotRunType = RobotRunType.kSimulation;
+            } else {
+                // (or prompt the user)
+                Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log
+                Logger
+                    .addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+                // Save outputs to a new log
+                setUseTiming(false); // Run as fast as possible
+                robotRunType = RobotRunType.kReplay;
+
+            }
+        }
+        // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the
+        // "Understanding Data Flow" page
+        Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values
+
+        // Instantiate our RobotContainer. This will perform all our button bindings,
+        // and put our autonomous chooser on the dashboard.
+        m_robotContainer = new RobotContainer(robotRunType);
     }
 
     /**
@@ -110,4 +175,38 @@ public void testInit() {
     /** This function is called periodically during test mode. */
     @Override
     public void testPeriodic() {}
+
+
+    private static final String environmentVariable = "AKIT_LOG_PATH";
+    private static final String advantageScopeFileName = "akit-log-path.txt";
+
+    /**
+     * Finds the path to a log file for replay, using the following priorities: 1. The value of the
+     * "AKIT_LOG_PATH" environment variable, if set 2. The file currently open in AdvantageScope, if
+     * available 3. The result of the prompt displayed to the user
+     */
+    public static String findReplayLog() {
+        // Read environment variables
+        String envPath = System.getenv(environmentVariable);
+        if (envPath != null) {
+            System.out.println("Using log from " + environmentVariable
+                + " environment variable - \"" + envPath + "\"");
+            return envPath;
+        }
+
+        // Read file from AdvantageScope
+        Path advantageScopeTempPath =
+            Paths.get(System.getProperty("java.io.tmpdir"), advantageScopeFileName);
+        String advantageScopeLogPath = null;
+        try (Scanner fileScanner = new Scanner(advantageScopeTempPath)) {
+            advantageScopeLogPath = fileScanner.nextLine();
+        } catch (IOException e) {
+            System.out.println("Something went wrong");
+        }
+        if (advantageScopeLogPath != null) {
+            System.out.println("Using log from AdvantageScope - \"" + advantageScopeLogPath + "\"");
+            return advantageScopeLogPath;
+        }
+        return null;
+    }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 81cf808e..71499b77 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,6 +24,7 @@
 import frc.lib.util.DisabledInstantCommand;
 import frc.lib.util.Scoring;
 import frc.lib.util.Scoring.GamePiece;
+import frc.robot.Robot.RobotRunType;
 import frc.robot.autos.PPExample;
 import frc.robot.commands.arm.ConeIntake;
 import frc.robot.commands.arm.ConeUpIntake;
@@ -40,8 +41,10 @@
 import frc.robot.commands.wrist.VariableIntake;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.LEDs;
-import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.WristIntake;
+import frc.robot.subsystems.swerve.Swerve;
+import frc.robot.subsystems.swerve.SwerveIO;
+import frc.robot.subsystems.swerve.SwerveReal;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -114,21 +117,34 @@ public class RobotContainer {
 
     /* Subsystems */
     private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT);
-    public final Swerve s_Swerve = new Swerve();
+    public Swerve s_Swerve;
     // private final DropIntake s_dIntake = new DropIntake();
     private final Arm s_Arm = new Arm(ph);
     private final WristIntake s_wristIntake = new WristIntake();
 
 
-    public GenericEntry photonSeeing = mainDriverTab
-        .add("Photon Good", s_Swerve.cam.latency() < 0.6).withWidget(BuiltInWidgets.kBooleanBox)
-        .withProperties(Map.of("Color when true", "green", "Color when false", "red"))
-        .withPosition(8, 0).withSize(2, 2).getEntry();
+    // public GenericEntry photonSeeing = mainDriverTab
+    // .add("Photon Good", s_Swerve.cam.latency() < 0.6).withWidget(BuiltInWidgets.kBooleanBox)
+    // .withProperties(Map.of("Color when true", "green", "Color when false", "red"))
+    // .withPosition(8, 0).withSize(2, 2).getEntry();
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
-    public RobotContainer() {
+    public RobotContainer(RobotRunType runtimeType) {
+        autoChooser.setDefaultOption("Wait 1 Second", "wait");
+        switch (runtimeType) {
+            case kReal:
+                s_Swerve = new Swerve(new SwerveReal());
+                break;
+            case kSimulation:
+                // drivetrain = new Drivetrain(new DrivetrainSim() {});
+                break;
+            default:
+                s_Swerve = new Swerve(new SwerveIO() {});
+        }
+
+
         ph.enableCompressorAnalog(90, 120);
         s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
             Constants.Swerve.IS_FIELD_RELATIVE, Constants.Swerve.IS_OPEN_LOOP, s_Arm));
diff --git a/src/main/java/frc/robot/autos/PPExample.java b/src/main/java/frc/robot/autos/PPExample.java
index c6a3a00b..92aea111 100644
--- a/src/main/java/frc/robot/autos/PPExample.java
+++ b/src/main/java/frc/robot/autos/PPExample.java
@@ -4,7 +4,7 @@
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Path Planner Example Auto
diff --git a/src/main/java/frc/robot/autos/TrajectoryBase.java b/src/main/java/frc/robot/autos/TrajectoryBase.java
index 8181f475..96fadc31 100644
--- a/src/main/java/frc/robot/autos/TrajectoryBase.java
+++ b/src/main/java/frc/robot/autos/TrajectoryBase.java
@@ -6,7 +6,7 @@
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Parent class for all autonomous commands
diff --git a/src/main/java/frc/robot/commands/drive/ClimbPlatform.java b/src/main/java/frc/robot/commands/drive/ClimbPlatform.java
index 227b8d75..21f49a1b 100644
--- a/src/main/java/frc/robot/commands/drive/ClimbPlatform.java
+++ b/src/main/java/frc/robot/commands/drive/ClimbPlatform.java
@@ -7,7 +7,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Test April tag transform
diff --git a/src/main/java/frc/robot/commands/drive/MoveToEngage.java b/src/main/java/frc/robot/commands/drive/MoveToEngage.java
index a510f2ae..1f807b92 100644
--- a/src/main/java/frc/robot/commands/drive/MoveToEngage.java
+++ b/src/main/java/frc/robot/commands/drive/MoveToEngage.java
@@ -4,8 +4,8 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.lib.util.FieldConstants;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.WristIntake;
+import frc.robot.subsystems.swerve.Swerve;
 
 
 /**
diff --git a/src/main/java/frc/robot/commands/drive/MoveToPos.java b/src/main/java/frc/robot/commands/drive/MoveToPos.java
index 04ee877b..eb16683f 100644
--- a/src/main/java/frc/robot/commands/drive/MoveToPos.java
+++ b/src/main/java/frc/robot/commands/drive/MoveToPos.java
@@ -13,7 +13,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.lib.util.FieldConstants;
 import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Move to Position
diff --git a/src/main/java/frc/robot/commands/drive/MoveToScore.java b/src/main/java/frc/robot/commands/drive/MoveToScore.java
index 8f2d8742..ad6d8f41 100644
--- a/src/main/java/frc/robot/commands/drive/MoveToScore.java
+++ b/src/main/java/frc/robot/commands/drive/MoveToScore.java
@@ -5,8 +5,8 @@
 import frc.lib.util.Scoring;
 import frc.robot.commands.arm.ScoreArm;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.Swerve;
 import frc.robot.subsystems.WristIntake;
+import frc.robot.subsystems.swerve.Swerve;
 
 
 /**
diff --git a/src/main/java/frc/robot/commands/drive/TeleopSwerve.java b/src/main/java/frc/robot/commands/drive/TeleopSwerve.java
index 31e2ebfe..52ba102c 100644
--- a/src/main/java/frc/robot/commands/drive/TeleopSwerve.java
+++ b/src/main/java/frc/robot/commands/drive/TeleopSwerve.java
@@ -7,7 +7,7 @@
 import frc.lib.math.Conversions;
 import frc.robot.Constants;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Creates an command for driving the swerve drive during tele-op
@@ -59,7 +59,7 @@ public void execute() {
 
         Translation2d translation = new Translation2d(yaxis, xaxis).times(speed);
         double rotation = raxis * angle_speed;
-        swerveDrive.driveWithTwist(translation, rotation, fieldRelative, openLoop);
+        swerveDrive.drive(translation, rotation, fieldRelative, openLoop);
 
     }
 }
diff --git a/src/main/java/frc/robot/commands/drive/TurnToAngle.java b/src/main/java/frc/robot/commands/drive/TurnToAngle.java
index 7b8ab373..0e2db30e 100644
--- a/src/main/java/frc/robot/commands/drive/TurnToAngle.java
+++ b/src/main/java/frc/robot/commands/drive/TurnToAngle.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * This command will turn the robot to a specified angle.
diff --git a/src/main/java/frc/robot/commands/test/TestTransform.java b/src/main/java/frc/robot/commands/test/TestTransform.java
index 957c2c06..c8f33d00 100644
--- a/src/main/java/frc/robot/commands/test/TestTransform.java
+++ b/src/main/java/frc/robot/commands/test/TestTransform.java
@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.lib.util.FieldConstants;
 import frc.robot.Constants;
-import frc.robot.subsystems.Swerve;
+import frc.robot.subsystems.swerve.Swerve;
 
 /**
  * Test April tag transform
diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java
deleted file mode 100644
index 10b38f75..00000000
--- a/src/main/java/frc/robot/subsystems/Swerve.java
+++ /dev/null
@@ -1,305 +0,0 @@
-package frc.robot.subsystems;
-
-import java.util.Map;
-import java.util.Optional;
-import com.kauailabs.navx.frc.AHRS;
-import com.pathplanner.lib.auto.AutoBuilder;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-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.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
-import edu.wpi.first.wpilibj.smartdashboard.Field2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.lib.util.PhotonCameraWrapper;
-import frc.lib.util.swerve.SwerveModule;
-import frc.robot.Constants;
-import frc.robot.RobotContainer;
-
-/**
- * Creates swerve drive and commands for drive.
- */
-public class Swerve extends SubsystemBase {
-    private AHRS gyro = new AHRS(Constants.Swerve.navXID);
-    private SwerveDrivePoseEstimator swerveOdometry;
-    public PhotonCameraWrapper cam = new PhotonCameraWrapper(Constants.CameraConstants.CAMERA_NAME,
-        Constants.CameraConstants.KCAMERA_TO_ROBOT.inverse());
-    private SwerveModule[] swerveMods;
-    private double fieldOffset = gyro.getYaw();
-    private final Field2d field = new Field2d();
-    private boolean hasInitialized = false;
-    // private ComplexWidget fieldWidget = RobotContainer.mainDriverTab.add("Field Pos", field)
-    // .withWidget(BuiltInWidgets.kField).withSize(8, 4) // make the widget 2x1
-    // .withPosition(0, 0); // place it in the top-left corner
-    private GenericEntry aprilTagTarget = RobotContainer.autoTab
-        .add("Currently Seeing April Tag", false).withWidget(BuiltInWidgets.kBooleanBox)
-        .withProperties(Map.of("Color when true", "green", "Color when false", "red"))
-        .withPosition(8, 4).withSize(2, 2).getEntry();
-
-    /**
-     * Initializes swerve modules.
-     */
-    public Swerve() {
-        swerveMods = new SwerveModule[] {new SwerveModule(0, Constants.Swerve.Mod0.constants),
-            new SwerveModule(1, Constants.Swerve.Mod1.constants),
-            new SwerveModule(2, Constants.Swerve.Mod2.constants),
-            new SwerveModule(3, Constants.Swerve.Mod3.constants)};
-
-        swerveOdometry = new SwerveDrivePoseEstimator(Constants.Swerve.SWERVE_KINEMATICS, getYaw(),
-            getPositions(), new Pose2d());
-
-        RobotContainer.autoTab.add("Field Pos", field).withWidget(BuiltInWidgets.kField)
-            .withSize(8, 6) // make the widget 2x1
-            .withPosition(0, 0); // place it in the top-left corner
-
-        AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, this::getChassisSpeeds,
-            this::setModuleStates, Constants.Swerve.pathFollowerConfig, (() -> shouldFlipPath()),
-            this);
-    }
-
-    /**
-     * New command to set wheels inward.
-     */
-    public void wheelsIn() {
-        swerveMods[0].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(45)), false);
-        swerveMods[1].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(135)), false);
-        swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), false);
-        swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)),
-            false);
-        this.setMotorsZero();
-    }
-
-    /**
-     * Moves the swerve drive train, creates twist2d that accounts for the fact that positions are
-     * not updated continuously, (updated every .02 seconds.)
-     *
-     * @param translation The 2d translation in the X-Y plane
-     * @param rotation The amount of rotation in the Z axis
-     * @param fieldRelative Whether the movement is relative to the field or absolute
-     * @param isOpenLoop Open or closed loop system
-     */
-    public void driveWithTwist(Translation2d translation, double rotation, boolean fieldRelative,
-        boolean isOpenLoop) {
-        ChassisSpeeds chassisSpeeds = fieldRelative
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(),
-                rotation, getFieldRelativeHeading())
-            : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
-        setModuleStates(chassisSpeeds);
-    }
-
-    /**
-     * Sets motors to 0 or inactive.
-     */
-    public void setMotorsZero() {
-        setModuleStates(new ChassisSpeeds(0, 0, 0));
-    }
-
-    /**
-     * Used by SwerveControllerCommand in Auto
-     *
-     * @param desiredStates The desired states of the swerve modules
-     */
-    public void setModuleStates(SwerveModuleState[] desiredStates) {
-        double speed = Constants.Swerve.MAX_SPEED;
-        SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, speed);
-
-        for (SwerveModule mod : swerveMods) {
-            mod.setDesiredState(desiredStates[mod.moduleNumber], false);
-        }
-    }
-
-    /**
-     * Sets swerve module states using Chassis Speeds.
-     *
-     * @param chassisSpeeds The desired Chassis Speeds
-     */
-    public void setModuleStates(ChassisSpeeds chassisSpeeds) {
-        ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(chassisSpeeds, 0.02);
-        SwerveModuleState[] swerveModuleStates =
-            Constants.Swerve.SWERVE_KINEMATICS.toSwerveModuleStates(targetSpeeds);
-        setModuleStates(swerveModuleStates);
-    }
-
-    /**
-     * Returns the position of the robot on the field.
-     *
-     * @return The pose of the robot (x and y are in meters).
-     */
-    public Pose2d getPose() {
-        return swerveOdometry.getEstimatedPosition();
-    }
-
-
-    /**
-     * Get the current Swerve Module States
-     *
-     * @return An array of the current {@link SwerveModuleState}
-     */
-    public SwerveModuleState[] getModuleStates() {
-        SwerveModuleState[] states = new SwerveModuleState[swerveMods.length];
-        for (int i = 0; i < swerveMods.length; i++) {
-            states[i] = swerveMods[i].getState();
-        }
-        return states;
-    }
-
-    /**
-     * Get the current Chassis Speeds
-     *
-     * @return The current {@link ChassisSpeeds}
-     */
-    public ChassisSpeeds getChassisSpeeds() {
-        return Constants.Swerve.SWERVE_KINEMATICS.toChassisSpeeds(getModuleStates());
-    }
-
-    /**
-     * Resets the robot's position on the field.
-     *
-     * @param pose The position on the field that your robot is at.
-     */
-    public void resetOdometry(Pose2d pose) {
-        swerveOdometry.resetPosition(getYaw(), getPositions(), pose);
-    }
-
-    /**
-     * Gets the states of each swerve module.
-     *
-     * @return Swerve module state
-     */
-    public SwerveModuleState[] getStates() {
-        SwerveModuleState[] states = new SwerveModuleState[4];
-        for (SwerveModule mod : swerveMods) {
-            states[mod.moduleNumber] = mod.getState();
-        }
-        return states;
-    }
-
-    /**
-     * Resets the gyro field relative driving offset
-     */
-    public void resetFieldRelativeOffset() {
-        fieldOffset = getYaw().getDegrees();
-    }
-
-    public Rotation2d getFieldRelativeHeading() {
-        return Rotation2d.fromDegrees(getYaw().getDegrees() - fieldOffset);
-    }
-
-    /**
-     * Gets the rotation degree from swerve modules.
-     */
-    public Rotation2d getYaw() {
-        float yaw = gyro.getYaw();
-        return (Constants.Swerve.INVERT_GYRO) ? Rotation2d.fromDegrees(-yaw)
-            : Rotation2d.fromDegrees(yaw);
-    }
-
-    /**
-     * Gets the roll (pitch) degree from swerve modules.
-     */
-    public float getRoll() {
-        return gyro.getRoll();
-    }
-
-    @Override
-    public void periodic() {
-        Rotation2d yaw = getYaw();
-        swerveOdometry.update(yaw, getPositions());
-        SmartDashboard.putBoolean("photonGood", cam.latency() < 0.6);
-        if (!hasInitialized /* || DriverStation.isDisabled() */) {
-            var robotPose = cam.getInitialPose();
-            if (robotPose.isPresent()) {
-                swerveOdometry.resetPosition(getYaw(), getPositions(), robotPose.get());
-                hasInitialized = true;
-            }
-        } else {
-            var result = cam.getEstimatedGlobalPose(swerveOdometry.getEstimatedPosition());
-            if (result.isPresent()) {
-                var camPose = result.get();
-                if (camPose.targetsUsed.get(0).getArea() > 0.7) {
-                    swerveOdometry.addVisionMeasurement(camPose.estimatedPose.toPose2d(),
-                        camPose.timestampSeconds);
-                }
-                field.getObject("Cam Est Pose").setPose(camPose.estimatedPose.toPose2d());
-            } else {
-                field.getObject("Cam Est Pose").setPose(new Pose2d(-100, -100, new Rotation2d()));
-            }
-        }
-
-        field.setRobotPose(getPose());
-        aprilTagTarget.setBoolean(cam.seesTarget());
-
-        SmartDashboard.putBoolean("Has Initialized", hasInitialized);
-        SmartDashboard.putNumber("Robot X", getPose().getX());
-        SmartDashboard.putNumber("Robot Y", getPose().getY());
-        SmartDashboard.putNumber("Robot Rotation", getPose().getRotation().getDegrees());
-        SmartDashboard.putNumber("Gyro Yaw", yaw.getDegrees());
-        SmartDashboard.putNumber("Field Offset", fieldOffset);
-        SmartDashboard.putNumber("Gyro Yaw - Offset", getFieldRelativeHeading().getDegrees());
-        SmartDashboard.putNumber("Gyro roll", getRoll());
-
-
-        for (SwerveModule mod : swerveMods) {
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Cancoder",
-                mod.getCanCoder().getDegrees());
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Integrated",
-                mod.getState().angle.getDegrees());
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity",
-                mod.getState().speedMetersPerSecond);
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Position",
-                mod.getPosition().distanceMeters);
-        }
-    }
-
-    /**
-     * Get position of all swerve modules
-     *
-     * @return Array of Swerve Module Positions
-     */
-    public SwerveModulePosition[] getPositions() {
-        SwerveModulePosition[] positions = new SwerveModulePosition[4];
-
-        for (SwerveModule mod : swerveMods) {
-            positions[mod.moduleNumber] = mod.getPosition();
-
-        }
-        return positions;
-    }
-
-    /**
-     * Reset April Tag position initialization
-     */
-    public void resetInitialized() {
-        this.hasInitialized = false;
-    }
-
-    /**
-     * Zero the Gyro
-     */
-    public void zeroGyro() {
-        gyro.zeroYaw();
-    }
-
-    /**
-     * Whether or not to flip the path if on Red Alliance
-     *
-     * @return True if the path needs to be flipped when on Red Alliance
-     */
-    public boolean shouldFlipPath() {
-        Optional<Alliance> ally = DriverStation.getAlliance();
-        if (ally.isPresent()) {
-            return ally.get() == Alliance.Red;
-        }
-        return false;
-
-
-    }
-}
diff --git a/src/main/java/frc/robot/subsystems/WristIntake.java b/src/main/java/frc/robot/subsystems/WristIntake.java
index b23be6d9..f60e12cc 100644
--- a/src/main/java/frc/robot/subsystems/WristIntake.java
+++ b/src/main/java/frc/robot/subsystems/WristIntake.java
@@ -1,8 +1,7 @@
 package frc.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
@@ -12,7 +11,8 @@
  */
 public class WristIntake extends SubsystemBase {
 
-    private final WPI_TalonFX wristIntakeMotor = new WPI_TalonFX(Constants.Wrist.WRIST_INTAKE_ID);
+    private TalonFXConfiguration wristMotorConfig = new TalonFXConfiguration();
+    private final TalonFX wristIntakeMotor = new TalonFX(Constants.Wrist.WRIST_INTAKE_ID);
 
     private boolean invertForCone = true;
 
@@ -21,17 +21,20 @@ public class WristIntake extends SubsystemBase {
      */
     public WristIntake() {
         wristIntakeMotor.setInverted(false);
-        wristIntakeMotor.setNeutralMode(NeutralMode.Brake);
+        wristMotorConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode;
+        wristIntakeMotor.getConfigurator().apply(wristMotorConfig);
+
     }
 
     @Override
     public void periodic() {
-        SmartDashboard.putNumber("Wrist Intake Current", wristIntakeMotor.getStatorCurrent());
+        SmartDashboard.putNumber("Wrist Intake Current",
+            wristIntakeMotor.getStatorCurrent().getValueAsDouble());
     }
 
     // Runs wrist intake motor to intake a game piece.
     public void intake() {
-        wristIntakeMotor.set(ControlMode.PercentOutput, Constants.Wrist.INTAKE_SPEED);
+        wristIntakeMotor.set(Constants.Wrist.INTAKE_SPEED);
     }
 
     /**
@@ -40,7 +43,7 @@ public void intake() {
      * @param power power of motors from -1 to 1
      */
     public void setMotor(double power) {
-        wristIntakeMotor.set(ControlMode.PercentOutput, (invertForCone ? -1 : 1) * power);
+        wristIntakeMotor.set((invertForCone ? -1 : 1) * power);
     }
 
     /**
@@ -49,7 +52,7 @@ public void setMotor(double power) {
      * @param power power of motors from -1 to 1
      */
     public void setMotorRaw(double power) {
-        wristIntakeMotor.set(ControlMode.PercentOutput, power);
+        wristIntakeMotor.set(power);
     }
 
     /**
@@ -64,7 +67,8 @@ public void panic() {
      * Get if motor is stalling by checking if the stator current exceeds a constant
      */
     public boolean getVoltageSpike(boolean passedTime) {
-        return wristIntakeMotor.getStatorCurrent() > Constants.Wrist.STALL_CURRENT;
+        return wristIntakeMotor.getStatorCurrent()
+            .getValueAsDouble() > Constants.Wrist.STALL_CURRENT;
     }
 
     public void setInvert(boolean invert) {
diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
new file mode 100644
index 00000000..d5ec2df2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java
@@ -0,0 +1,276 @@
+package frc.robot.subsystems.swerve;
+
+import java.util.Optional;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
+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.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.lib.util.swerve.SwerveModule;
+import frc.robot.Constants;
+
+/**
+ * Swerve Subsystem
+ */
+public class Swerve extends SubsystemBase {
+    public SwerveDriveOdometry swerveOdometry;
+    public SwerveModule[] swerveMods;
+    public AHRS gyro = new AHRS(Constants.Swerve.navXID);
+    private double fieldOffset = gyro.getYaw();
+    private SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged();
+    private SwerveIO swerveIO;
+    private boolean hasInitialized = false;
+
+    /**
+     * Swerve Subsystem
+     */
+    public Swerve(SwerveIO swerveIO) {
+        this.swerveIO = swerveIO;
+        swerveIO.updateInputs(inputs);
+        swerveMods = new SwerveModule[] {
+            swerveIO.createSwerveModule(0, Constants.Swerve.Mod0.DRIVE_MOTOR_ID,
+                Constants.Swerve.Mod0.ANGLE_MOTOR_ID, Constants.Swerve.Mod0.CAN_CODER_ID,
+                Constants.Swerve.Mod0.ANGLE_OFFSET),
+            swerveIO.createSwerveModule(1, Constants.Swerve.Mod1.DRIVE_MOTOR_ID,
+                Constants.Swerve.Mod1.ANGLE_MOTOR_ID, Constants.Swerve.Mod1.CAN_CODER_ID,
+                Constants.Swerve.Mod1.ANGLE_OFFSET),
+            swerveIO.createSwerveModule(2, Constants.Swerve.Mod2.DRIVE_MOTOR_ID,
+                Constants.Swerve.Mod2.ANGLE_MOTOR_ID, Constants.Swerve.Mod2.CAN_CODER_ID,
+                Constants.Swerve.Mod2.ANGLE_OFFSET),
+            swerveIO.createSwerveModule(3, Constants.Swerve.Mod3.DRIVE_MOTOR_ID,
+                Constants.Swerve.Mod3.ANGLE_MOTOR_ID, Constants.Swerve.Mod3.CAN_CODER_ID,
+                Constants.Swerve.Mod3.ANGLE_OFFSET)};
+
+        swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.SWERVE_KINEMATICS, getGyroYaw(),
+            getModulePositions());
+
+        AutoBuilder.configureHolonomic(this::getPose, this::resetOdometry, this::getChassisSpeeds,
+            this::setModuleStates, Constants.Swerve.pathFollowerConfig, () -> shouldFlipPath(),
+            this);
+    }
+
+    /**
+     * Tele-Op Drive method
+     *
+     * @param translation The magnitude in XY
+     * @param rotation The magnitude in rotation
+     * @param fieldRelative Whether or not field relative
+     * @param isOpenLoop Whether or not Open or Closed Loop
+     */
+    public void drive(Translation2d translation, double rotation, boolean fieldRelative,
+        boolean isOpenLoop) {
+        ChassisSpeeds chassisSpeeds = fieldRelative
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(), translation.getY(),
+                rotation, getFieldRelativeHeading())
+            : new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
+
+        setModuleStates(chassisSpeeds);
+    }
+
+    /**
+     * Set Swerve Module States
+     *
+     * @param desiredStates Array of desired states
+     */
+    public void setModuleStates(SwerveModuleState[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.MAX_SPEED);
+        Logger.recordOutput("Swerve/DesiredStates", desiredStates);
+        for (SwerveModule mod : swerveMods) {
+            mod.setDesiredState(desiredStates[mod.moduleNumber], false);
+        }
+    }
+
+    /**
+     * Sets swerve module states using Chassis Speeds.
+     *
+     * @param chassisSpeeds The desired Chassis Speeds
+     */
+    public void setModuleStates(ChassisSpeeds chassisSpeeds) {
+        ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(chassisSpeeds, 0.02);
+        SwerveModuleState[] swerveModuleStates =
+            Constants.Swerve.SWERVE_KINEMATICS.toSwerveModuleStates(targetSpeeds);
+        setModuleStates(swerveModuleStates);
+    }
+
+    /**
+     * Get current Chassis Speeds
+     *
+     * @return The current {@link ChassisSpeeds}
+     */
+    public ChassisSpeeds getChassisSpeeds() {
+        return Constants.Swerve.SWERVE_KINEMATICS.toChassisSpeeds(getModuleStates());
+    }
+
+    /**
+     * Get Swerve Module States
+     *
+     * @return Array of Swerve Module States
+     */
+    @AutoLogOutput(key = "Swerve/CurrentStates")
+    public SwerveModuleState[] getModuleStates() {
+        SwerveModuleState[] states = new SwerveModuleState[4];
+        for (SwerveModule mod : swerveMods) {
+            states[mod.moduleNumber] = mod.getState();
+        }
+        return states;
+    }
+
+    /**
+     * Get Swerve Module Positions
+     *
+     * @return Array of Swerve Module Positions
+     */
+    public SwerveModulePosition[] getModulePositions() {
+        SwerveModulePosition[] positions = new SwerveModulePosition[4];
+        for (SwerveModule mod : swerveMods) {
+            positions[mod.moduleNumber] = mod.getPosition();
+        }
+        return positions;
+    }
+
+    /**
+     * Get Position on field from Odometry
+     *
+     * @return Pose2d on the field
+     */
+    @AutoLogOutput(key = "Odometry/Robot")
+    public Pose2d getPose() {
+        return swerveOdometry.getPoseMeters();
+    }
+
+    /**
+     * Set the position on the field with given Pose2d
+     *
+     * @param pose Pose2d to set
+     */
+    public void resetOdometry(Pose2d pose) {
+        swerveOdometry.resetPosition(getGyroYaw(), getModulePositions(), pose);
+    }
+
+    /**
+     * Get Rotation of robot from odometry
+     *
+     * @return Heading of robot relative to the field as {@link Rotation2d}
+     */
+    public Rotation2d getHeading() {
+        return getPose().getRotation();
+    }
+
+    /**
+     * Get Rotation from the gyro
+     *
+     * @return Current rotation/yaw of gyro as {@link Rotation2d}
+     */
+    public Rotation2d getGyroYaw() {
+        float yaw = gyro.getYaw();
+        return (Constants.Swerve.INVERT_GYRO) ? Rotation2d.fromDegrees(-yaw)
+            : Rotation2d.fromDegrees(yaw);
+    }
+
+    /**
+     * Gets the roll (pitch) degree from swerve modules.
+     */
+    public float getRoll() {
+        return inputs.roll;
+    }
+
+    /**
+     * Get Field Relative Heading
+     *
+     * @return The current field relative heading in {@link Rotation2d}
+     */
+    public Rotation2d getFieldRelativeHeading() {
+        return Rotation2d.fromDegrees(getGyroYaw().getDegrees() - fieldOffset);
+    }
+
+    /**
+     * Resets the gyro field relative driving offset
+     */
+    public void resetFieldRelativeOffset() {
+        // gyro.zeroYaw();
+        fieldOffset = getGyroYaw().getDegrees();
+    }
+
+    /**
+     * Reset all modules to their front facing position
+     */
+    public void resetModulesToAbsolute() {
+        for (SwerveModule mod : swerveMods) {
+            mod.resetToAbsolute();
+        }
+    }
+
+    @Override
+    public void periodic() {
+        swerveOdometry.update(getGyroYaw(), getModulePositions());
+        swerveIO.updateInputs(inputs);
+        Logger.processInputs("Swerve", inputs);
+        for (SwerveModule mod : swerveMods) {
+            mod.periodic();
+            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder",
+                mod.getCANcoder().getDegrees());
+            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle",
+                mod.getPosition().angle.getDegrees());
+            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity",
+                mod.getState().speedMetersPerSecond);
+        }
+    }
+
+    /**
+     * Sets motors to 0 or inactive.
+     */
+    public void setMotorsZero() {
+        System.out.println("Setting Zero!!!!!!");
+        setModuleStates(new ChassisSpeeds(0, 0, 0));
+    }
+
+    /**
+     * Make an X pattern with the wheels
+     */
+    public void wheelsIn() {
+        swerveMods[0].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(45)), false);
+        swerveMods[1].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(135)), false);
+        swerveMods[2].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-45)), false);
+        swerveMods[3].setDesiredState(new SwerveModuleState(2, Rotation2d.fromDegrees(-135)),
+            false);
+        this.setMotorsZero();
+    }
+
+    /**
+     * Determine whether or not to flight the auto path
+     *
+     * @return True if flip path to Red Alliance, False if Blue
+     */
+    public boolean shouldFlipPath() {
+        Optional<Alliance> ally = DriverStation.getAlliance();
+        if (ally.isPresent()) {
+            return ally.get() == Alliance.Red;
+        }
+        return false;
+    }
+
+    /**
+     * Reset April Tag position initialization
+     */
+    public void resetInitialized() {
+        this.hasInitialized = false;
+    }
+
+    /**
+     * Zero the Gyro
+     */
+    public void zeroGyro() {
+        swerveIO.zeroGyro();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
new file mode 100644
index 00000000..25b98ac0
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java
@@ -0,0 +1,39 @@
+package frc.robot.subsystems.swerve;
+
+import org.littletonrobotics.junction.AutoLog;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.lib.util.swerve.SwerveModule;
+import frc.lib.util.swerve.SwerveModuleIO;
+
+/** IO Class for Swerve */
+public interface SwerveIO {
+
+    /** Inputs Class for Swerve */
+    @AutoLog
+    public static class SwerveInputs {
+        public float yaw;
+        public float roll;
+    }
+
+    public default void updateInputs(SwerveInputs inputs) {}
+
+    /**
+     * Create a Swerve Module for the respective IO Layer
+     *
+     * @param moduleNumber The Module Number
+     * @param driveMotorID The Drive Motor CAN ID
+     * @param angleMotorID The Angle Motor CAN ID
+     * @param cancoderID The CANCode CAN ID
+     * @param angleOffset The Angle Offset in {@link Rotation2d}
+     * @return {@link SwerveModule}
+     */
+    public default SwerveModule createSwerveModule(int moduleNumber, int driveMotorID,
+        int angleMotorID, int cancoderID, Rotation2d angleOffset) {
+        return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset,
+            new SwerveModuleIO() {
+
+            });
+    }
+
+    public default void zeroGyro() {}
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
new file mode 100644
index 00000000..bd7a15eb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java
@@ -0,0 +1,42 @@
+package frc.robot.subsystems.swerve;
+
+import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.lib.util.swerve.SwerveModule;
+import frc.lib.util.swerve.SwerveModuleReal;
+import frc.robot.Constants;
+
+/** Real Class for Swerve */
+public class SwerveReal implements SwerveIO {
+
+    private AHRS gyro = new AHRS(Constants.Swerve.navXID);
+
+    /** Real Swerve Initializer */
+    public SwerveReal() {
+
+    }
+
+    /**
+     * Update Inputs
+     */
+    @Override
+    public void updateInputs(SwerveInputs inputs) {
+        inputs.yaw = gyro.getYaw();
+        inputs.roll = gyro.getRoll();
+    }
+
+    @Override
+    public SwerveModule createSwerveModule(int moduleNumber, int driveMotorID, int angleMotorID,
+        int cancoderID, Rotation2d angleOffset) {
+        return new SwerveModule(moduleNumber, driveMotorID, angleMotorID, cancoderID, angleOffset,
+            new SwerveModuleReal(moduleNumber, driveMotorID, angleMotorID, cancoderID,
+                angleOffset));
+    }
+
+    /**
+     * Zero the Gyro
+     */
+    public void zeroGyro() {
+        gyro.zeroYaw();
+    }
+}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
new file mode 100644
index 00000000..bdbd93d2
--- /dev/null
+++ b/vendordeps/AdvantageKit.json
@@ -0,0 +1,42 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "3.0.2",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2024",
+    "mavenUrls": [],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit.junction",
+            "artifactId": "wpilib-shim",
+            "version": "3.0.2"
+        },
+        {
+            "groupId": "org.littletonrobotics.akit.junction",
+            "artifactId": "junction-core",
+            "version": "3.0.2"
+        },
+        {
+            "groupId": "org.littletonrobotics.akit.conduit",
+            "artifactId": "conduit-api",
+            "version": "3.0.2"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.akit.conduit",
+            "artifactId": "conduit-wpilibio",
+            "version": "3.0.2",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json
deleted file mode 100644
index 88a68dd0..00000000
--- a/vendordeps/Phoenix5.json
+++ /dev/null
@@ -1,151 +0,0 @@
-{
-    "fileName": "Phoenix5.json",
-    "name": "CTRE-Phoenix (v5)",
-    "version": "5.33.0",
-    "frcYear": 2024,
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
-    "requires": [
-        {
-            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
-            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
-            "offlineFileName": "Phoenix6.json",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-java",
-            "version": "5.33.0"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-java",
-            "version": "5.33.0"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.33.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.33.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-cpp",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixCCI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixSim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixCCISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}
\ No newline at end of file