diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json
index acab627..6b13a10 100644
--- a/.wpilib/wpilib_preferences.json
+++ b/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
 {
-    "enableCppIntellisense": false,
-    "currentLanguage": "java",
-    "projectYear": "2025beta",
-    "teamNumber": 4829
-}
\ No newline at end of file
+  "enableCppIntellisense": false,
+  "currentLanguage": "java",
+  "projectYear": "2025beta",
+  "teamNumber": 4829
+}
diff --git a/build.gradle b/build.gradle
index 6762242..1abcd7b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,43 +1,11 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
     id "com.peterabeles.gversion" version "1.10"
     id "com.diffplug.spotless" version "6.25.0"
+    id "pmd"
     id "io.freefair.lombok" version "8.4"
     id "com.google.protobuf" version "0.9.4"
-    id "pmd"
-}
-
-def grpcVersion = '1.61.0' // CURRENT_GRPC_VERSION
-def protobufVersion = '3.25.1'
-def protocVersion = protobufVersion
-
-protobuf {
-    protoc {
-        artifact = "com.google.protobuf:protoc:${protobufVersion}"
-    }
-    plugins {
-        grpc {
-            artifact = "io.grpc:protoc-gen-grpc-java:${grpcVersion}"
-        }
-    }
-    generateProtoTasks {
-        all()*.plugins {
-            grpc {}
-        }
-    }
-}
-
-sourceSets {
-    main {
-        java {
-            srcDirs 'build/generated/source/proto/main/grpc'
-            srcDirs 'build/generated/source/proto/main/java'
-        }
-        proto {
-            srcDirs "trajectory_native/proto"
-        }
-    }
 }
 
 java {
@@ -64,14 +32,25 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
-                    gcType = edu.wpi.first.gradlerio.deploy.roborio.GarbageCollectorType.G1_Base
+                    // https://www.chiefdelphi.com/t/2024-wpilib-feedback/464322/141
+                    final MAX_JAVA_HEAP_SIZE_MB = 100;
+                    jvmArgs.add("-XX:+UnlockExperimentalVMOptions")
+
+                    // Set the minimum heap size to the maximum heap size to avoid resizing
+                    jvmArgs.add("-Xmx" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    jvmArgs.add("-Xms" + MAX_JAVA_HEAP_SIZE_MB + "M")
+                    jvmArgs.add("-XX:GCTimeRatio=5")
+                    jvmArgs.add("-XX:+UseSerialGC")
+                    jvmArgs.add("-XX:MaxGCPauseMillis=50")
                 }
 
                 // Static files artifact
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
-                    predeploy << { execute 'rm -rf /home/lvuser/deploy' }
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
+                    // Change to true to delete files on roboRIO that no
+                    // longer exist in deploy directory on roboRIO
+                    deleteOldFiles = false
                 }
             }
         }
@@ -98,62 +77,15 @@ repositories {
     mavenLocal()
 }
 
-configurations.all {
-    exclude group: "edu.wpi.first.wpilibj"
-}
-
-task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
-    mainClass = "org.littletonrobotics.junction.CheckInstall"
+task(replayWatch, type: JavaExec) {
+    mainClass = "org.littletonrobotics.junction.ReplayWatch"
     classpath = sourceSets.main.runtimeClasspath
 }
-compileJava.finalizedBy checkAkitInstall
-
-
-//Check robot type when deploying
-task(checkRobot, dependsOn: "classes", type: JavaExec) {
-    main = "frc.robot.Constants"
-    classpath = sourceSets.main.runtimeClasspath
-}
-
-deployroborio.dependsOn(checkRobot)
-
-//Create commit with working changes on event branches
-task(eventDeploy) {
-    doLast {
-        if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
-
-            def branchPrefix = "event"
-            def branch = 'git branch --show-current'.execute().text.trim()
-            def commitMessage = "Update at '${new Date().toString()}'"
-
-            if (branch.startsWith(branchPrefix)) {
-                exec {
-                    workingDir(projectDir)
-                    executable 'git'
-                    args 'add', '-A'
-                }
-                exec {
-                    workingDir(projectDir)
-                    executable 'git'
-                    args 'commit', '-m', commitMessage
-                    ignoreExitValue = true
-                }
-
-                println "Committed to branch: '$branch'"
-                println "Commit message: '$commitMessage'"
-            } else {
-                println "Not on an event branch, skipping commit"
-            }
-        } else {
-            println "Not running deploy task, skipping commit"
-        }
-    }
-}
-createVersionFile.dependsOn(eventDeploy)
 
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 4.
 dependencies {
+    annotationProcessor wpi.java.deps.wpilibAnnotations()
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
@@ -171,25 +103,13 @@ dependencies {
     nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop)
     simulationRelease wpi.sim.enableRelease()
 
-    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    testRuntimeOnly 'org.junit.platform:junit-platform-launcher:1.10.2'
+    implementation "gov.nist.math:jama:1.0.3"
 
-
-    // Mockito for mocking objects in unit tests
-    testImplementation 'org.mockito:mockito-core:4.8.1'
-    testImplementation 'org.mockito:mockito-junit-jupiter:4.8.1'
-
-    implementation 'gov.nist.math:jama:1.0.3'
-
-    implementation "io.grpc:grpc-protobuf:${grpcVersion}"
-    implementation "io.grpc:grpc-services:${grpcVersion}"
-    implementation "io.grpc:grpc-stub:${grpcVersion}"
-    implementation "javax.annotation:javax.annotation-api:1.3.2"
-    runtimeOnly "io.grpc:grpc-netty-shaded:${grpcVersion}"
-    implementation "com.google.guava:guava:31.1-jre"
+    testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
+    testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
-    annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
+    annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"
 }
 
 test {
@@ -198,7 +118,12 @@ test {
 }
 
 // Simulation configuration (e.g. environment variables).
-wpi.sim.addGui().defaultEnabled = true
+//
+// The sim GUI is *disabled* by default to support running
+// AdvantageKit log replay from the command line. Set the
+// value to "true" to enable the sim GUI by default (this
+// is the standard WPILib behavior).
+wpi.sim.addGui().defaultEnabled = false
 wpi.sim.addDriverstation()
 
 // Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar')
@@ -226,22 +151,58 @@ tasks.withType(JavaCompile) {
 }
 
 // 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       = "  "
-}
+// 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       = "  "
+// }
+
+// TODO: set this shit up
+// Create commit with working changes on event branches
+// task(eventDeploy) {
+//     doLast {
+//         if (project.gradle.startParameter.taskNames.any({ it.toLowerCase().contains("deploy") })) {
+//             def branchPrefix = "event"
+//             def branch = 'git branch --show-current'.execute().text.trim()
+//             def commitMessage = "Update at '${new Date().toString()}'"
+
+//             if (branch.startsWith(branchPrefix)) {
+//                 exec {
+//                     workingDir(projectDir)
+//                     executable 'git'
+//                     args 'add', '-A'
+//                 }
+//                 exec {
+//                     workingDir(projectDir)
+//                     executable 'git'
+//                     args 'commit', '-m', commitMessage
+//                     ignoreExitValue = true
+//                 }
+
+//                 println "Committed to branch: '$branch'"
+//                 println "Commit message: '$commitMessage'"
+//             } else {
+//                 println "Not on an event branch, skipping commit"
+//             }
+//         } else {
+//             println "Not running deploy task, skipping commit"
+//         }
+//     }
+// }
+// createVersionFile.dependsOn(eventDeploy)
+
 // Spotless formatting
+project.compileJava.dependsOn(spotlessApply)
 spotless {
     enforceCheck = false
     java {
-        target fileTree('.') {
-            include '**/*.java'
-            exclude '**/build/**', '**/build-*/**'
+        target fileTree(".") {
+            include "**/*.java"
+            exclude "**/build/**", "**/build-*/**"
         }
         toggleOffOn()
         googleJavaFormat()
@@ -250,36 +211,32 @@ spotless {
         endWithNewline()
     }
     groovyGradle {
-        target fileTree('.') {
-            include '**/*.gradle'
-            exclude '**/build/**', '**/build-*/**'
+        target fileTree(".") {
+            include "**/*.gradle"
+            exclude "**/build/**", "**/build-*/**"
         }
         greclipse()
         indentWithSpaces(4)
         trimTrailingWhitespace()
         endWithNewline()
     }
-    format 'xml', {
-        target fileTree('.') {
-            include '**/*.xml'
-            exclude '**/build/**', '**/build-*/**'
+    json {
+        target fileTree(".") {
+            include "**/*.json"
+            exclude "**/build/**", "**/build-*/**"
         }
-        eclipseWtp('xml')
-        trimTrailingWhitespace()
-        indentWithSpaces(2)
-        endWithNewline()
+        gson().indentWithSpaces(2)
     }
-    format 'misc', {
-        target fileTree('.') {
-            include '**/*.md', '**/.gitignore'
-            exclude '**/build/**', '**/build-*/**'
+    format "misc", {
+        target fileTree(".") {
+            include "**/*.md", "**/.gitignore"
+            exclude "**/build/**", "**/build-*/**"
         }
         trimTrailingWhitespace()
         indentWithSpaces(2)
         endWithNewline()
     }
 }
-
 // PMD configuration
 pmd {
     ignoreFailures = true
diff --git a/simgui-ds.json b/simgui-ds.json
index 73cc713..69b1a3c 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }
diff --git a/simgui.json b/simgui.json
index 5f9d275..896ae55 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,7 +1,10 @@
 {
   "NTProvider": {
     "types": {
-      "/FMSInfo": "FMSInfo"
+      "/AdvantageKit/RealOutputs/Alerts": "Alerts",
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Alerts": "Alerts",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
     }
   },
   "NetworkTables Info": {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index eefde9d..cf81f13 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,22 +1,28 @@
 package frc.robot;
 
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
+import frc.robot.subsystems.swerve.SwerveConstants.*;
+import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
+import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
 
 public final class Constants {
 
-  public class LogPaths {
+  public static final class LogPaths {
     public static final String SYSTEM_PERFORMANCE_PATH = "SystemPerformance/";
     public static final String PHYSICS_SIMULATION_PATH = "MaplePhysicsSimulation/";
     public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/";
   }
 
-  public static final Mode currentMode = Mode.REAL;
+  public static final Mode CURRENT_MODE = Mode.SIM;
 
   public static enum Mode {
     /** Running on a real robot. */
     REAL,
+
     /** Running a physics simulator. */
     SIM,
+
     /** Replaying from a log file. */
     REPLAY
   }
@@ -41,6 +47,85 @@ public static final class HardwareConstants {
     public static final double MIN_FALCON_DEADBAND = 0.001;
   }
 
+  /**
+   * stores the constants and PID configs for chassis because we want an all-real simulation for the
+   * chassis, the numbers are required to be considerably precise
+   */
+  public class SimulationConstants {
+    /**
+     * numbers that needs to be changed to fit each robot TODO: change these numbers to match your
+     * robot
+     */
+    public static final double WHEEL_COEFFICIENT_OF_FRICTION = 0.95,
+        ROBOT_MASS_KG = 60; // with bumpers
+
+    /** TODO: change motor type to match your robot */
+    public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1),
+        STEER_MOTOR = DCMotor.getFalcon500(1);
+
+    public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2.0,
+        DRIVE_GEAR_RATIO = ModuleConstants.DRIVE_GEAR_RATIO,
+        STEER_GEAR_RATIO = 11.0,
+        TIME_ROBOT_STOP_ROTATING_SECONDS = 0.06,
+        STEER_FRICTION_VOLTAGE = 0.12,
+        DRIVE_FRICTION_VOLTAGE = ModuleConstants.DRIVE_S,
+        DRIVE_INERTIA = 0.01,
+        STEER_INERTIA = 0.01;
+
+    /* adjust current limit */
+    public static final double DRIVE_CURRENT_LIMIT = ModuleConstants.DRIVE_STATOR_LIMIT;
+    // public static final double STEER_CURRENT_LIMIT = ModuleConstants.ST;
+
+    /* equations that calculates some constants for the simulator (don't modify) */
+    private static final double GRAVITY_CONSTANT = 9.81;
+    public static final double DRIVE_BASE_RADIUS = DriveConstants.MODULE_TRANSLATIONS[0].getNorm(),
+        /* friction_force = normal_force * coefficient_of_friction */
+        MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION,
+        MAX_FRICTION_FORCE_PER_MODULE =
+            MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / DriveConstants.MODULE_TRANSLATIONS.length,
+        /* force = torque / distance */
+        MAX_PROPELLING_FORCE_NEWTONS =
+            DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS,
+        /* floor_speed = wheel_angular_velocity * wheel_radius */
+        CHASSIS_MAX_VELOCITY =
+            DRIVE_MOTOR.freeSpeedRadPerSec
+                / DRIVE_GEAR_RATIO
+                * WHEEL_RADIUS_METERS, // calculate using choreo
+        CHASSIS_MAX_ACCELERATION_MPS_SQ =
+            Math.min(
+                MAX_FRICTION_ACCELERATION, // cannot exceed max friction
+                MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG),
+        CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS,
+        CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ =
+            CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS,
+        CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION =
+            CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC / TIME_ROBOT_STOP_ROTATING_SECONDS;
+
+    /* for collision detection in simulation */
+    public static final double BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5),
+        BUMPER_LENGTH_METERS = Units.inchesToMeters(36),
+        /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */
+        BUMPER_COEFFICIENT_OF_FRICTION = 0.75,
+        /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */
+        BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08;
+
+    /* Gyro Sim */
+    public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100;
+    public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2);
+    /*
+     * https://store.ctr-electronics.com/pigeon-2/
+     * for a well-installed one with vibration reduction, only 0.4 degree
+     * but most teams just install it directly on the rigid chassis frame (including my team :D)
+     * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second
+     * which is the average velocity during normal swerve-circular-offense
+     * */
+    public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2);
+    public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60);
+
+    public static final int SIMULATION_TICKS_IN_1_PERIOD = 5;
+    public static final double SIMULATED_PERIOD_SECONDS = 0.02;
+  }
+
   /**
    * This is where constants used to describe the game's field go. This will have the dimensions of
    * the field, but also the coordinates of obstacles, game pieces, or other places of interest.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 41a77a0..5dd899d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
 package frc.robot;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc.robot.Constants.Mode;
+import frc.robot.extras.simulation.field.SimulatedField;
+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;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -23,62 +19,82 @@
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
   private RobotContainer m_robotContainer;
-  private static final Mode JAVA_SIM_MODE = Mode.SIM;
-  public static final Mode CURRENT_ROBOT_MODE = isReal() ? Mode.REAL : JAVA_SIM_MODE;
 
-  /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
-   */
-  @Override
-  public void robotInit() {
-    Logger.recordMetadata("ProjectName", "MyProject"); // Set a metadata value
-
-    if (isReal()) {
-      // Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-      Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
-      // new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
-    } else {
-      setUseTiming(false); // Run as fast as possible
-      // String logPath = LogFileUtil.findReplayLog(); // Pull the replay log from AdvantageScope
-      // (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
+  public Robot() {
+    // 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);
+
+    // This tells you if you have uncommitted changes in your project
+    switch (BuildConstants.DIRTY) {
+      case 0:
+        Logger.recordMetadata("GitDirty", "All changes committed");
+        break;
+      case 1:
+        Logger.recordMetadata("GitDirty", "Uncomitted changes");
+        break;
+      default:
+        Logger.recordMetadata("GitDirty", "Unknown");
+        break;
     }
 
-    // Logger.disableDeterministicTimestamps() // See "Deterministic Timestamps" in the
-    // "Understanding Data Flow" page
-    Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
-    // be added.
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-    // autonomous chooser on the dashboard.
-    m_robotContainer = new RobotContainer();
-    //    uncomment the line below if there is a USB camera plugged into the RoboRIO
-    //    CameraServer.startAutomaticCapture();
+    // Set up data receivers & replay source
+    switch (Constants.CURRENT_MODE) {
+      case REAL:
+        // Running on a real robot, log to a USB stick ("/U/logs")
+        Logger.addDataReceiver(new WPILOGWriter());
+        // Gets data from network tables
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case SIM:
+        // Running a physics simulator, log to NT
+        Logger.addDataReceiver(new NT4Publisher());
+        break;
+
+      case REPLAY:
+        // Replaying a log, set up replay source
+        setUseTiming(false); // Run as fast as possible
+        String logPath = LogFileUtil.findReplayLog();
+        Logger.setReplaySource(new WPILOGReader(logPath));
+        Logger.addDataReceiver(new WPILOGWriter(LogFileUtil.addPathSuffix(logPath, "_sim")));
+        break;
+    }
+
+    // Start AdvantageKit logger
+    Logger.start();
 
+    // Instantiate our RobotContainer. This will perform all our button bindings,
+    // and put our autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
   }
 
-  /**
-   * This function is called every robot packet, no matter the mode. Use this for items like
-   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
-   * SmartDashboard integrated updating.
-   */
+  /** This function is called periodically during all modes. */
   @Override
   public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
-    // block in order for anything in the Command-based framework to work.
+    // TODO: test this!
+    // Switch thread to high priority to improve loop timing
+    // Threads.setCurrentThreadPriority(true, 99);
+
+    // Runs the Scheduler. This is responsible for polling buttons, adding
+    // newly-scheduled commands, running already-scheduled commands, removing
+    // finished or interrupted commands, and running subsystem periodic() methods.
+    // This must be called from the robot's periodic block in order for anything in
+    // the Command-based framework to work.
     CommandScheduler.getInstance().run();
+
+    // Return to normal thread priority
+    // Threads.setCurrentThreadPriority(false, 10);
   }
 
-  /** This function is called once each time the robot enters Disabled mode. */
+  /** This function is called once when the robot is disabled. */
   @Override
   public void disabledInit() {}
 
+  /** This function is called periodically when disabled. */
   @Override
   public void disabledPeriodic() {}
 
@@ -97,6 +113,7 @@ public void autonomousInit() {
   @Override
   public void autonomousPeriodic() {}
 
+  /** This function is called once when teleop is enabled. */
   @Override
   public void teleopInit() {
     // This makes sure that the autonomous stops running when
@@ -106,13 +123,15 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    // m_robotContainer.teleopInit();
+
+    m_robotContainer.teleopInit();
   }
 
   /** This function is called periodically during operator control. */
   @Override
   public void teleopPeriodic() {}
 
+  /** This function is called once when test mode is enabled. */
   @Override
   public void testInit() {
     // Cancels all running commands at the start of test mode.
@@ -123,6 +142,14 @@ public void testInit() {
   @Override
   public void testPeriodic() {}
 
+  /** This function is called once when the robot is first started up. */
   @Override
-  public void simulationPeriodic() {}
+  public void simulationInit() {}
+
+  /** This function is called periodically whilst in simulation. */
+  @Override
+  public void simulationPeriodic() {
+    SimulatedField.getInstance().simulationPeriodic();
+    m_robotContainer.updateFieldSimAndDisplay();
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index de2c9d0..0f249f0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,20 +1,328 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// 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 frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.SimulationConstants;
+import frc.robot.commands.drive.DriveCommand;
+import frc.robot.extras.simulation.field.SimulatedField;
+import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
+import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
+import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
+import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
+import frc.robot.extras.util.JoystickUtil;
+import frc.robot.subsystems.swerve.SwerveConstants;
+import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
+import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
+import frc.robot.subsystems.swerve.SwerveDrive;
+import frc.robot.subsystems.swerve.gyro.GyroInterface;
+import frc.robot.subsystems.swerve.gyro.PhysicalGyro;
+import frc.robot.subsystems.swerve.gyro.SimulatedGyro;
+import frc.robot.subsystems.swerve.module.ModuleInterface;
+import frc.robot.subsystems.swerve.module.PhysicalModule;
+import frc.robot.subsystems.swerve.module.SimulatedModule;
+import frc.robot.subsystems.vision.PhysicalVision;
+import frc.robot.subsystems.vision.SimulatedVision;
+import frc.robot.subsystems.vision.VisionInterface;
+import frc.robot.subsystems.vision.VisionSubsystem;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.Logger;
 
 public class RobotContainer {
+
+  private final VisionSubsystem visionSubsystem;
+  private final SwerveDrive swerveDrive;
+  private final CommandXboxController operatorController = new CommandXboxController(1);
+  // private final Indexer indexer = new Indexer(new IndexerIOTalonFX());
+  // private final Intake intake = new Intake(new IntakeIOTalonFX());
+  // private final Pivot pivot = new Pivot(new PivotIOTalonFX());
+  // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX());
+  private final CommandXboxController driverController = new CommandXboxController(0);
+
+  // Simulation, we store them here in the robot container
+  // private final SimulatedField simulatedArena;
+  private final SwerveDriveSimulation swerveDriveSimulation;
+  private final GyroSimulation gyroSimulation;
+
+  // Subsystems
+  // private final XboxController driverController = new XboxController(0);
+
   public RobotContainer() {
-    configureBindings();
+    switch (Constants.CURRENT_MODE) {
+      case REAL -> {
+        /* Real robot, instantiate hardware IO implementations */
+
+        /* Disable Simulations */
+        // this.simulatedArena = null;
+        this.gyroSimulation = null;
+        this.swerveDriveSimulation = null;
+
+        swerveDrive =
+            new SwerveDrive(
+                new PhysicalGyro(),
+                new PhysicalModule(SwerveConstants.moduleConfigs[0]),
+                new PhysicalModule(SwerveConstants.moduleConfigs[1]),
+                new PhysicalModule(SwerveConstants.moduleConfigs[2]),
+                new PhysicalModule(SwerveConstants.moduleConfigs[3]));
+        visionSubsystem = new VisionSubsystem(new PhysicalVision());
+      }
+
+      case SIM -> {
+        /* Sim robot, instantiate physics sim IO implementations */
+
+        /* create simulations */
+        /* create simulation for pigeon2 IMU (different IMUs have different measurement erros) */
+        this.gyroSimulation = GyroSimulation.createNavX2();
+        /* create a swerve drive simulation */
+        this.swerveDriveSimulation =
+            new SwerveDriveSimulation(
+                SimulationConstants.ROBOT_MASS_KG,
+                DriveConstants.TRACK_WIDTH,
+                DriveConstants.WHEEL_BASE,
+                DriveConstants.TRACK_WIDTH + .2,
+                DriveConstants.WHEEL_BASE + .2,
+                SwerveModuleSimulation.getModule(
+                    DCMotor.getFalcon500(1),
+                    DCMotor.getFalcon500(1),
+                    60,
+                    WHEEL_GRIP.TIRE_WHEEL,
+                    ModuleConstants.DRIVE_GEAR_RATIO),
+                gyroSimulation,
+                new Pose2d(3, 3, new Rotation2d()));
+        SimulatedField.getInstance().addDriveTrainSimulation(swerveDriveSimulation);
+        swerveDrive =
+            new SwerveDrive(
+                new SimulatedGyro(
+                    gyroSimulation), // SimulatedGyro is a wrapper around gyro simulation, that
+                // reads
+                // the simulation result
+                /* SimulatedModule are edited such that they also wraps around module simulations */
+                new SimulatedModule(swerveDriveSimulation.getModules()[0]),
+                new SimulatedModule(swerveDriveSimulation.getModules()[1]),
+                new SimulatedModule(swerveDriveSimulation.getModules()[2]),
+                new SimulatedModule(swerveDriveSimulation.getModules()[3]));
+
+        visionSubsystem =
+            new VisionSubsystem(
+                new SimulatedVision(() -> swerveDriveSimulation.getSimulatedDriveTrainPose()));
+
+        SimulatedField.getInstance().resetFieldForAuto();
+        resetFieldAndOdometryForAuto(
+            new Pose2d(1.3980597257614136, 5.493067741394043, Rotation2d.fromRadians(3.1415)));
+      }
+
+      default -> {
+        visionSubsystem = new VisionSubsystem(new VisionInterface() {});
+        /* Replayed robot, disable IO implementations */
+
+        /* physics simulations are also not needed */
+        this.gyroSimulation = null;
+        this.swerveDriveSimulation = null;
+        // this.simulatedArena = null;
+        swerveDrive =
+            new SwerveDrive(
+                new GyroInterface() {},
+                new ModuleInterface() {},
+                new ModuleInterface() {},
+                new ModuleInterface() {},
+                new ModuleInterface() {});
+      }
+    }
+  }
+
+  private void resetFieldAndOdometryForAuto(Pose2d robotStartingPoseAtBlueAlliance) {
+    final Pose2d startingPose = robotStartingPoseAtBlueAlliance;
+
+    if (swerveDriveSimulation != null) {
+      swerveDriveSimulation.setSimulationWorldPose(startingPose);
+      SimulatedField.getInstance().resetFieldForAuto();
+      updateFieldSimAndDisplay();
+    }
+
+    // swerveDrive.periodic();
+    swerveDrive.setPose(startingPose);
+  }
+
+  public void teleopInit() {
+    configureButtonBindings();
   }
 
-  private void configureBindings() {}
+  // public void intakeCallback(boolean hasNote) {
+  //   if (hasNote) {
+  //     driverController.setRumble(RumbleType.kBothRumble, 0.1);
+  //     operatorController.setRumble(RumbleType.kBothRumble, 1);
+  //   } else {
+  //     driverController.setRumble(RumbleType.kBothRumble, 0);
+  //     operatorController.setRumble(RumbleType.kBothRumble, 0);
+  //   }
+  // }
+  private void configureButtonBindings() {
+    DoubleSupplier driverLeftStickX = driverController::getLeftX;
+    DoubleSupplier driverLeftStickY = driverController::getLeftY;
+    DoubleSupplier driverRightStickX = driverController::getRightX;
+    DoubleSupplier driverLeftStick[] =
+        new DoubleSupplier[] {
+          () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[0],
+          () -> JoystickUtil.modifyAxisPolar(driverLeftStickX, driverLeftStickY, 3)[1]
+        };
+
+    DoubleSupplier operatorLeftStickX = operatorController::getLeftX;
+    DoubleSupplier operatorRightStickY = operatorController::getRightY;
+
+    Trigger driverRightBumper = new Trigger(driverController.rightBumper());
+    Trigger driverRightDirectionPad = new Trigger(driverController.pov(90));
+    Trigger driverDownDirectionPad = new Trigger(driverController.pov(180));
+    Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270));
+
+    // // autodrive
+    // Trigger driverAButton = new Trigger(driverController::getAButton);
+    // lol whatever
+    // // intake
+    // Trigger operatorLeftTrigger = new Trigger(()->operatorController.getLeftTriggerAxis() > 0.2);
+    // Trigger operatorLeftBumper = new Trigger(operatorController::getLeftBumper);
+    // // amp and speaker
+    // Trigger operatorBButton = new Trigger(operatorController::getBButton);
+    // Trigger operatorRightBumper = new Trigger(operatorController::getRightBumper);
+    // Trigger operatorRightTrigger = new Trigger(()->operatorController.getRightTriggerAxis() >
+    // 0.2);
+    // Trigger driverRightTrigger = new Trigger(()->driverController.getRightTriggerAxis() > 0.2);
+
+    // // manual pivot and intake rollers
+    // Trigger operatorAButton = new Trigger(operatorController::getAButton);
+    // Trigger operatorXButton = new Trigger(operatorController::getXButton);
+    // Trigger operatorYButton = new Trigger(operatorController::getYButton);
+    // DoubleSupplier operatorRightStickY = operatorController::getRightY;
+    // // unused
+    // Trigger operatorUpDirectionPad = new Trigger(()->operatorController.getPOV() == 0);
+    // Trigger operatorLeftDirectionPad = new Trigger(()->operatorController.getPOV() == 270);
+    // Trigger operatorDownDirectionPad = new Trigger(()->operatorController.getPOV() == 180);
+    // Trigger driverLeftTrigger = new Trigger(()->driverController.getLeftTriggerAxis() > 0.2);
+    Trigger driverLeftBumper = new Trigger(driverController.leftBumper());
+    // Trigger driverBButton = new Trigger(driverController::getBButton);
+    // Trigger driverYButton = new Trigger(driverController::getYButton);
+    // DoubleSupplier operatorLeftStickY = operatorController::getLeftY;
+
+    // //DRIVER BUTTONS
+
+    // // driving
+
+    Command driveCommand =
+        new DriveCommand(
+            swerveDrive,
+            visionSubsystem,
+            driverLeftStick[1],
+            driverLeftStick[0],
+            () -> JoystickUtil.modifyAxis(driverRightStickX, 3),
+            () -> !driverRightBumper.getAsBoolean(),
+            () -> driverLeftBumper.getAsBoolean());
+    swerveDrive.setDefaultCommand(driveCommand);
+
+    // // shooterSubsystem.setDefaultCommand(new FlywheelSpinUpAuto(shooterSubsystem,
+    // visionSubsystem));
+
+    // driverLeftTrigger.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem, false, ledSubsystem, this::intakeCallback));
+    // driverLeftTrigger.whileFalse(new TowerIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem, false, ledSubsystem, this::intakeCallback).withTimeout(0.3));
+    // // Amp Lineup
+    // driverAButton.whileTrue(new AutoAlignWithAmp(swerveDrive, visionSubsystem));
+    // // Spinup for shoot
+    // driverRightTrigger.whileTrue(new SpinUpForSpeaker(swerveDrive, shooterSubsystem,
+    // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper,
+    // ledSubsystem));
+
+    // // driverLeftBumper.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem,
+    // pivotSubsystem, visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper,
+    // ledSubsystem));
+    // // driverRightTrigger.whileTrue(new ShootWhileMove(swerveDrive, shooterSubsystem,
+    // pivotSubsystem, visionSubsystem, driverLeftStick, driverYButton, ledSubsystem));
+
+    // // Resets the robot angle in the odometry, factors in which alliance the robot is on
+    driverRightDirectionPad.onTrue(
+        new InstantCommand(
+            () ->
+                swerveDrive.setPose(
+                    new Pose2d(
+                        swerveDrive.getPose().getX(),
+                        swerveDrive.getPose().getY(),
+                        Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())))));
+    driverController
+        .x()
+        .onTrue(
+            new InstantCommand(
+                () -> swerveDrive.setPose(swerveDriveSimulation.getSimulatedDriveTrainPose())));
+    // // // Reset robot odometry based on vision pose measurement from april tags
+    driverLeftDirectionPad.onTrue(
+        new InstantCommand(() -> swerveDrive.setPose(visionSubsystem.getLastSeenPose())));
+    // // driverLeftDpad.onTrue(new InstantCommand(() -> swerveDrive.resetOdometry(new
+    // Pose2d(15.251774787902832, 5.573054313659668, Rotation2d.fromRadians(3.14159265)))));
+    // // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem,
+    // visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper, ledSubsystem));
+
+    // // driverXButton.
+    // driverBButton.whileTrue(new ShootPass(swerveDrive, shooterSubsystem, pivotSubsystem,
+    // visionSubsystem, driverLeftStickY, operatorLeftStickY, driverYButton, ledSubsystem));
+    // // driverDownDirectionPad.whileTrue(new IntakeFromShooter(shooterSubsystem,
+    // intakeSubsystem));
+    // // driverYButton.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem, pivotSubsystem,
+    // visionSubsystem, driverLeftStickX, operatorLeftStickY, driverRightBumper, ledSubsystem));
+    // // OPERATOR BUTTONS
+
+    // // speaker
+    // operatorRightTrigger.whileTrue(new ShootSpeaker(swerveDrive, shooterSubsystem,
+    // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightBumper,
+    // ledSubsystem));
+    // // amp
+    // operatorRightBumper.whileTrue(new ShootAmp(shooterSubsystem, pivotSubsystem, ledSubsystem,
+    // operatorBButton));
+    // // fender shot
+    // operatorUpDirectionPad.whileTrue(new SubwooferShot(swerveDrive, shooterSubsystem,
+    // pivotSubsystem, visionSubsystem, driverLeftStickX, driverLeftStickY, driverRightStickX,
+    // driverRightBumper, ledSubsystem));
+    // // intake (aka SUCC_BUTTON)
+    // operatorLeftTrigger.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem, false, ledSubsystem, this::intakeCallback));
+    // operatorLeftTrigger.whileFalse(new TowerIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem, false, ledSubsystem, this::intakeCallback).withTimeout(0.2));
+    // // outtake (aka UNSUCC_BUTTON)
+    // operatorLeftBumper.whileTrue(new TowerIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem, true, ledSubsystem, this::intakeCallback));
+    // // manual pivot (possible climb, unlikely)
+    // operatorAButton.whileTrue(new ManualPivot(pivotSubsystem,
+    // ()->modifyAxisCubed(operatorRightStickY)));
+    // operatorDownDirectionPad.whileTrue(new ManualPivot(pivotSubsystem, ()->-0.2));
+    // // manual rollers
+    // operatorYButton.whileTrue(new ManualIntake(intakeSubsystem, true));
+    // operatorXButton.whileTrue(new ManualIntake(intakeSubsystem, false));
+
+    // operatorBButton.onTrue(new StopShooterAndIntake(intakeSubsystem, pivotSubsystem,
+    // shooterSubsystem));
+  }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    // Resets the pose factoring in the robot side
+    // This is just a failsafe, pose should be reset at the beginning of auto
+    swerveDrive.setPose(
+        new Pose2d(
+            swerveDrive.getPose().getX(),
+            swerveDrive.getPose().getY(),
+            Rotation2d.fromDegrees(swerveDrive.getAllianceAngleOffset())));
+    // return autoChooser.getSelected();
+    // return new DriveForwardAndBack(swerveDrive);
+    return null;
+  }
+
+  public void updateFieldSimAndDisplay() {
+    if (swerveDriveSimulation == null) return;
+    Logger.recordOutput(
+        "FieldSimulation/RobotPosition", swerveDriveSimulation.getSimulatedDriveTrainPose());
+    Logger.recordOutput(
+        "FieldSimulation/Notes",
+        SimulatedField.getInstance().getGamePiecesByType("Note").toArray(Pose3d[]::new));
   }
 }
diff --git a/src/main/java/frc/robot/commands/drive/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java
index 4d1b522..83b1ac9 100644
--- a/src/main/java/frc/robot/commands/drive/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java
@@ -2,7 +2,7 @@
 
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveDrive;
-import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.VisionSubsystem;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
@@ -29,7 +29,7 @@ public class DriveCommand extends DriveCommandBase {
    */
   public DriveCommand(
       SwerveDrive driveSubsystem,
-      Vision visionSubsystem,
+      VisionSubsystem visionSubsystem,
       DoubleSupplier leftJoystickY,
       DoubleSupplier leftJoystickX,
       DoubleSupplier rightJoystickX,
diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
index ea557e1..b8136a3 100644
--- a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
+++ b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
@@ -6,8 +6,9 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.extras.interpolators.MultiLinearInterpolator;
 import frc.robot.subsystems.swerve.SwerveDrive;
-import frc.robot.subsystems.vision.Vision;
 import frc.robot.subsystems.vision.VisionConstants;
+import frc.robot.subsystems.vision.VisionConstants.Limelight;
+import frc.robot.subsystems.vision.VisionSubsystem;
 
 public abstract class DriveCommandBase extends Command {
 
@@ -16,7 +17,7 @@ public abstract class DriveCommandBase extends Command {
   private final MultiLinearInterpolator twoAprilTagLookupTable =
       new MultiLinearInterpolator(VisionConstants.TWO_APRIL_TAG_LOOKUP_TABLE);
 
-  private final Vision vision;
+  private final VisionSubsystem vision;
   private final SwerveDrive swerveDrive;
 
   private double lastTimeStampSeconds = 0;
@@ -27,7 +28,7 @@ public abstract class DriveCommandBase extends Command {
    * @param driveSubsystem The subsystem for the swerve drive
    * @param vision The subsystem for vision measurements
    */
-  public DriveCommandBase(SwerveDrive swerveDrive, Vision vision) {
+  public DriveCommandBase(SwerveDrive swerveDrive, VisionSubsystem vision) {
     this.swerveDrive = swerveDrive;
     this.vision = vision;
     // It is important that you do addRequirements(driveSubsystem, vision) in whatever
@@ -40,12 +41,12 @@ public void execute() {
     swerveDrive.addPoseEstimatorSwerveMeasurement();
     vision.setHeadingInfo(
         swerveDrive.getPose().getRotation().getDegrees(), swerveDrive.getGyroRate());
-    calculatePoseFromLimelight(VisionConstants.SHOOTER_LIMELIGHT_NUMBER);
-    calculatePoseFromLimelight(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER);
-    calculatePoseFromLimelight(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER);
+    calculatePoseFromLimelight(Limelight.SHOOTER);
+    calculatePoseFromLimelight(Limelight.FRONT_LEFT);
+    calculatePoseFromLimelight(Limelight.FRONT_RIGHT);
   }
 
-  public void calculatePoseFromLimelight(int limelightNumber) {
+  public void calculatePoseFromLimelight(Limelight limelightNumber) {
     double currentTimeStampSeconds = lastTimeStampSeconds;
 
     // Updates the robot's odometry with april tags
diff --git a/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java b/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java
deleted file mode 100644
index c0f62f1..0000000
--- a/src/main/java/frc/robot/extras/debug/LoggedTunableNumber.java
+++ /dev/null
@@ -1,122 +0,0 @@
-// Copyright (c) 2024 FRC 6328
-// http://github.com/Mechanical-Advantage
-//
-// Use of this source code is governed by an MIT-style
-// license that can be found in the LICENSE file at
-// the root directory of this project.
-
-package frc.robot.extras.debug;
-
-import java.util.Arrays;
-import java.util.HashMap;
-import java.util.Map;
-import java.util.function.Consumer;
-import java.util.function.DoubleSupplier;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
-
-/**
- * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
- * value not in dashboard.
- */
-public class LoggedTunableNumber implements DoubleSupplier {
-  private static final String tableKey = "TunableNumbers";
-
-  private final String key;
-  private boolean hasDefault = false;
-  private double defaultValue;
-  private LoggedDashboardNumber dashboardNumber;
-  private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
-
-  /**
-   * Create a new LoggedTunableNumber
-   *
-   * @param dashboardKey Key on dashboard
-   */
-  public LoggedTunableNumber(String dashboardKey) {
-    this.key = tableKey + "/" + dashboardKey;
-  }
-
-  /**
-   * Create a new LoggedTunableNumber with the default value
-   *
-   * @param dashboardKey Key on dashboard
-   * @param defaultValue Default value
-   */
-  public LoggedTunableNumber(String dashboardKey, double defaultValue) {
-    this(dashboardKey);
-    initDefault(defaultValue);
-  }
-
-  /**
-   * Set the default value of the number. The default value can only be set once.
-   *
-   * @param defaultValue The default value
-   */
-  public void initDefault(double defaultValue) {
-    if (!hasDefault) {
-      hasDefault = true;
-      this.defaultValue = defaultValue;
-      if (hasDefault) {
-        dashboardNumber = new LoggedDashboardNumber(key, defaultValue);
-      }
-    }
-  }
-
-  /**
-   * Get the current value, from dashboard if available and in tuning mode.
-   *
-   * @return The current value
-   */
-  public double get() {
-    if (!hasDefault) {
-      return 0.0;
-    } else {
-      return hasDefault ? dashboardNumber.get() : defaultValue;
-    }
-  }
-
-  /**
-   * Checks whether the number has changed since our last check
-   *
-   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
-   *     objects. Recommended approach is to pass the result of "hashCode()"
-   * @return True if the number has changed since the last time this method was called, false
-   *     otherwise.
-   */
-  public boolean hasChanged(int id) {
-    double currentValue = get();
-    Double lastValue = lastHasChangedValues.get(id);
-    if (lastValue == null || currentValue != lastValue) {
-      lastHasChangedValues.put(id, currentValue);
-      return true;
-    }
-
-    return false;
-  }
-
-  /**
-   * Runs action if any of the tunableNumbers have changed
-   *
-   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
-   *     objects. Recommended approach is to pass the result of "hashCode()"
-   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
-   *     numbers in order inputted in method
-   * @param tunableNumbers All tunable numbers to check
-   */
-  public static void ifChanged(
-      int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
-    if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
-      action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
-    }
-  }
-
-  /** Runs action if any of the tunableNumbers have changed */
-  public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
-    ifChanged(id, values -> action.run(), tunableNumbers);
-  }
-
-  @Override
-  public double getAsDouble() {
-    return get();
-  }
-}
diff --git a/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java b/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java
deleted file mode 100644
index 4e65ecc..0000000
--- a/src/main/java/frc/robot/extras/interpolators/BilinearInterpolator.java
+++ /dev/null
@@ -1,266 +0,0 @@
-// Copyright (c) LukeLib
-
-package frc.robot.extras.interpolators;
-
-public class BilinearInterpolator {
-
-  private final double[][] lookupTable;
-  private final double dataRowSize;
-
-  /**
-   * Handles finding values through a lookup table in a linear fashion.
-   *
-   * @param lookupTable an array containing {x_input, y_input, x_val, y_val} points. The collected
-   *     data must be in ascending y order, collected from left to right, reading up.
-   */
-  public BilinearInterpolator(double[][] lookupTable) {
-    this.lookupTable = lookupTable;
-    // If you don't use rows of seven, replace the numbers 6, 7, 8 in the commands with the
-    // dataRowSize variable with dataRowSize - 1, dataRowSize, dataRowSize + 1
-    // TODO: POST Comp Incorporate this variable
-    this.dataRowSize = lookupTable[0].length;
-  }
-
-  /**
-   * Returns an interpolated array from the lookup table corresponding to the given input x and y
-   * value.
-   */
-  public double[] getLookupValue(double inputXValue, double inputYValue) {
-    // if the note center is approximately in the middle, then only need to do y linear
-    // interpolation (no bilinear :)
-    if (inputXValue >= 155 && inputXValue <= 165) {
-      for (int i = 0; i < lookupTable.length - 8; i += 7) {
-        // check if it is equal to a collected data point
-        if (inputYValue == lookupTable[i][1]) {
-          return new double[] {lookupTable[i][2], lookupTable[i][3]};
-        }
-
-        // interpolate in purely y-direction
-        else if (inputYValue < lookupTable[i][1] && inputYValue > lookupTable[i + 7][1]) {
-          // if x matches up with a data pt, interpolate between above's data pts' y's to determine
-          // pose
-          double ySlope =
-              (lookupTable[i + 7][3] - lookupTable[i][3])
-                  / (lookupTable[i + 7][1] - lookupTable[i][1]);
-          double yYIntercept = lookupTable[i][3];
-          return new double[] {
-            lookupTable[i][2], ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept
-          };
-        }
-      }
-      // in case nothing works or like the note x is good but its too far away in y direction (test)
-      return new double[] {0.0, -1.0};
-    }
-
-    // if the note center is less than half the x_pixels (160)
-    if (inputXValue < 160) {
-      // if note center is less than halfway, then we reflect across y-axis
-      inputXValue = 320 - inputXValue;
-      // Loop through the second row to the second to top most row drawing diagonal vectors. Cannot
-      // start at top/bottom because there cannot be diagonal vectors in both directions
-      for (int i = 7; i < lookupTable.length - 7; i++) {
-        // check if it is equivalent to collected data point
-        if (inputXValue == lookupTable[i][0]) {
-          if (inputYValue == lookupTable[i][1]) {
-            return new double[] {-(lookupTable[i][2]), lookupTable[i][3]};
-          }
-        }
-
-        // bilinear interpolation approximation used diagnoal vectored pt (facing top right)
-        if (inputXValue > lookupTable[i][0]
-            && inputYValue < lookupTable[i][1]
-            && inputXValue < lookupTable[i + 8][0]
-            && inputYValue > lookupTable[i + 8][1]) {
-          // Interpolate diagonal points
-          double xSlope =
-              (lookupTable[i + 8][2] - lookupTable[i][2])
-                  / (lookupTable[i + 8][0] - lookupTable[i][0]);
-          double xYIntercept = lookupTable[i][2];
-          double ySlope =
-              (lookupTable[i + 8][3] - lookupTable[i][3])
-                  / (lookupTable[i + 8][1] - lookupTable[i][1]);
-          double yYIntercept = lookupTable[i][3];
-
-          return new double[] {
-            -(xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept),
-            ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept
-          };
-        }
-
-        // bilinear interpolation approximation used diagnoal in opposite direction vectored pt
-        // (facing bottom right)
-        if (inputXValue > lookupTable[i][0]
-            && inputYValue > lookupTable[i][1]
-            && inputXValue < lookupTable[i - 6][0]
-            && inputYValue < lookupTable[i - 6][1]) {
-          // Interpolate diagonal points
-          double xSlope =
-              (lookupTable[i - 6][2] - lookupTable[i][2])
-                  / (lookupTable[i - 6][0] - lookupTable[i][0]);
-          double xYIntercept = lookupTable[i][2];
-          double ySlope =
-              (lookupTable[i - 6][3] - lookupTable[i][3])
-                  / (lookupTable[i - 6][1] - lookupTable[i][1]);
-          double yYIntercept = lookupTable[i][3];
-
-          return new double[] {
-            -(xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept),
-            ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept
-          };
-        }
-      }
-
-      // check for first row for only x interpolation
-      for (int i = 0; i < 7; i++) {
-        if (inputYValue == lookupTable[i][1]) {
-          if (inputXValue == lookupTable[i][0]) {
-            return new double[] {-lookupTable[i][2], lookupTable[i][3]};
-          } else {
-            // if y matches up with a data pt, interpolate between adjacent (right for over 160)
-            // data pts' x's to determine pose
-            double xSlope =
-                (lookupTable[i + 1][2] - lookupTable[i][2])
-                    / (lookupTable[i + 1][0] - lookupTable[i][0]);
-            double xYIntercept = lookupTable[i][2];
-            return new double[] {
-              -(xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept), lookupTable[i][3]
-            };
-          }
-        }
-      }
-
-      // check for last row just to check for x only interpolation or point checking. (You can't do
-      // y_interpolation or diagonal)
-      for (int i = lookupTable.length - 7; i < lookupTable.length; i++) {
-        if (inputYValue == lookupTable[i][1]) {
-          if (inputXValue == lookupTable[i][0]) {
-            return new double[] {-lookupTable[i][2], lookupTable[i][3]};
-          } else {
-            // if y matches up with a data pt, interpolate between adjacent (right for over 160)
-            // data pts' x's to determine pose
-            double xSlope =
-                (lookupTable[i + 1][2] - lookupTable[i][2])
-                    / (lookupTable[i + 1][0] - lookupTable[i][0]);
-            double xYIntercept = lookupTable[i][2];
-            return new double[] {
-              -(xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept), lookupTable[i][3]
-            };
-          }
-        }
-      }
-
-      // if outside the collected data pt range,
-      // then return unused values for now. I could make a handle to get the max one and then
-      // interpolate or get max for both if its outside and thats what the above 4 cases are
-      // supposed to do but the
-      // logic may need to be changed
-      return new double[] {-1.0, -1.0, 69.0};
-    }
-
-    // if the note center is greater than half the x_pixels (160)
-    if (inputXValue > 160) {
-      // Loop through the second row to the second to top most row drawing diagonal vectors. Cannot
-      // start at top/bottom because there cannot be diagonal vectors in both directions
-      for (int i = 7; i < lookupTable.length - 7; i++) {
-        // check to see if the found note center is equivalent to a collected data point
-        if (inputXValue == lookupTable[i][0]) {
-          if (inputYValue == lookupTable[i][1]) {
-            return new double[] {lookupTable[i][2], lookupTable[i][3]};
-          }
-        }
-
-        // bilinear interpolation approximation used diagnoal vectored pt (facing top right)
-        if (inputXValue > lookupTable[i][0]
-            && inputYValue < lookupTable[i][1]
-            && inputXValue < lookupTable[i + 8][0]
-            && inputYValue > lookupTable[i + 8][1]) {
-          // interpolate in both directions using vector
-          double xSlope =
-              (lookupTable[i + 8][2] - lookupTable[i][2])
-                  / (lookupTable[i + 8][0] - lookupTable[i][0]);
-          double xYIntercept = lookupTable[i][2];
-          double ySlope =
-              (lookupTable[i + 8][3] - lookupTable[i][3])
-                  / (lookupTable[i + 8][1] - lookupTable[i][1]);
-          double yYIntercept = lookupTable[i][3];
-
-          return new double[] {
-            xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept,
-            ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept
-          };
-        }
-
-        // bilinear interpolation approximation used diagnoal in opposite direction vectored pt
-        if (inputXValue > lookupTable[i][0]
-            && inputYValue > lookupTable[i][1]
-            && inputXValue < lookupTable[i - 6][0]
-            && inputYValue < lookupTable[i - 6][1]) {
-          // interpolate in both directions using vector
-          double xSlope =
-              (lookupTable[i - 6][2] - lookupTable[i][2])
-                  / (lookupTable[i - 6][0] - lookupTable[i][0]);
-          double xYIntercept = lookupTable[i][2];
-          double ySlope =
-              (lookupTable[i - 6][3] - lookupTable[i][3])
-                  / (lookupTable[i - 6][1] - lookupTable[i][1]);
-          double yYIntercept = lookupTable[i][3];
-
-          return new double[] {
-            xSlope * (inputXValue - lookupTable[i][0]) + xYIntercept,
-            ySlope * (inputYValue - lookupTable[i][1]) + yYIntercept
-          };
-        }
-      }
-
-      // check for first row for only x interpolation
-      for (int i = 0; i < 7; i++) {
-        if (inputYValue == lookupTable[i][1]) {
-          if (inputXValue == lookupTable[i][0]) {
-            return new double[] {lookupTable[i][2], lookupTable[i][3]};
-          } else {
-            // if y matches up with a data pt, interpolate between adjacent (right for over 160)
-            // data pts' x's to determine pose
-            double xSlope =
-                (lookupTable[i + 1][2] - lookupTable[i][2])
-                    / (lookupTable[i + 1][0] - lookupTable[i][0]);
-            double xYIntercept = lookupTable[i][2];
-            return new double[] {
-              xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept, lookupTable[i][3]
-            };
-          }
-        }
-      }
-
-      // check for last row just to check for x only interpolation or point checking. (You can't do
-      // y_interpolation or diagonal)
-      for (int i = lookupTable.length - 7; i < lookupTable.length; i++) {
-        if (inputYValue == lookupTable[i][1]) {
-          if (inputXValue == lookupTable[i][0]) {
-            return new double[] {lookupTable[i][2], lookupTable[i][3]};
-          } else {
-            // if y matches up with a data pt, interpolate between adjacent (right for over 160)
-            // data pts' x's to determine pose
-            double xSlope =
-                (lookupTable[i + 1][2] - lookupTable[i][2])
-                    / (lookupTable[i + 1][0] - lookupTable[i][0]);
-            double xYIntercept = lookupTable[i][2];
-            return new double[] {
-              xSlope * (inputYValue - lookupTable[i][0]) + xYIntercept, lookupTable[i][3]
-            };
-          }
-        }
-      }
-
-      // if outside the collected data pt range,
-      // then return unused values for now. I could make a handle to get the max one and then
-      // interpolate or get max for both if its outside and thats what the above 4 cases are
-      // supposed to do but the
-      // logic may need to be changed
-      return new double[] {-1.0, -1.0, 96.0};
-    }
-
-    // The third number in each double is just for debugging purposes to determine which case was
-    // selected
-    return new double[] {-1.0, -1.0, 77.0};
-  }
-}
diff --git a/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java b/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java
new file mode 100644
index 0000000..1559662
--- /dev/null
+++ b/src/main/java/frc/robot/extras/setpointGen/AdvancedSwerveModuleState.java
@@ -0,0 +1,27 @@
+package frc.robot.extras.setpointGen;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.util.struct.Struct;
+
+public final class AdvancedSwerveModuleState extends SwerveModuleState {
+  public double steerVelocityFF;
+  public double driveAccelerationFF;
+
+  public AdvancedSwerveModuleState(
+      double speedMetersPerSecond,
+      Rotation2d angle,
+      double steerVelocityFF,
+      double driveAccelerationFF) {
+    super(speedMetersPerSecond, angle);
+    this.steerVelocityFF = steerVelocityFF;
+    this.driveAccelerationFF = driveAccelerationFF;
+  }
+
+  // todo: implement custom struct
+  public static final Struct<SwerveModuleState> struct = SwerveModuleState.struct;
+
+  public static AdvancedSwerveModuleState fromBase(SwerveModuleState base) {
+    return new AdvancedSwerveModuleState(base.speedMetersPerSecond, base.angle, 0.0, 0.0);
+  }
+}
diff --git a/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java b/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java
new file mode 100644
index 0000000..a01f64d
--- /dev/null
+++ b/src/main/java/frc/robot/extras/setpointGen/SPGCalcs.java
@@ -0,0 +1,285 @@
+package frc.robot.extras.setpointGen;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+import frc.robot.extras.util.ProceduralStructGenerator;
+import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray;
+import frc.robot.extras.util.ProceduralStructGenerator.IgnoreStructField;
+import java.nio.ByteBuffer;
+import java.util.Arrays;
+
+class SPGCalcs {
+  private static final double kEpsilon = 1E-8;
+  static final int NUM_MODULES = 4;
+
+  static double unwrapAngle(double ref, double angle) {
+    double diff = angle - ref;
+    if (diff > Math.PI) {
+      return angle - 2.0 * Math.PI;
+    } else if (diff < -Math.PI) {
+      return angle + 2.0 * Math.PI;
+    } else {
+      return angle;
+    }
+  }
+
+  static double findSteeringMaxS(
+      double prevVx,
+      double prevVy,
+      double prevHeading,
+      double desiredVx,
+      double desiredVy,
+      double desiredHeading,
+      double maxDeviation) {
+    desiredHeading = unwrapAngle(prevHeading, desiredHeading);
+    double diff = desiredHeading - prevHeading;
+    if (Math.abs(diff) <= maxDeviation) {
+      // Can go all the way to s=1.
+      return 1.0;
+    }
+
+    double target = prevHeading + Math.copySign(maxDeviation, diff);
+
+    // Rotate the velocity vectors such that the target angle becomes the +X
+    // axis. We only need find the Y components, h_0 and h_1, since they are
+    // proportional to the distances from the two points to the solution
+    // point (x_0 + (x_1 - x_0)s, y_0 + (y_1 - y_0)s).
+    double sin = Math.sin(-target);
+    double cos = Math.cos(-target);
+    double h_0 = sin * prevVx + cos * prevVy;
+    double h_1 = sin * desiredVx + cos * desiredVy;
+
+    // Undo linear interpolation from h_0 to h_1:
+    // 0 = h_0 + (h_1 - h_0) * s
+    // -h_0 = (h_1 - h_0) * s
+    // -h_0 / (h_1 - h_0) = s
+    // h_0 / (h_0 - h_1) = s
+    // Guaranteed to not divide by zero, since if h_0 was equal to h_1, theta_0
+    // would be equal to theta_1, which is caught by the difference check.
+    return h_0 / (h_0 - h_1);
+  }
+
+  private static boolean isValidS(double s) {
+    return Double.isFinite(s) && s >= 0 && s <= 1;
+  }
+
+  static double findDriveMaxS(double x_0, double y_0, double x_1, double y_1, double max_vel_step) {
+    // Derivation:
+    // Want to find point P(s) between (x_0, y_0) and (x_1, y_1) where the
+    // length of P(s) is the target T. P(s) is linearly interpolated between the
+    // points, so P(s) = (x_0 + (x_1 - x_0) * s, y_0 + (y_1 - y_0) * s).
+    // Then,
+    //     T = sqrt(P(s).x^2 + P(s).y^2)
+    //   T^2 = (x_0 + (x_1 - x_0) * s)^2 + (y_0 + (y_1 - y_0) * s)^2
+    //   T^2 = x_0^2 + 2x_0(x_1-x_0)s + (x_1-x_0)^2*s^2
+    //       + y_0^2 + 2y_0(y_1-y_0)s + (y_1-y_0)^2*s^2
+    //   T^2 = x_0^2 + 2x_0x_1s - 2x_0^2*s + x_1^2*s^2 - 2x_0x_1s^2 + x_0^2*s^2
+    //       + y_0^2 + 2y_0y_1s - 2y_0^2*s + y_1^2*s^2 - 2y_0y_1s^2 + y_0^2*s^2
+    //     0 = (x_0^2 + y_0^2 + x_1^2 + y_1^2 - 2x_0x_1 - 2y_0y_1)s^2
+    //       + (2x_0x_1 + 2y_0y_1 - 2x_0^2 - 2y_0^2)s
+    //       + (x_0^2 + y_0^2 - T^2).
+    //
+    // To simplify, we can factor out some common parts:
+    // Let l_0 = x_0^2 + y_0^2, l_1 = x_1^2 + y_1^2, and
+    // p = x_0 * x_1 + y_0 * y_1.
+    // Then we have
+    //   0 = (l_0 + l_1 - 2p)s^2 + 2(p - l_0)s + (l_0 - T^2),
+    // with which we can solve for s using the quadratic formula.
+
+    double l_0 = x_0 * x_0 + y_0 * y_0;
+    double l_1 = x_1 * x_1 + y_1 * y_1;
+    double sqrt_l_0 = Math.sqrt(l_0);
+    double diff = Math.sqrt(l_1) - sqrt_l_0;
+    if (Math.abs(diff) <= max_vel_step) {
+      // Can go all the way to s=1.
+      return 1.0;
+    }
+
+    double target = sqrt_l_0 + Math.copySign(max_vel_step, diff);
+    double p = x_0 * x_1 + y_0 * y_1;
+
+    // Quadratic of s
+    double a = l_0 + l_1 - 2 * p;
+    double b = 2 * (p - l_0);
+    double c = l_0 - target * target;
+    double root = Math.sqrt(b * b - 4 * a * c);
+
+    // Check if either of the solutions are valid
+    // Won't divide by zero because it is only possible for a to be zero if the
+    // target velocity is exactly the same or the reverse of the current
+    // velocity, which would be caught by the difference check.
+    double s_1 = (-b + root) / (2 * a);
+    if (isValidS(s_1)) {
+      return s_1;
+    }
+    double s_2 = (-b - root) / (2 * a);
+    if (isValidS(s_2)) {
+      return s_2;
+    }
+
+    // Since we passed the initial max_vel_step check, a solution should exist,
+    // but if no solution was found anyway, just don't limit movement
+    return 1.0;
+  }
+
+  static boolean epsilonEquals(double a, double b, double epsilon) {
+    return (a - epsilon <= b) && (a + epsilon >= b);
+  }
+
+  static boolean epsilonEquals(double a, double b) {
+    return epsilonEquals(a, b, kEpsilon);
+  }
+
+  static boolean epsilonEquals(ChassisSpeeds s1, ChassisSpeeds s2) {
+    return epsilonEquals(s1.vxMetersPerSecond, s2.vxMetersPerSecond)
+        && epsilonEquals(s1.vyMetersPerSecond, s2.vyMetersPerSecond)
+        && epsilonEquals(s1.omegaRadiansPerSecond, s2.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Check if it would be faster to go to the opposite of the goal heading (and reverse drive
+   * direction).
+   *
+   * @param prevToGoal The rotation from the previous state to the goal state (i.e.
+   *     prev.inverse().rotateBy(goal)).
+   * @return True if the shortest path to achieve this rotation involves flipping the drive
+   *     direction.
+   */
+  static boolean flipHeading(double prevToGoal) {
+    return Math.abs(prevToGoal) > Math.PI / 2.0;
+  }
+
+  private static final Struct<LocalVectors> structVectors;
+  private static final Struct<LocalVars> structVars;
+
+  static {
+    final var structVectorsProc =
+        ProceduralStructGenerator.genObject(LocalVectors.class, LocalVectors::new);
+    structVectors =
+        new Struct<SPGCalcs.LocalVectors>() {
+          @Override
+          public String getSchema() {
+            return "float64 vx;float64 vy;float64 cos;float64 sin;";
+          }
+
+          @Override
+          public int getSize() {
+            return 32;
+          }
+
+          @Override
+          public Class<LocalVectors> getTypeClass() {
+            return LocalVectors.class;
+          }
+
+          @Override
+          public String getTypeName() {
+            return "LocalVectors";
+          }
+
+          @Override
+          public void pack(ByteBuffer bb, LocalVectors value) {
+            bb.putDouble(value.vx);
+            bb.putDouble(value.vy);
+            bb.putDouble(value.cos);
+            bb.putDouble(value.sin);
+          }
+
+          @Override
+          public LocalVectors unpack(ByteBuffer bb) {
+            return structVectorsProc.unpack(bb);
+          }
+
+          @Override
+          public String toString() {
+            return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.getSchema() + "}";
+          }
+        };
+    structVars = ProceduralStructGenerator.genObject(LocalVars.class, LocalVars::new);
+    System.out.println(structVectors);
+    System.out.println(structVars);
+  }
+
+  static final class LocalVectors implements StructSerializable {
+    public double vx, vy, cos, sin;
+
+    public LocalVectors() {}
+
+    public void reset() {
+      vx = vy = cos = sin = 0.0;
+    }
+
+    public void applyModuleState(SwerveModuleState state) {
+      cos = state.angle.getCos();
+      sin = state.angle.getSin();
+      vx = cos * state.speedMetersPerSecond;
+      vy = sin * state.speedMetersPerSecond;
+      if (state.speedMetersPerSecond < 0.0) {
+        applyRotation(Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin());
+      }
+    }
+
+    public LocalVectors applyRotation(double otherCos, double otherSin) {
+      double newCos = cos * otherCos - sin * otherSin;
+      double newSin = cos * otherSin + sin * otherCos;
+      cos = newCos;
+      sin = newSin;
+
+      return this;
+    }
+
+    public double radians() {
+      return Math.atan2(sin, cos);
+    }
+
+    public static final Struct<LocalVectors> struct = structVectors;
+  }
+
+  static final class LocalVars implements StructSerializable {
+    @FixedSizeArray(size = NUM_MODULES)
+    public LocalVectors[] prev;
+
+    @FixedSizeArray(size = NUM_MODULES)
+    public LocalVectors[] desired;
+
+    public boolean needToSteer = true, allModulesShouldFlip = true;
+    public double minS, dt;
+    public double dx, dy, dtheta;
+    public ChassisSpeeds prevSpeeds, desiredSpeeds;
+
+    @FixedSizeArray(size = NUM_MODULES)
+    public SwerveModuleState[] prevModuleStates;
+
+    @FixedSizeArray(size = NUM_MODULES)
+    public SwerveModuleState[] desiredModuleStates;
+
+    @IgnoreStructField public Rotation2d[] steeringOverride;
+
+    public LocalVars() {
+      desiredSpeeds = prevSpeeds = new ChassisSpeeds();
+      prev = new LocalVectors[NUM_MODULES];
+      desired = new LocalVectors[NUM_MODULES];
+      steeringOverride = new Rotation2d[NUM_MODULES];
+      for (int i = 0; i < NUM_MODULES; i++) {
+        prev[i] = new LocalVectors();
+        desired[i] = new LocalVectors();
+      }
+    }
+
+    public LocalVars reset() {
+      needToSteer = allModulesShouldFlip = true;
+      Arrays.fill(steeringOverride, null);
+      for (int i = 0; i < NUM_MODULES; i++) {
+        prev[i].reset();
+        desired[i].reset();
+      }
+
+      return this;
+    }
+
+    public static final Struct<LocalVars> struct = structVars;
+  }
+}
diff --git a/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java
new file mode 100644
index 0000000..7bb5e7c
--- /dev/null
+++ b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpoint.java
@@ -0,0 +1,30 @@
+package frc.robot.extras.setpointGen;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+import frc.robot.extras.util.ProceduralStructGenerator;
+import frc.robot.extras.util.ProceduralStructGenerator.FixedSizeArray;
+
+public record SwerveSetpoint(
+    ChassisSpeeds chassisSpeeds,
+    @FixedSizeArray(size = 4) AdvancedSwerveModuleState[] moduleStates) //
+    implements StructSerializable {
+  public static SwerveSetpoint zeroed() {
+    return new SwerveSetpoint(
+        new ChassisSpeeds(),
+        new AdvancedSwerveModuleState[] {
+          new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0),
+          new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0),
+          new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0),
+          new AdvancedSwerveModuleState(0, Rotation2d.kZero, 0, 0)
+        });
+  }
+
+  public static final Struct<SwerveSetpoint> struct;
+
+  static {
+    struct = ProceduralStructGenerator.genRecord(SwerveSetpoint.class);
+  }
+}
diff --git a/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java
new file mode 100644
index 0000000..3f55029
--- /dev/null
+++ b/src/main/java/frc/robot/extras/setpointGen/SwerveSetpointGenerator.java
@@ -0,0 +1,363 @@
+package frc.robot.extras.setpointGen;
+
+import static frc.robot.extras.setpointGen.SPGCalcs.*;
+
+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.SwerveModuleState;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.RobotController;
+
+/**
+ * Swerve setpoint generatoR that has been passed around so many times its hard to keep track, just
+ * know i didn't write most the logic in this code that credit goes to 254 and mjansen
+ *
+ * <p>Takes a prior setpoint, a desired setpoint, and outputs a new setpoint that respects all the
+ * kinematic constraints on module rotation and wheel velocity/torque, as well as preventing any
+ * forces acting on a module's wheel from exceeding the force of friction.
+ */
+public class SwerveSetpointGenerator {
+  private static final ChassisSpeeds ZERO_SPEEDS = new ChassisSpeeds();
+
+  private static final int NUM_MODULES = SPGCalcs.NUM_MODULES;
+
+  private final SwerveDriveKinematics kinematics;
+  private final Translation2d[] moduleLocations;
+  private final DCMotor driveMotor;
+  private final double driveCurrentLimitAmps,
+      maxDriveVelocityMPS,
+      maxSteerVelocityRadsPerSec,
+      massKg,
+      moiKgMetersSquared,
+      wheelRadiusMeters,
+      wheelFrictionForce,
+      // maxTorqueFriction,
+      torqueLoss;
+
+  public SwerveSetpointGenerator(
+      final Translation2d[] moduleLocations,
+      final DCMotor driveMotor,
+      final DCMotor angleMotor,
+      final double driveCurrentLimitAmps,
+      final double massKg,
+      final double moiKgMetersSquared,
+      final double wheelDiameterMeters,
+      final double wheelCoF,
+      final double torqueLoss) {
+
+    if (moduleLocations.length != NUM_MODULES) {
+      throw new IllegalArgumentException("Module locations must have 4 elements");
+    }
+
+    this.driveMotor = driveMotor;
+    this.driveCurrentLimitAmps = driveCurrentLimitAmps;
+    this.maxSteerVelocityRadsPerSec = angleMotor.freeSpeedRadPerSec;
+    kinematics = new SwerveDriveKinematics(moduleLocations);
+    this.moduleLocations = moduleLocations;
+    this.massKg = massKg;
+    this.moiKgMetersSquared = moiKgMetersSquared;
+    this.wheelRadiusMeters = wheelDiameterMeters / 2;
+    this.maxDriveVelocityMPS = driveMotor.freeSpeedRadPerSec * wheelRadiusMeters;
+
+    wheelFrictionForce = wheelCoF * ((massKg / 4) * 9.81);
+    // maxTorqueFriction = this.wheelFrictionForce * wheelRadiusMeters;
+    this.torqueLoss = torqueLoss;
+  }
+
+  // alot of work was done to reduce allocations in this hot loop,
+  // migrating everything over to a vars object that gets reused
+  // was the best way to do this.
+  private static final LocalVars VARS_TEMPLATE = new LocalVars();
+
+  /**
+   * Generate a new setpoint. Note: Do not discretize ChassisSpeeds passed into or returned from
+   * this method. This method will discretize the speeds for you.
+   *
+   * @param prevSetpoint The previous setpoint motion. Normally, you'd pass in the previous
+   *     iteration setpoint instead of the actual measured/estimated kinematic state.
+   * @param desiredRobotRelativeSpeeds The desired state of motion, such as from the driver sticks
+   *     or a path following algorithm.
+   * @param dt The loop time.
+   * @return A Setpoint object that satisfies all the kinematic/friction limits while converging to
+   *     desiredState quickly.
+   */
+  public SwerveSetpoint generateSetpoint(
+      final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) {
+    // https://github.com/wpilibsuite/allwpilib/issues/7332
+    SwerveModuleState[] desiredModuleStates =
+        kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds);
+    // Make sure desiredState respects velocity limits.
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS);
+    desiredRobotRelativeSpeeds = kinematics.toChassisSpeeds(desiredModuleStates);
+
+    final LocalVars vars = VARS_TEMPLATE.reset();
+    vars.dt = dt;
+    vars.prevSpeeds = prevSetpoint.chassisSpeeds();
+    vars.desiredSpeeds = desiredRobotRelativeSpeeds;
+    vars.desiredModuleStates = desiredModuleStates;
+    vars.prevModuleStates = prevSetpoint.moduleStates();
+    vars.dx =
+        desiredRobotRelativeSpeeds.vxMetersPerSecond
+            - prevSetpoint.chassisSpeeds().vxMetersPerSecond;
+    vars.dy =
+        desiredRobotRelativeSpeeds.vyMetersPerSecond
+            - prevSetpoint.chassisSpeeds().vyMetersPerSecond;
+    vars.dtheta =
+        desiredRobotRelativeSpeeds.omegaRadiansPerSecond
+            - prevSetpoint.chassisSpeeds().omegaRadiansPerSecond;
+    vars.minS = 1.0;
+
+    checkNeedToSteer(vars);
+    makeVectors(vars);
+
+    // if (vars.allModulesShouldFlip
+    //         && !epsilonEquals(prevSetpoint.chassisSpeeds(), ZERO_SPEEDS)
+    //         && !epsilonEquals(desiredRobotRelativeSpeeds, ZERO_SPEEDS)) {
+    //     // It will (likely) be faster to stop the robot, rotate the modules in place to the
+    // complement
+    //     // of the desired angle, and accelerate again.
+    //     return generateSetpoint(prevSetpoint, ZERO_SPEEDS, dt);
+    // }
+
+    solveSteering(vars);
+
+    solveDriving(vars);
+
+    ChassisSpeeds retSpeeds =
+        new ChassisSpeeds(
+            vars.prevSpeeds.vxMetersPerSecond + vars.minS * vars.dx,
+            vars.prevSpeeds.vyMetersPerSecond + vars.minS * vars.dy,
+            vars.prevSpeeds.omegaRadiansPerSecond + vars.minS * vars.dtheta);
+    retSpeeds.discretize(retSpeeds, dt);
+
+    double chassisAccelX = (retSpeeds.vxMetersPerSecond - vars.prevSpeeds.vxMetersPerSecond) / dt;
+    double chassisAccelY = (retSpeeds.vyMetersPerSecond - vars.prevSpeeds.vyMetersPerSecond) / dt;
+    double angularAccel =
+        (retSpeeds.omegaRadiansPerSecond - vars.prevSpeeds.omegaRadiansPerSecond) / dt;
+
+    SwerveModuleState[] retStates = kinematics.toSwerveModuleStates(retSpeeds);
+    SwerveModuleState[] accelStates =
+        kinematics.toSwerveModuleStates(
+            new ChassisSpeeds(chassisAccelX, chassisAccelY, angularAccel));
+
+    AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES];
+    for (int m = 0; m < NUM_MODULES; m++) {
+      retStates[m].optimize(vars.prevModuleStates[m].angle);
+      double steerVelocity =
+          (vars.prevModuleStates[m].angle.getRadians() - retStates[m].angle.getRadians()) / dt;
+      outputStates[m] =
+          new AdvancedSwerveModuleState(
+              retStates[m].speedMetersPerSecond,
+              retStates[m].angle,
+              steerVelocity,
+              accelStates[m].speedMetersPerSecond);
+    }
+
+    // log("output",
+    return new SwerveSetpoint(retSpeeds, outputStates);
+  }
+
+  public SwerveSetpoint generateSimpleSetpoint(
+      final SwerveSetpoint prevSetpoint, ChassisSpeeds desiredRobotRelativeSpeeds, double dt) {
+    AdvancedSwerveModuleState[] outputStates = new AdvancedSwerveModuleState[NUM_MODULES];
+    SwerveModuleState[] desiredModuleStates =
+        kinematics.toSwerveModuleStates(desiredRobotRelativeSpeeds);
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleStates, maxDriveVelocityMPS);
+    for (int m = 0; m < NUM_MODULES; m++) {
+      desiredModuleStates[m].optimize(prevSetpoint.moduleStates()[m].angle);
+      outputStates[m] = AdvancedSwerveModuleState.fromBase(desiredModuleStates[m]);
+    }
+
+    return new SwerveSetpoint(kinematics.toChassisSpeeds(desiredModuleStates), outputStates);
+  }
+
+  private static void checkNeedToSteer(LocalVars vars) {
+    if (epsilonEquals(vars.desiredSpeeds, ZERO_SPEEDS)) {
+      vars.needToSteer = false;
+      for (int m = 0; m < NUM_MODULES; m++) {
+        vars.desiredModuleStates[m].angle = vars.prevModuleStates[m].angle;
+        vars.desiredModuleStates[m].speedMetersPerSecond = 0.0;
+      }
+    }
+  }
+
+  private static double rotateBy(double rad, double otherCos, double otherSin) {
+    return Math.atan2(
+        Math.sin(rad) * otherCos + Math.cos(rad) * otherSin,
+        Math.cos(rad) * otherCos - Math.sin(rad) * otherSin);
+  }
+
+  private static double requiredRotation(double prevRadians, double desiredRads) {
+    // this looks messy without using Rotation2d methods.
+    // this is roughly equivalent to:
+    //
+    // double r = vars.prev[m].rot2d().unaryMinus()
+    //    .rotateBy(vars.desired[m].rot2d()).getRadians();
+    double unaryMinusPrevRads = -prevRadians;
+    return rotateBy(unaryMinusPrevRads, Math.cos(desiredRads), Math.sin(desiredRads));
+  }
+
+  private static void makeVectors(LocalVars vars) {
+    for (int m = 0; m < NUM_MODULES; m++) {
+      vars.prev[m].applyModuleState(vars.prevModuleStates[m]);
+      vars.desired[m].applyModuleState(vars.desiredModuleStates[m]);
+      if (vars.allModulesShouldFlip) {
+        double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians());
+        if (flipHeading(requiredRots)) {
+          vars.allModulesShouldFlip = false;
+        }
+      }
+    }
+  }
+
+  private void solveSteering(LocalVars vars) {
+    // In cases where an individual module is stopped, we want to remember the right steering angle
+    // to command (since inverse kinematics doesn't care about angle, we can be opportunistically
+    // lazy).
+    // Enforce steering velocity limits. We do this by taking the derivative of steering angle at
+    // the current angle, and then backing out the maximum interpolant between start and goal
+    // states. We remember the minimum across all modules, since that is the active constraint.
+    for (int m = 0; m < NUM_MODULES; m++) {
+      if (!vars.needToSteer) {
+        vars.steeringOverride[m] = vars.prevModuleStates[m].angle;
+        continue;
+      }
+
+      double maxThetaStep = vars.dt * maxSteerVelocityRadsPerSec;
+
+      if (epsilonEquals(vars.prevModuleStates[m].speedMetersPerSecond, 0.0)) {
+        // If module is stopped, we know that we will need to move straight to the final steering
+        // angle, so limit based purely on rotation in place.
+        if (epsilonEquals(vars.desiredModuleStates[m].speedMetersPerSecond, 0.0)) {
+          // Goal angle doesn't matter. Just leave module at its current angle.
+          vars.steeringOverride[m] = vars.prevModuleStates[m].angle;
+          continue;
+        }
+
+        double requiredRots = requiredRotation(vars.prev[m].radians(), vars.desired[m].radians());
+        if (flipHeading(requiredRots)) {
+          requiredRots =
+              rotateBy(requiredRots, Rotation2d.k180deg.getCos(), Rotation2d.k180deg.getSin());
+        }
+
+        // radians bounds to +/- Pi.
+        final double numStepsNeeded = Math.abs(requiredRots) / maxThetaStep;
+
+        if (numStepsNeeded <= 1.0) {
+          // Steer directly to goal angle.
+          vars.steeringOverride[m] = vars.desiredModuleStates[m].angle;
+        } else {
+          // Adjust steering by max_theta_step.
+          // there really is no way to avoid this allocation.
+          Rotation2d adjusted =
+              vars.prevModuleStates[m].angle.rotateBy(
+                  Rotation2d.fromRadians(Math.signum(requiredRots) * maxThetaStep));
+          vars.steeringOverride[m] = adjusted;
+          vars.minS = 0.0;
+        }
+        continue;
+      }
+      if (vars.minS == 0.0) {
+        // s can't get any lower. Save some CPU.
+        continue;
+      }
+
+      double maxS =
+          findSteeringMaxS(
+              vars.prev[m].vx,
+              vars.prev[m].vy,
+              vars.prev[m].radians(),
+              vars.desired[m].vx,
+              vars.desired[m].vy,
+              vars.desired[m].radians(),
+              maxThetaStep);
+      vars.minS = Math.min(vars.minS, maxS);
+    }
+  }
+
+  private void solveDriving(LocalVars vars) {
+    // Enforce drive wheel torque limits
+    Translation2d chassisForceVec = Translation2d.kZero;
+    double chassisTorque = 0.0;
+    for (int m = 0; m < NUM_MODULES; m++) {
+      double lastVelRadPerSec = vars.prevModuleStates[m].speedMetersPerSecond / wheelRadiusMeters;
+      // Use the current battery voltage since we won't be able to supply 12v if the
+      // battery is sagging down to 11v, which will affect the max torque output
+      double currentDraw =
+          driveMotor.getCurrent(Math.abs(lastVelRadPerSec), RobotController.getInputVoltage());
+      currentDraw = Math.min(currentDraw, driveCurrentLimitAmps);
+      double moduleTorque = driveMotor.getTorque(currentDraw);
+
+      double prevSpeed = vars.prevModuleStates[m].speedMetersPerSecond;
+      vars.desiredModuleStates[m].optimize(vars.prevModuleStates[m].angle);
+      double desiredSpeed = vars.desiredModuleStates[m].speedMetersPerSecond;
+
+      int forceSign;
+      Rotation2d forceAngle = vars.prevModuleStates[m].angle;
+      if (epsilonEquals(prevSpeed, 0.0)
+          || (prevSpeed > 0 && desiredSpeed >= prevSpeed)
+          || (prevSpeed < 0 && desiredSpeed <= prevSpeed)) {
+        // Torque loss will be fighting motor
+        moduleTorque -= torqueLoss;
+        forceSign = 1; // Force will be applied in direction of module
+        if (prevSpeed < 0) {
+          forceAngle = forceAngle.plus(Rotation2d.k180deg);
+        }
+      } else {
+        // Torque loss will be helping the motor
+        moduleTorque += torqueLoss;
+        forceSign = -1; // Force will be applied in opposite direction of module
+        if (prevSpeed > 0) {
+          forceAngle = forceAngle.plus(Rotation2d.k180deg);
+        }
+      }
+
+      // Limit torque to prevent wheel slip
+      moduleTorque = Math.min(moduleTorque, wheelFrictionForce * wheelRadiusMeters);
+
+      double forceAtCarpet = moduleTorque / wheelRadiusMeters;
+      Translation2d moduleForceVec = new Translation2d(forceAtCarpet * forceSign, forceAngle);
+
+      // Add the module force vector to the chassis force vector
+      chassisForceVec = chassisForceVec.plus(moduleForceVec);
+
+      // Calculate the torque this module will apply to the chassis
+      Rotation2d angleToModule = moduleLocations[m].getAngle();
+      Rotation2d theta = moduleForceVec.getAngle().minus(angleToModule);
+      chassisTorque += forceAtCarpet * moduleLocations[m].getNorm() * theta.getSin();
+    }
+
+    Translation2d chassisAccelVec = chassisForceVec.div(massKg);
+    double chassisAngularAccel = chassisTorque / moiKgMetersSquared;
+
+    // Use kinematics to convert chassis accelerations to module accelerations
+    ChassisSpeeds chassisAccel =
+        new ChassisSpeeds(chassisAccelVec.getX(), chassisAccelVec.getY(), chassisAngularAccel);
+    var accelStates = kinematics.toSwerveModuleStates(chassisAccel);
+
+    for (int m = 0; m < NUM_MODULES; m++) {
+      if (vars.minS == 0.0) {
+        // No need to carry on.
+        break;
+      }
+
+      double maxVelStep = Math.abs(accelStates[m].speedMetersPerSecond * vars.dt);
+
+      double vxMinS;
+      double vyMinS;
+      if (vars.minS == 1.0) {
+        vxMinS = vars.desired[m].vx;
+        vyMinS = vars.desired[m].vy;
+      } else {
+        vxMinS = (vars.desired[m].vx - vars.prev[m].vx) * vars.minS + vars.prev[m].vx;
+        vyMinS = (vars.desired[m].vy - vars.prev[m].vy) * vars.minS + vars.prev[m].vy;
+      }
+      // Find the max s for this drive wheel. Search on the interval between 0 and min_s, because we
+      // already know we can't go faster than that.
+      double s = findDriveMaxS(vars.prev[m].vx, vars.prev[m].vy, vxMinS, vyMinS, maxVelStep);
+      vars.minS = Math.min(vars.minS, s);
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java
index d991fa8..e73bc04 100644
--- a/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java
+++ b/src/main/java/frc/robot/extras/simulation/field/SimulatedField.java
@@ -3,12 +3,17 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.extras.simulation.gamePiece.CrescendoNoteSimulation;
 import frc.robot.extras.simulation.gamePiece.GamePieceSimulation;
 import frc.robot.extras.simulation.mechanismSim.IntakeSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.AbstractDriveTrainSimulation;
+import frc.robot.extras.simulation.mechanismSim.swerve.BrushlessMotorSim;
 import frc.robot.extras.util.GeomUtil;
+// import java.lang.ref.WeakReference;
 import java.util.*;
 import org.dyn4j.dynamics.Body;
 import org.dyn4j.dynamics.BodyFixture;
@@ -125,6 +130,7 @@ public static void overrideSimulationTimings(
   protected final Set<AbstractDriveTrainSimulation> driveTrainSimulations;
   protected final Set<GamePieceSimulation> gamePieces;
   protected final List<Runnable> simulationSubTickActions;
+  protected final List<BrushlessMotorSim> motors;
   private final List<IntakeSimulation> intakeSimulations;
 
   /**
@@ -149,6 +155,7 @@ protected SimulatedField(FieldMap obstaclesMap) {
     simulationSubTickActions = new ArrayList<>();
     this.gamePieces = new HashSet<>();
     this.intakeSimulations = new ArrayList<>();
+    motors = new ArrayList<>();
   }
 
   /**
@@ -276,6 +283,10 @@ public void simulationPeriodic() {
         "MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0);
   }
 
+  public void addMotor(BrushlessMotorSim motor) {
+    motors.add(motor);
+  }
+
   /**
    *
    *
@@ -293,6 +304,17 @@ public void simulationPeriodic() {
    * </ul>
    */
   private void simulationSubTick() {
+    ArrayList<Double> motorCurrents = new ArrayList<>();
+    for (var motor : motors) {
+      BrushlessMotorSim motorRef = motor;
+      if (motorRef != null) {
+        motorRef.update();
+      }
+    }
+    double vin =
+        BatterySim.calculateLoadedBatteryVoltage(
+            12.2, 0.015, motorCurrents.stream().mapToDouble(Double::doubleValue).toArray());
+    RoboRioSim.setVInVoltage(vin);
     for (AbstractDriveTrainSimulation driveTrainSimulation : driveTrainSimulations)
       driveTrainSimulation.simulationSubTick();
 
diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java
index 5ba3122..76f1e64 100644
--- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java
+++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/BrushlessMotorSim.java
@@ -1,41 +1,41 @@
-// A drop-in replacement for WPILib's DCMotorSim to simulate modern brushless motors
-// Copyright (C) 2024 Team-5516-"Iron Maple"
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in
-// all copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-//
-// Original source:
-// https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/main/src/main/java/org/ironmaple/simulation/drivesims/BrushlessMotorSim.java
-
 package frc.robot.extras.simulation.mechanismSim.swerve;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.NewtonMeters;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.units.AngleUnit;
+import edu.wpi.first.units.AngularAccelerationUnit;
+import edu.wpi.first.units.AngularVelocityUnit;
+import edu.wpi.first.units.CurrentUnit;
+import edu.wpi.first.units.PerUnit;
+import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularAcceleration;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.MomentOfInertia;
+import edu.wpi.first.units.measure.Per;
+import edu.wpi.first.units.measure.Torque;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import frc.robot.extras.simulation.field.SimulatedField;
 
 /**
  *
  *
  * <h1>{@link DCMotorSim} with a bit of extra spice.</h1>
  *
- * <p>By Team 5516 "IRON MAPLE".
- *
  * <p>This class extends the functionality of the original {@link DCMotorSim} and models the
  * following aspects in addition:
  *
@@ -43,16 +43,47 @@
  *   <li>Friction force on the rotor.
  *   <li>Smart current limiting.
  *   <li>Brake and coast modes (only for simulating brushless motors).
- *   <li>Simulated encoder readings.
  * </ul>
  */
 public class BrushlessMotorSim {
+  public enum OutputType {
+    VOLTAGE,
+    CURRENT
+  }
+
+  public enum OutputMode {
+    VELOCITY,
+    POSITION,
+    OPEN_LOOP
+  }
+
+  /** The Constants for the motor */
   private final DCMotor motor;
-  private final double gearRatio, frictionTorque, loadInertiaJKgMetersSquared;
-  private double requestedVoltageOutput, currentLimitAmps;
-  private boolean breakModeEnabled;
 
-  private double angularPositionRad, angularVelocityRadPerSec, appliedVolts, currentDrawAmps;
+  /** The dynamics simulation for the motor */
+  private final DCMotorSim sim;
+
+  /** The gear ratio, value above 1.0 are a reduction */
+  private final double gearing;
+
+  /** The voltage required to overcome friction */
+  private final Voltage frictionVoltage;
+
+  private final PIDController poseVoltController = new PIDController(0, 0, 0);
+  private final PIDController veloVoltController = new PIDController(0, 0, 0);
+  private final PIDController poseCurrentController = new PIDController(0, 0, 0);
+  private final PIDController veloCurrentController = new PIDController(0, 0, 0);
+
+  private SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(0, 0, 0);
+
+  private Current currentLimit = Amps.of(300.0);
+
+  private OutputType outputType = OutputType.VOLTAGE;
+  private OutputMode outputMode = OutputMode.OPEN_LOOP;
+  private double output = 0.0;
+
+  private Angle forwardLimit = Radians.of(Double.POSITIVE_INFINITY);
+  private Angle reverseLimit = Radians.of(Double.NEGATIVE_INFINITY);
 
   /**
    *
@@ -61,283 +92,413 @@ public class BrushlessMotorSim {
    *
    * @param motor the {@link DCMotor} model representing the motor(s) in the simulation
    * @param gearRatio the gear ratio of the mechanism; values greater than 1 indicate a reduction
-   * @param loadIntertiaJKgMetersSquared the rotational inertia of the mechanism, in kg·m²
-   * @param frictionVoltage the voltage required to overcome friction and make the mechanism move
+   * @param loadIntertia the rotational inertia of the mechanism
+   * @param frictionVoltage the voltage required to keep the motor moving at a constant velocity
    */
   public BrushlessMotorSim(
+      SimulatedField arena,
       DCMotor motor,
       double gearRatio,
-      double loadIntertiaJKgMetersSquared,
-      double frictionVoltage) {
+      MomentOfInertia loadIntertia,
+      Voltage frictionVoltage) {
+    this.sim =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                motor, loadIntertia.in(KilogramSquareMeters), gearRatio),
+            motor);
     this.motor = motor;
-    this.gearRatio = gearRatio;
-    this.frictionTorque = motor.getTorque(motor.getCurrent(0, frictionVoltage)) * gearRatio;
-
-    this.loadInertiaJKgMetersSquared = loadIntertiaJKgMetersSquared;
-    disableCurrentLimit();
-    this.breakModeEnabled = true;
+    this.gearing = gearRatio;
+    this.frictionVoltage = frictionVoltage;
 
-    this.angularPositionRad = angularVelocityRadPerSec = appliedVolts = currentDrawAmps = 0;
+    arena.addMotor(this);
   }
 
   /**
+   * Configures the angle of the motor.
    *
-   *
-   * <h2>Requests an Output Voltage for the Motor.</h2>
-   *
-   * <p><strong>Note:</strong> The requested voltage may not always be fully applied due to current
-   * limits. For details on current limiting, refer to {@link #enableCurrentLimit(double)}.
-   *
-   * @param volts the requested output voltage for the motor
+   * @param angle the angle of the motor
+   * @return this instance for method chaining
    */
-  public void requestVoltageOutput(double volts) {
-    this.requestedVoltageOutput = volts;
+  public BrushlessMotorSim withOverrideAngle(Angle angle) {
+    sim.setAngle(angle.in(Radians));
+    return this;
   }
 
   /**
+   * Configures the angular velocity of the motor.
    *
-   *
-   * <h2>Configures a Current Limit for the Motor.</h2>
-   *
-   * <p>If the motor's supply current exceeds the specified limit, the motor will reduce its output
-   * to prevent exceeding the limit.
-   *
-   * @param currentLimitAmps the maximum allowed current, in amperes
+   * @param angularVelocity the angular velocity of the motor
+   * @return this instance for method chaining
    */
-  public void enableCurrentLimit(double currentLimitAmps) {
-    this.currentLimitAmps = currentLimitAmps;
+  public BrushlessMotorSim withOverrideAngularVelocity(AngularVelocity angularVelocity) {
+    sim.setAngularVelocity(angularVelocity.in(RadiansPerSecond));
+    return this;
+  }
+
+  public BrushlessMotorSim withFeedForward(
+      Voltage kS,
+      Per<VoltageUnit, AngularVelocityUnit> kV,
+      Per<VoltageUnit, AngularAccelerationUnit> kA) {
+    var kVUnit = PerUnit.combine(Volts, RadiansPerSecond);
+    var kAUnit = PerUnit.combine(Volts, RadiansPerSecondPerSecond);
+    feedforward =
+        new SimpleMotorFeedforward(
+            kS.in(Volts), kV.in(kVUnit), kA.in(kAUnit), SimulatedField.getSimulationDt());
+    return this;
   }
 
   /**
+   * Configures the PD controller for Positional Requests using {@link OutputType#VOLTAGE}.
    *
+   * <p>This is unit safe and can be configure like so:
    *
-   * <h2>Disables the Current Limit of the Motor.</h2>
+   * <pre><code>
+   * // Volts per Rotation of error is how CTRE handles PID when used with voltage requests
+   * sim.withPositionalVoltageController(
+   *   Volts.per(Rotation).ofNative(100.0),
+   *   Volts.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   * </code></pre>
    *
-   * <p>This method removes the current limit, allowing the motor to operate without any current
-   * restrictions.
+   * @param kP the proportional gain
+   * @param kD the derivative gain
+   * @return this instance for method chaining
    */
-  public void disableCurrentLimit() {
-    this.currentLimitAmps = Double.POSITIVE_INFINITY;
+  public BrushlessMotorSim withPositionalVoltageController(
+      Per<VoltageUnit, AngleUnit> kP, Per<VoltageUnit, AngularVelocityUnit> kD) {
+    var kPUnit = PerUnit.combine(Volts, Radians);
+    var kDUnit = PerUnit.combine(Volts, RadiansPerSecond);
+    poseVoltController.setP(kP.in(kPUnit));
+    poseVoltController.setD(kD.in(kDUnit));
+    return this;
   }
 
   /**
+   * Configures the PD controller for Velocity Requests using {@link OutputType#VOLTAGE}.
    *
+   * <p>This is unit safe and can be configure like so:
    *
-   * <h2>Configures the Zero Power Behavior of the Motor.</h2>
-   *
-   * <p>This method sets the motor's zero power behavior, similar to the <code>
-   * setZeroPowerBehavior()</code> method for brushless motors.
+   * <pre><code>
+   * // Volts per RPS of error is how CTRE handles PID when used with voltage requests
+   * sim.withVelocityVoltageController(
+   *   Volts.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   * </code></pre>
    *
-   * <p>Use this feature only when simulating brushless motors.
-   *
-   * @param enabled <code>true</code> to enable brake mode, <code>false</code> to enable coast mode
+   * @param kP the proportional gain
+   * @return this instance for method chaining
    */
-  public void setMotorBrakeEnabled(boolean enabled) {
-    this.breakModeEnabled = enabled;
+  public BrushlessMotorSim withVelocityVoltageController(Per<VoltageUnit, AngleUnit> kP) {
+    var kPUnit = PerUnit.combine(Volts, Radians);
+    veloVoltController.setP(kP.in(kPUnit));
+    return this;
   }
 
   /**
+   * Configures the PD controller for Positional Requests using {@link OutputType#CURRENT}.
    *
+   * <p>This is unit safe and can be configure like so:
    *
-   * <h2>Sets the Current State of the Motor.</h2>
-   *
-   * <p>This method instantly shifts the motor to a given state, setting its angular position and
-   * velocity.
+   * <pre><code>
+   * // Amps per Rotation of error is how CTRE handles PID when used with current requests
+   * sim.withPositionalCurrentController(
+   *   Amps.per(Rotation).ofNative(100.0),
+   *   Amps.per(RotationsPerSecond).ofNative(5.0)
+   * );
+   * </code></pre>
    *
-   * <p>Equivalent to the {@link DCMotorSim#setState(double, double)} method.
-   *
-   * @param angularPositionRad the angular position of the motor, in radians
-   * @param angularVelocityRadPerSec the angular velocity of the motor, in radians per second
+   * @param kP the proportional gain
+   * @param kD the derivative gain
+   * @return this instance for method chaining
    */
-  public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
-    this.angularPositionRad = angularPositionRad;
-    this.angularVelocityRadPerSec = angularVelocityRadPerSec;
+  public BrushlessMotorSim withPositionalCurrentController(
+      Per<CurrentUnit, AngleUnit> kP, Per<CurrentUnit, AngularVelocityUnit> kD) {
+    var kPUnit = PerUnit.combine(Amps, Radians);
+    var kDUnit = PerUnit.combine(Amps, RadiansPerSecond);
+    poseCurrentController.setP(kP.in(kPUnit));
+    poseCurrentController.setD(kD.in(kDUnit));
+    return this;
   }
 
   /**
+   * Configures the PD controller for Velocity Requests using {@link OutputType#CURRENT}.
    *
+   * <p>This is unit safe and can be configure like so:
    *
-   * <h2>Updates the Simulation.</h2>
-   *
-   * <p>This method steps the motor simulation forward by a given time interval (<code>dt</code>),
-   * recalculating and storing the states of the motor.
+   * <pre><code>
+   * // Amps per RPS of error is how CTRE handles PID when used with current requests
+   * sim.withVelocityCurrentController(
+   *   Amps.per(RotationsPerSecond).ofNative(0.4)
+   * );
+   * </code></pre>
    *
-   * <p>Equivalent to {@link DCMotorSim#update(double)}.
-   *
-   * @param dtSeconds the time step for the simulation, in seconds
+   * @param kP the proportional gain
+   * @return this instance for method chaining
    */
-  public void update(double dtSeconds) {
-    appliedVolts =
-        constrainOutputVoltage(
-            motor, angularVelocityRadPerSec * gearRatio, currentLimitAmps, requestedVoltageOutput);
-
-    double totalTorque = getMotorElectricTorque();
-
-    /* apply friction force */
-    final boolean electricTorqueResultingInAcceleration =
-        totalTorque * angularVelocityRadPerSec > 0;
-    if (electricTorqueResultingInAcceleration)
-      totalTorque = MathUtil.applyDeadband(totalTorque, frictionTorque, Double.POSITIVE_INFINITY);
-    else totalTorque += getCurrentFrictionTorque();
-
-    this.angularVelocityRadPerSec += totalTorque / loadInertiaJKgMetersSquared * dtSeconds;
-    this.angularPositionRad += this.angularVelocityRadPerSec * dtSeconds;
+  public BrushlessMotorSim withVelocityCurrentController(Per<CurrentUnit, AngleUnit> kP) {
+    var kPUnit = PerUnit.combine(Amps, Radians);
+    veloCurrentController.setP(kP.in(kPUnit));
+    return this;
   }
 
   /**
+   * Configures the positionaly controllers to use continuous wrap.
    *
-   *
-   * <h2>Calculates the Electric Torque on the Rotor.</h2>
-   *
-   * @return the torque applied to the mechanism by the motor's electrical output
+   * @param min the minimum angle
+   * @param max the maximum angle
+   * @return this instance for method chaining
+   * @see PIDController#enableContinuousInput(double, double)
    */
-  private double getMotorElectricTorque() {
-    if (!breakModeEnabled && appliedVolts == 0) return currentDrawAmps = 0;
-    currentDrawAmps = motor.getCurrent(angularVelocityRadPerSec * gearRatio, appliedVolts);
-    return motor.getTorque(currentDrawAmps) * gearRatio;
+  public BrushlessMotorSim withControllerContinousInput(Angle min, Angle max) {
+    poseVoltController.enableContinuousInput(min.in(Radians), max.in(Radians));
+    poseCurrentController.enableContinuousInput(min.in(Radians), max.in(Radians));
+    return this;
   }
 
   /**
+   * Configures the current limit for the motor.
    *
+   * <p>This is the total current limit for the sim
    *
-   * <h2>Calculates the Dynamic Friction Torque on the Mechanism.</h2>
-   *
-   * <p>This method simulates the amount of dynamic friction acting on the rotor when the mechanism
-   * is rotating.
-   *
-   * <p>In the real world, dynamic friction torque is typically constant. However, in the
-   * simulation, it is made proportional to the rotor speed when the speed is small to avoid
-   * oscillation.
-   *
-   * @return the amount of dynamic friction torque on the mechanism, in Newton-meters
+   * @param currentLimit the current limit for the motor
+   * @return
    */
-  private double getCurrentFrictionTorque() {
-    final double kFriction = 3.0,
-        percentAngularVelocity =
-            Math.abs(angularVelocityRadPerSec) * gearRatio / motor.freeSpeedRadPerSec,
-        currentFrictionTorqueMagnitude =
-            Math.min(percentAngularVelocity * kFriction * frictionTorque, frictionTorque);
-    return Math.copySign(currentFrictionTorqueMagnitude, -angularVelocityRadPerSec);
+  public BrushlessMotorSim withStatorCurrentLimit(Current currentLimit) {
+    // this is a limit across the sum of all motors output,
+    // so it should be set to the total current limit of the mechanism
+    this.currentLimit = currentLimit;
+    return this;
   }
 
   /**
+   * Configures the hard limits for the motor.
    *
-   *
-   * <h2>Constrains the Output Voltage of the Motor.</h2>
-   *
-   * <p>This method constrains the output voltage of the motor to ensure it operates within the
-   * current limit and does not exceed the available voltage from the robot's system.
-   *
-   * <p>The output voltage is constrained such that the motor does not function outside of the
-   * specified current limit. Additionally, it ensures that the voltage does not exceed the robot's
-   * input voltage, which can be obtained from {@link RoboRioSim#getVInVoltage()}.
-   *
-   * @param motor the {@link DCMotor} that models the motor
-   * @param motorCurrentVelocityRadPerSec the current velocity of the motor's rotor (geared), in
-   *     radians per second
-   * @param currentLimitAmps the configured current limit, in amperes. You can configure the current
-   *     limit using {@link #enableCurrentLimit(double)}.
-   * @param requestedOutputVoltage the requested output voltage
-   * @return the constrained output voltage based on the current limit and system voltage
+   * @param forwardLimit the forward limit
+   * @param reverseLimit the reverse limit
+   * @return this instance for method chaining
    */
-  public static double constrainOutputVoltage(
-      DCMotor motor,
-      double motorCurrentVelocityRadPerSec,
-      double currentLimitAmps,
-      double requestedOutputVoltage) {
-    final double currentAtRequestedVolts =
-        motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage);
+  public BrushlessMotorSim withHardLimits(Angle forwardLimit, Angle reverseLimit) {
+    this.forwardLimit = forwardLimit;
+    this.reverseLimit = reverseLimit;
+    return this;
+  }
 
-    /* normally, motor controller starts cutting the supply voltage when the current exceed 120% the current limit */
-    final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * currentLimitAmps;
-    double limitedVoltage = requestedOutputVoltage;
-    if (currentTooHigh) {
-      final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts);
-      limitedVoltage =
-          motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec);
+  public MomentOfInertia getMOI() {
+    return KilogramSquareMeters.of(sim.getJKgMetersSquared());
+  }
+
+  public Angle getPosition() {
+    return Radians.of(sim.getAngularPositionRad());
+  }
+
+  public AngularVelocity getVelocity() {
+    return RadiansPerSecond.of(sim.getAngularVelocityRadPerSec());
+  }
+
+  public AngularAcceleration getAcceleration() {
+    return RadiansPerSecondPerSecond.of(sim.getAngularAccelerationRadPerSecSq());
+  }
+
+  public Current getStatorCurrentDraw() {
+    return Amps.of(sim.getCurrentDrawAmps());
+  }
+
+  public Current getSupplyCurrent() {
+    // https://www.chiefdelphi.com/t/current-limiting-talonfx-values/374780/10
+    return getStatorCurrentDraw()
+        .times(sim.getInputVoltage() / RobotController.getBatteryVoltage());
+  }
+
+  public Voltage getRotorVoltage() {
+    return Volts.of(sim.getInputVoltage());
+  }
+
+  public Voltage getSupplyVoltage() {
+    return Volts.of(RobotController.getBatteryVoltage());
+  }
+
+  public void setControl(OutputType outputType, AngularVelocity velo) {
+    this.outputType = outputType;
+    this.outputMode = OutputMode.VELOCITY;
+    this.output = velo.in(RadiansPerSecond);
+  }
+
+  public void setControl(OutputType outputType, Angle pos) {
+    this.outputType = outputType;
+    this.outputMode = OutputMode.POSITION;
+    this.output = pos.in(Radians);
+  }
+
+  public void setControl(Current amps) {
+    this.outputType = OutputType.CURRENT;
+    this.outputMode = OutputMode.OPEN_LOOP;
+    this.output = amps.in(Amps);
+  }
+
+  public void setControl(Voltage volts) {
+    this.outputType = OutputType.VOLTAGE;
+    this.outputMode = OutputMode.OPEN_LOOP;
+    this.output = volts.in(Volts);
+  }
+
+  public void setControl() {
+    this.outputType = OutputType.VOLTAGE;
+    this.outputMode = OutputMode.OPEN_LOOP;
+    this.output = 0.0;
+  }
+
+  public void update() {
+    double dtSeconds = SimulatedField.getSimulationDt();
+    switch (this.outputType) {
+      case VOLTAGE -> {
+        switch (this.outputMode) {
+          case OPEN_LOOP -> {
+            driveAtVoltage(Volts.of(output));
+          }
+          case POSITION -> {
+            Voltage voltage =
+                Volts.of(poseVoltController.calculate(getPosition().in(Radians), output));
+            Voltage feedforwardVoltage =
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        velocityForVolts(voltage).in(RadiansPerSecond)));
+            driveAtVoltage(feedforwardVoltage.plus(voltage));
+          }
+          case VELOCITY -> {
+            Voltage voltage =
+                Volts.of(veloVoltController.calculate(getVelocity().in(RadiansPerSecond), output));
+            Voltage feedforwardVoltage =
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        RadiansPerSecond.of(output).in(RadiansPerSecond)));
+            driveAtVoltage(voltage.plus(feedforwardVoltage));
+          }
+        }
+      }
+      case CURRENT -> {
+        switch (this.outputMode) {
+          case OPEN_LOOP -> {
+            sim.setInputVoltage(voltsForAmps(Amps.of(output), getVelocity()).in(Volts));
+          }
+          case POSITION -> {
+            Current current =
+                Amps.of(poseCurrentController.calculate(getPosition().in(Radians), output));
+            Voltage voltage = voltsForAmps(current, getVelocity());
+            Voltage feedforwardVoltage =
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        velocityForVolts(voltage).in(RadiansPerSecond)));
+            driveAtVoltage(feedforwardVoltage.plus(voltage));
+          }
+          case VELOCITY -> {
+            Current current =
+                Amps.of(veloCurrentController.calculate(getPosition().in(Radians), output));
+            Voltage feedforwardVoltage =
+                Volts.of(
+                    feedforward.calculate(
+                        getVelocity().in(RadiansPerSecond),
+                        RadiansPerSecond.of(output).in(RadiansPerSecond)));
+            Voltage voltage = voltsForAmps(current, getVelocity()).plus(feedforwardVoltage);
+            driveAtVoltage(voltage);
+          }
+        }
+      }
     }
 
-    if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage))
-      limitedVoltage = requestedOutputVoltage;
+    sim.update(dtSeconds);
 
-    return MathUtil.clamp(limitedVoltage, -RoboRioSim.getVInVoltage(), RoboRioSim.getVInVoltage());
+    if (getPosition().lte(reverseLimit)) {
+      sim.setState(reverseLimit.in(Radians), 0.0);
+    } else if (getPosition().gte(forwardLimit)) {
+      sim.setState(forwardLimit.in(Radians), 0.0);
+    }
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Voltage Actually Applied to the Motor.</h2>
-   *
-   * <p>This method returns the voltage that is actually applied to the motor after being
-   * constrained by {@link #constrainOutputVoltage(DCMotor, double, double, double)}.
-   *
-   * <p>The voltage is constrained to ensure that the current limit is not exceeded and that it does
-   * not surpass the robot's battery voltage.
-   *
-   * @return the constrained voltage actually applied to the motor
-   */
-  public double getAppliedVolts() {
-    return appliedVolts;
+  private void driveAtVoltage(Voltage voltage) {
+    // The voltage constrained to current limits and battery voltage
+    Voltage constrained = constrainOutputVoltage(voltage);
+    Voltage frictionVoltage = applyFriction(constrained);
+
+    sim.setInputVoltage(frictionVoltage.in(Volts));
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Angular Position of the Mechanism.</h2>
-   *
-   * <p>This method is equivalent to {@link DCMotorSim#getAngularPositionRad()}.
-   *
-   * @return the final angular position of the mechanism, in radians
-   */
-  public double getAngularPositionRad() {
-    return angularPositionRad;
+  private Voltage applyFriction(Voltage voltage) {
+    // This function is responsible for slowing down acceleration
+    // and slowing down the velocity of the motor when at lowere output.
+    // This is not the same as the static friction, which is the force
+    // required to get the motor moving.
+
+    // to apply friction we convert the motors output to torque then back to voltage
+    double current =
+        motor.getCurrent(sim.getAngularVelocityRadPerSec() * gearing, voltage.in(Volts));
+    double currentVelo = getVelocity().in(RadiansPerSecond) * gearing;
+    double torque = motor.getTorque(current);
+    double friction = frictionTorque().in(NewtonMeters);
+
+    boolean movingForward = currentVelo > 0;
+
+    if (movingForward && currentVelo > motor.getSpeed(torque, sim.getInputVoltage())) {
+      // the motor is moving faster than it should based on the output voltage
+      // apply the friction to slow it down
+      torque -= friction;
+    } else if (!movingForward && currentVelo < motor.getSpeed(torque, sim.getInputVoltage())) {
+      // the motor is moving slower than it should based on the output voltage
+      // apply the friction to speed it up
+      torque += friction;
+    }
+
+    return Volts.of(motor.getVoltage(torque, currentVelo));
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Angular Position of the Motor.</h2>
-   *
-   * @return the un-geared angular position of the motor (encoder reading), in radians
-   */
-  public double getEncoderPositionRad() {
-    return angularPositionRad * gearRatio;
+  private Voltage voltsForAmps(Current current, AngularVelocity angularVelocity) {
+    // find what voltage is needed to get the current
+    return Volts.of(
+        motor.getVoltage(current.in(Amps), angularVelocity.in(RadiansPerSecond) * gearing));
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Angular Velocity of the Mechanism.</h2>
-   *
-   * <p>This method is equivalent to {@link DCMotorSim#getAngularVelocityRadPerSec()}.
-   *
-   * @return the final angular velocity of the mechanism, in radians per second
-   */
-  public double getAngularVelocityRadPerSec() {
-    return angularVelocityRadPerSec;
+  private AngularVelocity velocityForVolts(Voltage voltage) {
+    return RadiansPerSecond.of(
+        motor.getSpeed(motor.getTorque(getStatorCurrentDraw().in(Amps)), voltage.in(Volts)));
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Angular Velocity of the Encoder.</h2>
-   *
-   * @return the un-geared angular velocity of the motor (encoder velocity), in radians per second
-   */
-  public double getEncoderVelocityRadPerSec() {
-    return angularVelocityRadPerSec * gearRatio;
+  private Torque frictionTorque() {
+    return NewtonMeters.of(
+        motor.getTorque(motor.getCurrent(0.0, frictionVoltage.in(Volts))) * gearing);
   }
 
-  /**
-   *
-   *
-   * <h2>Obtains the Amount of Current Flowing into the Motor.</h2>
-   *
-   * <p>This method is equivalent to {@link DCMotorSim#getCurrentDrawAmps()}.
-   *
-   * @return the amount of current flowing into the motor, in amperes
-   */
-  public double getCurrentDrawAmps() {
-    return currentDrawAmps;
+  private Voltage constrainOutputVoltage(Voltage requestedOutput) {
+    final double kCurrentThreshold = 1.2;
+
+    final double motorCurrentVelocityRadPerSec = getVelocity().in(RadiansPerSecond);
+    final double currentLimitAmps = currentLimit.in(Amps);
+    final double requestedOutputVoltage = requestedOutput.in(Volts);
+    final double currentAtRequestedVolts =
+        motor.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltage);
+
+    // Resource for current limiting:
+    // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3)
+    final boolean currentTooHigh =
+        Math.abs(currentAtRequestedVolts) > (kCurrentThreshold * currentLimitAmps);
+    double limitedVoltage = requestedOutputVoltage;
+    if (currentTooHigh) {
+      final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVolts);
+      limitedVoltage =
+          motor.getVoltage(motor.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec);
+    }
+
+    // ensure the current limit doesn't cause an increase to output voltage
+    if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltage)) {
+      limitedVoltage = requestedOutputVoltage;
+    }
+
+    // constrain the output voltage to the battery voltage
+    return Volts.of(
+        MathUtil.clamp(
+            limitedVoltage,
+            -RobotController.getBatteryVoltage(),
+            RobotController.getBatteryVoltage()));
   }
 }
diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java
index 74235ba..0702d5e 100644
--- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java
+++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/GyroSimulation.java
@@ -170,7 +170,7 @@ public static GyroSimulation createPigeon2() {
    * IMU</a>
    */
   public static GyroSimulation createNavX2() {
-    return new GyroSimulation(2, 0.04);
+    return new GyroSimulation(0.00208333333, 0.04);
   }
 
   /**
diff --git a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java
index cd5fa62..5c2c25e 100644
--- a/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java
+++ b/src/main/java/frc/robot/extras/simulation/mechanismSim/swerve/SwerveModuleSimulation.java
@@ -1,42 +1,85 @@
 package frc.robot.extras.simulation.mechanismSim.swerve;
 
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.KilogramSquareMeters;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
 import static edu.wpi.first.units.Units.Volts;
-import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_DT;
-import static frc.robot.extras.simulation.field.SimulatedField.SIMULATION_SUB_TICKS_IN_1_PERIOD;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
+import frc.robot.extras.simulation.field.SimulatedField;
+import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
 import java.util.Queue;
 import java.util.concurrent.ConcurrentLinkedQueue;
 import java.util.function.Supplier;
 import org.dyn4j.geometry.Vector2;
 
+/**
+ *
+ *
+ * <h2>Simulation for a Single Swerve Module.</h2>
+ *
+ * <p>This class provides a simulation for a single swerve module in the {@link
+ * SwerveDriveSimulation}.
+ *
+ * <h3>1. Purpose</h3>
+ *
+ * <p>This class serves as the bridge between your code and the physics engine.
+ *
+ * <p>You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder
+ * readings in your code, just as how you deal with your physical motors.
+ *
+ * <h3>2. Perspectives</h3>
+ *
+ * <ul>
+ *   <li>Simulates the steering mechanism using a custom brushless motor simulator.
+ *   <li>Simulates the propelling force generated by the driving motor, with a current limit.
+ *   <li>Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}.
+ * </ul>
+ *
+ * <h3>3. Simulating Odometry</h3>
+ *
+ * <ul>
+ *   <li>Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPositionRad()}} and
+ *       {@link #getTurnAbsolutePosition()}.
+ *   <li>Use {@link SwerveDriveOdometry} to estimate the pose of your robot.
+ *   <li><a
+ *       href="https://v6.docs.ctr-electronics.com/en/latest/docs/application-notes/update-frequency-impact.html">250Hz
+ *       Odometry</a> is supported. You can retrive cached encoder readings from every sub-tick
+ *       through {@link #getCachedDriveEncoderUnGearedPositionsRad()} and {@link
+ *       #getCachedTurnAbsolutePositions()}.
+ * </ul>
+ *
+ * <p>An example of how to simulate odometry using this class is the <a
+ * href='https://github.com/Shenzhen-Robotics-Alliance/maple-sim/blob/main/templates/AdvantageKit_AdvancedSwerveDriveProject/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java'>ModuleIOSim.java</a>
+ * from the <code>Advanced Swerve Drive with maple-sim</code> example.
+ */
 public class SwerveModuleSimulation {
-  private final DCMotor DRIVE_MOTOR;
+  public final DCMotor DRIVE_MOTOR;
   private final BrushlessMotorSim turnMotorSim;
-  private final double DRIVE_CURRENT_LIMIT,
+  public final double DRIVE_CURRENT_LIMIT,
       DRIVE_GEAR_RATIO,
       TURN_GEAR_RATIO,
       DRIVE_FRICTION_VOLTAGE,
-      TURN_FRICTION_VOLTAGE,
       WHEELS_COEFFICIENT_OF_FRICTION,
       WHEEL_RADIUS_METERS,
       DRIVE_WHEEL_INERTIA = 0.01;
   private double driveMotorRequestedVolts = 0.0,
-      turnMotorAppliedVolts = 0.0,
       driveMotorAppliedVolts = 0.0,
       driveMotorSupplyCurrentAmps = 0.0,
-      turnMotorSupplyCurrentAmps = 0.0,
       turnRelativeEncoderPositionRad = 0.0,
       turnRelativeEncoderSpeedRadPerSec = 0.0,
       turnAbsoluteEncoderSpeedRadPerSec = 0.0,
       driveEncoderUnGearedPositionRad = 0.0,
       driveEncoderUnGearedSpeedRadPerSec = 0.0;
-  private Rotation2d turnAbsoluteRotation = Rotation2d.fromRotations(Math.random());
+  private Rotation2d turnAbsolutePosition = Rotation2d.fromRotations(Math.random());
 
   private final double turnRelativeEncoderOffSet = (Math.random() - 0.5) * 30;
 
@@ -44,6 +87,31 @@ public class SwerveModuleSimulation {
       cachedDriveEncoderUnGearedPositionsRad;
   private final Queue<Rotation2d> cachedTurnAbsolutePositions;
 
+  /**
+   *
+   *
+   * <h2>Constructs a Swerve Module Simulation.</h2>
+   *
+   * <p>If you are using {@link SimulatedField#overrideSimulationTimings(double, int)} to use custom
+   * timings, you must call the method before constructing any swerve module simulations using this
+   * constructor.
+   *
+   * @param driveMotor the model of the driving motor
+   * @param turnMotor the model of the steering motor
+   * @param driveCurrentLimit the current limit for the driving motor, in amperes
+   * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction
+   * @param turnGearRatio the gear ratio for the steering motor, >1 is reduction
+   * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving
+   *     rotter, in volts
+   * @param turnFrictionVoltage the measured minimum amount of voltage that can turn the steering
+   *     rotter, in volts
+   * @param tireCoefficientOfFriction the <a
+   *     href='https://simple.wikipedia.org/wiki/Coefficient_of_friction#:~:text=A%20coefficient%20of%20friction%20is%20a%20value%20that%20shows%20the'>coefficient
+   *     of friction</a> of the tires, normally around 1.5
+   * @param wheelsRadiusMeters the radius of the wheels, in meters. Calculate it using {@link
+   *     Units#inchesToMeters(double)}.
+   * @param turnRotationalInertia the rotational inertia of the steering mechanism
+   */
   public SwerveModuleSimulation(
       DCMotor driveMotor,
       DCMotor turnMotor,
@@ -60,112 +128,309 @@ public SwerveModuleSimulation(
     DRIVE_GEAR_RATIO = driveGearRatio;
     TURN_GEAR_RATIO = turnGearRatio;
     DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage;
-    TURN_FRICTION_VOLTAGE = turnFrictionVoltage;
     WHEELS_COEFFICIENT_OF_FRICTION = tireCoefficientOfFriction;
     WHEEL_RADIUS_METERS = wheelsRadiusMeters;
 
     this.turnMotorSim =
         new BrushlessMotorSim(
-            turnMotor, TURN_GEAR_RATIO, turnRotationalInertia, TURN_FRICTION_VOLTAGE);
+            SimulatedField.getInstance(),
+            turnMotor,
+            turnGearRatio,
+            KilogramSquareMeters.of(turnRotationalInertia),
+            Volts.of(turnFrictionVoltage));
 
     this.cachedDriveEncoderUnGearedPositionsRad = new ConcurrentLinkedQueue<>();
-    for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++)
+    for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++)
       cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad);
     this.cachedTurnRelativeEncoderPositionsRad = new ConcurrentLinkedQueue<>();
-    for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++)
+    for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++)
       cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad);
     this.cachedTurnAbsolutePositions = new ConcurrentLinkedQueue<>();
-    for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++)
-      cachedTurnAbsolutePositions.offer(turnAbsoluteRotation);
+    for (int i = 0; i < SimulatedField.getSimulationSubTicksIn1Period(); i++)
+      cachedTurnAbsolutePositions.offer(turnAbsolutePosition);
 
     this.turnRelativeEncoderPositionRad =
-        turnAbsoluteRotation.getRadians() + turnRelativeEncoderOffSet;
-  }
-
-  public void requestDriveVoltageOut(double volts) {
-    this.driveMotorRequestedVolts = volts;
-  }
-
-  public void requestTurnVoltageOut(double volts) {
-    this.turnMotorAppliedVolts = volts;
-    // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE,
-    // 12));
+        turnAbsolutePosition.getRadians() + turnRelativeEncoderOffSet;
   }
 
+  /**
+   *
+   *
+   * <h2>Requests the Driving Motor to Run at a Specified Voltage Output.</h2>
+   *
+   * <h3>Think of it as the setVoltage() of your physical driving motor.</h3>
+   *
+   * <p>This method sets the desired voltage output for the driving motor. The change will be
+   * applied in the next sub-tick of the simulation.
+   *
+   * <p><strong>Note:</strong> The requested voltage may not always be fully applied if the current
+   * is too high. The current limit may reduce the motor's output, similar to real motors.
+   *
+   * <p>To check the actual voltage applied to the drivetrain, use {@link
+   * #getDriveMotorAppliedVolts()}.
+   *
+   * @param volts the voltage to be applied to the driving motor
+   */
   public void requestDriveVoltageOut(Voltage volts) {
     this.driveMotorRequestedVolts = volts.in(Volts);
   }
 
+  /**
+   *
+   *
+   * <h2>Requests the Steering Motor to Run at a Specified Voltage Output.</h2>
+   *
+   * <h3>Think of it as the setVoltage() of your physical steering motor.</h3>
+   *
+   * <p>This method sets the desired voltage output for the steering motor. The change will be
+   * applied in the next sub-tick of the simulation.
+   *
+   * <p><strong>Note:</strong> Similar to the drive motor, the requested voltage may not always be
+   * fully applied if the current exceeds the limit. The current limit will reduce the motor's
+   * output as needed, mimicking real motor behavior.
+   *
+   * <p>To check the actual voltage applied to the steering motor, use {@link
+   * #getTurnMotorAppliedVolts()}.
+   *
+   * @param volts the voltage to be applied to the steering motor
+   */
   public void requestTurnVoltageOut(Voltage volts) {
-    this.turnMotorAppliedVolts = volts.in(Volts);
-    // this.turnMotorSim.setInputVoltage(MathUtil.applyDeadband(volts, turn_FRICTION_VOLTAGE,
-    // 12));
+    this.turnMotorSim.setControl(volts);
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Actual Output Voltage of the Drive Motor.</h2>
+   *
+   * <p>This method returns the actual voltage being applied to the drive motor. The actual applied
+   * voltage may differ from the value set by {@link #requestDriveVoltageOut(double)}.
+   *
+   * <p>If the motor's supply current is too high, the motor will automatically reduce its output
+   * voltage to protect the system.
+   *
+   * @return the actual output voltage of the drive motor, in volts
+   */
   public double getDriveMotorAppliedVolts() {
     return driveMotorAppliedVolts;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Actual Output Voltage of the Steering Motor.</h2>
+   *
+   * <p>This method returns the actual voltage being applied to the steering motor. It wraps around
+   * the {@link BrushlessMotorSim#getAppliedVolts()} method.
+   *
+   * <p>The actual applied voltage may differ from the value set by {@link
+   * #requestTurnVoltageOut(double)}. If the motor's supply current is too high, the motor will
+   * automatically reduce its output voltage to protect the system.
+   *
+   * @return the actual output voltage of the steering motor, in volts
+   */
   public double getTurnMotorAppliedVolts() {
-    return turnMotorAppliedVolts;
+    return turnMotorSim.getRotorVoltage().in(Volts);
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Amount of Current Supplied to the Drive Motor.</h2>
+   *
+   * <h3>Think of it as the getSupplyCurrent() of your physical drive motor.</h3>
+   *
+   * @return the current supplied to the drive motor, in amperes
+   */
   public double getDriveMotorSupplyCurrentAmps() {
     return driveMotorSupplyCurrentAmps;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Amount of Current Supplied to the Steer Motor.</h2>
+   *
+   * <h3>Think of it as the getSupplyCurrent() of your physical steer motor.</h3>
+   *
+   * <p>This method wraps around {@link BrushlessMotorSim#getCurrentDrawAmps()}.
+   *
+   * @return the current supplied to the steer motor, in amperes
+   */
   public double getTurnMotorSupplyCurrentAmps() {
-    return turnMotorSupplyCurrentAmps;
+    return turnMotorSim.getSupplyCurrent().in(Amps);
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Position of the Drive Encoder.</h2>
+   *
+   * <h3>Think of it as the getPosition() of your physical driving motor.</h3>
+   *
+   * <p>This method is used to simulate your {@link SwerveDrivePoseEstimator}.
+   *
+   * <p>This value represents the un-geared position of the encoder, i.e., the amount of radians the
+   * drive motor's encoder has rotated.
+   *
+   * <p>To get the final wheel rotations, use {@link #getDriveEncoderFinalPositionRad()}.
+   *
+   * @return the position of the drive motor's encoder, in radians (un-geared)
+   */
   public double getDriveEncoderUnGearedPositionRad() {
     return driveEncoderUnGearedPositionRad;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Final Position of the Drive Encoder (Wheel Rotations).</h2>
+   *
+   * <p>This method is used to simulate the {@link SwerveDrivePoseEstimator} by providing the final
+   * position of the drive encoder in terms of wheel rotations.
+   *
+   * <p>The value is calculated by dividing the un-geared encoder position (obtained from {@link
+   * #getDriveEncoderUnGearedPositionRad()}) by the drive gear ratio.
+   *
+   * @return the final position of the drive encoder (wheel rotations), in radians
+   */
   public double getDriveEncoderFinalPositionRad() {
     return getDriveEncoderUnGearedPositionRad() / DRIVE_GEAR_RATIO;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Speed of the Drive Encoder (Un-Geared), in Radians per Second.</h2>
+   *
+   * <h3>Think of it as the <code>getVelocity()</code> of your physical drive motor.</h3>
+   *
+   * <p>This method returns the current speed of the drive encoder in radians per second, without
+   * accounting for the drive gear ratio.
+   *
+   * @return the un-geared speed of the drive encoder, in radians per second
+   */
   public double getDriveEncoderUnGearedSpeedRadPerSec() {
     return driveEncoderUnGearedSpeedRadPerSec;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Final Speed of the Wheel, in Radians per Second.</h2>
+   *
+   * @return the final speed of the drive wheel, in radians per second
+   */
   public double getDriveWheelFinalSpeedRadPerSec() {
     return getDriveEncoderUnGearedSpeedRadPerSec() / DRIVE_GEAR_RATIO;
   }
 
-  /** geared */
+  /**
+   *
+   *
+   * <h2>Obtains the Relative Position of the Steer Encoder, in Radians.</h2>
+   *
+   * <h3>Think of it as the <code>getPosition()</code> of your physical steer motor.</h3>
+   *
+   * @return the relative encoder position of the steer motor, in radians
+   */
   public double getTurnRelativeEncoderPositionRad() {
     return turnRelativeEncoderPositionRad;
   }
 
-  /** geared */
+  /**
+   *
+   *
+   * <h2>Obtains the Speed of the Steer Relative Encoder, in Radians per Second (Geared).</h2>
+   *
+   * <h3>Think of it as the <code>getVelocity()</code> of your physical steer motor.</h3>
+   *
+   * @return the speed of the steer relative encoder, in radians per second (geared)
+   */
   public double getTurnRelativeEncoderSpeedRadPerSec() {
     return turnRelativeEncoderSpeedRadPerSec;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Absolute Facing of the Steer Mechanism.</h2>
+   *
+   * <h3>Think of it as the <code>getAbsolutePosition()</code> of your CanCoder.</h3>
+   *
+   * @return the absolute facing of the steer mechanism, as a {@link Rotation2d}
+   */
   public Rotation2d getTurnAbsolutePosition() {
-    return turnAbsoluteRotation;
+    return turnAbsolutePosition;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Absolute Rotational Velocity of the Steer Mechanism.</h2>
+   *
+   * <h3>Think of it as the <code>getVelocity()</code> of your CanCoder.</h3>
+   *
+   * @return the absolute angular velocity of the steer mechanism, in radians per second
+   */
   public double getTurnAbsoluteEncoderSpeedRadPerSec() {
     return turnAbsoluteEncoderSpeedRadPerSec;
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.</h2>
+   *
+   * <p>The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and
+   * can be retrieved using this method.
+   *
+   * @return an array of cached drive encoder un-geared positions, in radians
+   */
   public double[] getCachedDriveEncoderUnGearedPositionsRad() {
     return cachedDriveEncoderUnGearedPositionsRad.stream().mapToDouble(value -> value).toArray();
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).</h2>
+   *
+   * <p>The values of {@link #getDriveEncoderUnGearedPositionRad()} are cached at each sub-tick and
+   * are divided by the gear ratio to obtain the final wheel rotations.
+   *
+   * @return an array of cached drive encoder final positions (wheel rotations), in radians
+   */
   public double[] getCachedDriveWheelFinalPositionsRad() {
     return cachedDriveEncoderUnGearedPositionsRad.stream()
-        .mapToDouble(value -> value / DRIVE_GEAR_RATIO)
+        .mapToDouble(value -> value * ModuleConstants.DRIVE_TO_METERS)
         .toArray();
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Cached Readings of the Steer Relative Encoder's Position.</h2>
+   *
+   * <p>The values of {@link #getTurnRelativeEncoderPositionRad()} are cached at each sub-tick and
+   * can be retrieved using this method.
+   *
+   * @return an array of cached steer relative encoder positions, in radians
+   */
   public double[] getCachedTurnRelativeEncoderPositions() {
     return cachedTurnRelativeEncoderPositionsRad.stream().mapToDouble(value -> value).toArray();
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the Cached Readings of the Steer Absolute Positions.</h2>
+   *
+   * <p>The values of {@link #getTurnAbsolutePosition()} are cached at each sub-tick and can be
+   * retrieved using this method.
+   *
+   * @return an array of cached absolute steer positions, as {@link Rotation2d} objects
+   */
   public Rotation2d[] getCachedTurnAbsolutePositions() {
     return cachedTurnAbsolutePositions.toArray(Rotation2d[]::new);
   }
@@ -175,50 +440,90 @@ protected double getGrippingForceNewtons(double gravityForceOnModuleNewtons) {
   }
 
   /**
-   * updates the simulation sub-tick for this module, updating its inner status (sensor readings)
-   * and calculating a total force
    *
-   * @param moduleCurrentGroundVelocityWorldRelative
-   * @return the propelling force that the module generates
+   *
+   * <h2>Updates the Simulation for This Module.</h2>
+   *
+   * <p>This method is called once during every sub-tick of the simulation. It performs the
+   * following actions:
+   *
+   * <ul>
+   *   <li>Updates the simulation of the steering mechanism.
+   *   <li>Simulates the propelling force generated by the module.
+   *   <li>Updates and caches the encoder readings for odometry simulation.
+   * </ul>
+   *
+   * <p><strong>Note:</strong> Friction forces are not simulated in this method.
+   *
+   * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module,
+   *     relative to the world
+   * @param robotFacing the absolute facing of the robot, relative to the world
+   * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons
+   * @return the propelling force generated by the module, as a {@link Vector2} object
    */
   public Vector2 updateSimulationSubTickGetModuleForce(
       Vector2 moduleCurrentGroundVelocityWorldRelative,
-      Rotation2d robotRotation,
+      Rotation2d robotFacing,
       double gravityForceOnModuleNewtons) {
-    updateturnSimulation();
+    updateTurnSimulation();
 
     /* the maximum gripping force that the wheel can generate */
     final double grippingForceNewtons = getGrippingForceNewtons(gravityForceOnModuleNewtons);
-    final Rotation2d moduleWorldRotation = this.turnAbsoluteRotation.plus(robotRotation);
+    final Rotation2d moduleWorldFacing = this.turnAbsolutePosition.plus(robotFacing);
     final Vector2 propellingForce =
         getPropellingForce(
-            grippingForceNewtons, moduleWorldRotation, moduleCurrentGroundVelocityWorldRelative);
-    updateEncoderTicks();
+            grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative);
+    updateDriveEncoders();
 
     return propellingForce;
   }
 
-  /** */
-  private void updateturnSimulation() {
-    turnMotorSim.update(SIMULATION_DT);
-
+  /**
+   *
+   *
+   * <h2>updates the simulation for the steer mechanism.</h2>
+   *
+   * <p>Updates the simulation for the steer mechanism and cache the encoder readings.
+   *
+   * <p>The steer mechanism is modeled by a {@link BrushlessMotorSim}.
+   */
+  private void updateTurnSimulation() {
     /* update the readings of the sensor */
-    this.turnAbsoluteRotation = Rotation2d.fromRadians(turnMotorSim.getAngularPositionRad());
+    this.turnAbsolutePosition = new Rotation2d(turnMotorSim.getPosition());
     this.turnRelativeEncoderPositionRad =
-        turnMotorSim.getAngularPositionRad() + turnRelativeEncoderOffSet;
-    this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getAngularVelocityRadPerSec();
+        turnMotorSim.getPosition().in(Radians) + turnRelativeEncoderOffSet;
+    this.turnAbsoluteEncoderSpeedRadPerSec = turnMotorSim.getVelocity().in(RadiansPerSecond);
     this.turnRelativeEncoderSpeedRadPerSec = turnAbsoluteEncoderSpeedRadPerSec * TURN_GEAR_RATIO;
 
     /* cache sensor readings to queue for high-frequency odometry */
     this.cachedTurnAbsolutePositions.poll();
-    this.cachedTurnAbsolutePositions.offer(turnAbsoluteRotation);
+    this.cachedTurnAbsolutePositions.offer(turnAbsolutePosition);
     this.cachedTurnRelativeEncoderPositionsRad.poll();
     this.cachedTurnRelativeEncoderPositionsRad.offer(turnRelativeEncoderPositionRad);
   }
 
+  /**
+   *
+   *
+   * <h2>Calculates the amount of propelling force that the module generates.</h2>
+   *
+   * <p>The swerve module's drive motor will generate a propelling force.
+   *
+   * <p>For most of the time, that propelling force is directly applied to the drivetrain. And the
+   * drive wheel runs as fast as the ground velocity
+   *
+   * <p>However, if the propelling force exceeds the gripping, only the max gripping force is
+   * applied. The rest of the propelling force will cause the wheel to start skidding and make the
+   * odometry inaccurate.
+   *
+   * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons
+   * @param moduleWorldFacing the current world facing of the module
+   * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference
+   * @return a vector representing the propelling force that the module generates, world-reference
+   */
   private Vector2 getPropellingForce(
       double grippingForceNewtons,
-      Rotation2d moduleWorldRotation,
+      Rotation2d moduleWorldFacing,
       Vector2 moduleCurrentGroundVelocity) {
     final double driveWheelTorque = getDriveWheelTorque(),
         theoreticalMaxPropellingForceNewtons = driveWheelTorque / WHEEL_RADIUS_METERS;
@@ -233,39 +538,34 @@ private Vector2 getPropellingForce(
         moduleCurrentGroundVelocity.getMagnitude()
             * Math.cos(
                 moduleCurrentGroundVelocity.getAngleBetween(
-                    new Vector2(moduleWorldRotation.getRadians())));
-
-    if (skidding) {
-      /* if the chassis is skidding, part of the toque will cause the wheels to spin freely */
-      final double torqueOnWheel = driveWheelTorque * 0.3;
-      this.driveEncoderUnGearedSpeedRadPerSec +=
-          torqueOnWheel / DRIVE_WHEEL_INERTIA * SIMULATION_DT * DRIVE_GEAR_RATIO;
-    } else // if the chassis is tightly gripped on floor, the floor velocity is projected to the
-      // wheel
+                    new Vector2(moduleWorldFacing.getRadians())));
+
+    // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel
+    this.driveEncoderUnGearedSpeedRadPerSec =
+        floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO;
+
+    // if the module is skidding
+    if (skidding)
       this.driveEncoderUnGearedSpeedRadPerSec =
-          floorVelocityProjectionOnWheelDirectionMPS / WHEEL_RADIUS_METERS * DRIVE_GEAR_RATIO;
+          0.5 * driveEncoderUnGearedSpeedRadPerSec
+              + 0.5 * DRIVE_MOTOR.getSpeed(0, driveMotorAppliedVolts);
 
-    return Vector2.create(propellingForceNewtons, moduleWorldRotation.getRadians());
+    return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians());
   }
 
+  /**
+   *
+   *
+   * <h2>Calculates the amount of torque that the drive motor can generate on the wheel.</h2>
+   *
+   * <p>Before calculating the torque of the motor, the output voltage of the drive motor is
+   * constrained for the current limit through {@link
+   * BrushlessMotorSim#constrainOutputVoltage(DCMotor, double, double, double)}.
+   *
+   * @return the amount of torque on the wheel by the drive motor, in Newton * Meters
+   */
   private double getDriveWheelTorque() {
-    final double currentAtRequestedVolts =
-        DRIVE_MOTOR.getCurrent(this.driveEncoderUnGearedSpeedRadPerSec, driveMotorRequestedVolts);
-
     driveMotorAppliedVolts = driveMotorRequestedVolts;
-    /* normally, motor controller starts cutting the supply voltage when the current exceed 150% the current limit */
-    final boolean currentTooHigh = Math.abs(currentAtRequestedVolts) > 1.2 * DRIVE_CURRENT_LIMIT,
-        driveMotorTryingToAccelerate = driveMotorRequestedVolts * driveMotorSupplyCurrentAmps > 0;
-
-    if (currentTooHigh && driveMotorTryingToAccelerate) {
-      /* activate current limit, cut down the applied voltage to match current limit */
-      final double currentWithLimits = Math.copySign(DRIVE_CURRENT_LIMIT, currentAtRequestedVolts);
-      driveMotorAppliedVolts =
-          DRIVE_MOTOR.getVoltage(
-              DRIVE_MOTOR.getTorque(currentWithLimits), this.driveEncoderUnGearedSpeedRadPerSec);
-    }
-
-    driveMotorAppliedVolts = MathUtil.clamp(driveMotorAppliedVolts, -12, 12);
 
     /* calculate the actual supply current */
     driveMotorSupplyCurrentAmps =
@@ -274,21 +574,25 @@ private double getDriveWheelTorque() {
             MathUtil.applyDeadband(driveMotorAppliedVolts, DRIVE_FRICTION_VOLTAGE, 12));
 
     /* calculate the torque generated,  */
-    final double torqueOnRotor = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps);
-    return torqueOnRotor * DRIVE_GEAR_RATIO;
+    final double torqueOnRotter = DRIVE_MOTOR.getTorque(driveMotorSupplyCurrentAmps);
+    return torqueOnRotter * DRIVE_GEAR_RATIO;
   }
 
   /**
    * @return the current module state of this simulation module
    */
-  protected SwerveModuleState getCurrentState() {
+  public SwerveModuleState getCurrentState() {
     return new SwerveModuleState(
-        getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsoluteRotation);
+        getDriveWheelFinalSpeedRadPerSec() * WHEEL_RADIUS_METERS, turnAbsolutePosition);
   }
 
   /**
-   * gets the state of the module, if it is allowed to spin freely for a long time under the current
-   * applied drive volts
+   *
+   *
+   * <h2>Obtains the "free spin" state of the module</h2>
+   *
+   * <p>The "free spin" state of a simulated module refers to its state after spinning freely for a
+   * long time under the current input voltage
    *
    * @return the free spinning module state
    */
@@ -299,37 +603,88 @@ protected SwerveModuleState getFreeSpinState() {
                 driveMotorAppliedVolts)
             / DRIVE_GEAR_RATIO
             * WHEEL_RADIUS_METERS,
-        turnAbsoluteRotation);
+        turnAbsolutePosition);
   }
 
-  private void updateEncoderTicks() {
-    this.driveEncoderUnGearedPositionRad += this.driveEncoderUnGearedSpeedRadPerSec * SIMULATION_DT;
+  /**
+   *
+   *
+   * <h2>Cache the encoder values.</h2>
+   *
+   * <p>An internal method to cache the encoder values to their queues.
+   */
+  private void updateDriveEncoders() {
+    this.driveEncoderUnGearedPositionRad +=
+        this.driveEncoderUnGearedSpeedRadPerSec * SimulatedField.getSimulationDt();
     this.cachedDriveEncoderUnGearedPositionsRad.poll();
     this.cachedDriveEncoderUnGearedPositionsRad.offer(driveEncoderUnGearedPositionRad);
   }
 
+  /**
+   *
+   *
+   * <h2>Obtains the theoretical speed that the module can achieve.</h2>
+   *
+   * @return the theoretical maximum ground speed that the module can achieve, in m/s
+   */
   public double getModuleTheoreticalSpeedMPS() {
     return DRIVE_MOTOR.freeSpeedRadPerSec / DRIVE_GEAR_RATIO * WHEEL_RADIUS_METERS;
   }
 
-  public double getTheoreticalPropellingForcePerModule(double robotMass, int modulesCount) {
+  /**
+   *
+   *
+   * <h2>Obtains the theoretical maximum propelling force of ONE module.</h2>
+   *
+   * <p>Calculates the maximum propelling force with respect to the gripping force and the drive
+   * motor's torque under its current limit.
+   *
+   * @param robotMassKg the mass of the robot, is kilograms
+   * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force
+   *     equally
+   * @return the maximum propelling force of EACH module
+   */
+  public double getTheoreticalPropellingForcePerModule(double robotMassKg, int modulesCount) {
     final double
         maxThrustNewtons =
             DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS,
-        maxGrippingNewtons = 9.8 * robotMass / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION;
+        maxGrippingNewtons = 9.8 * robotMassKg / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION;
 
     return Math.min(maxThrustNewtons, maxGrippingNewtons);
   }
 
-  public double getModuleMaxAccelerationMPSsq(double robotMass, int modulesCount) {
-    return getTheoreticalPropellingForcePerModule(robotMass, modulesCount)
+  /**
+   *
+   *
+   * <h2>Obtains the theatrical linear acceleration that the robot can achieve.</h2>
+   *
+   * <p>Calculates the maximum linear acceleration of a robot, with respect to its mass and {@link
+   * #getTheoreticalPropellingForcePerModule(double, int)}.
+   *
+   * @param robotMassKg the mass of the robot, is kilograms
+   * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force
+   *     equally
+   */
+  public double getModuleMaxAccelerationMPSsq(double robotMassKg, int modulesCount) {
+    return getTheoreticalPropellingForcePerModule(robotMassKg, modulesCount)
         * modulesCount
-        / robotMass;
+        / robotMassKg;
   }
 
-  public enum DRIVE_WHEEL_TYPE {
-    RUBBER,
-    TIRE
+  /**
+   *
+   *
+   * <h2>Stores the coefficient of friction of some common used wheels.</h2>
+   */
+  public enum WHEEL_GRIP {
+    RUBBER_WHEEL(1.25),
+    TIRE_WHEEL(1.2);
+
+    public final double cof;
+
+    WHEEL_GRIP(double cof) {
+      this.cof = cof;
+    }
   }
 
   /**
@@ -340,7 +695,7 @@ public static Supplier<SwerveModuleSimulation> getModule(
       DCMotor driveMotor,
       DCMotor turnMotor,
       double driveCurrentLimitAmps,
-      DRIVE_WHEEL_TYPE driveWheelType,
+      WHEEL_GRIP driveWheelType,
       double driveGearRatio) {
     return () ->
         new SwerveModuleSimulation(
@@ -351,10 +706,7 @@ public static Supplier<SwerveModuleSimulation> getModule(
             11.3142,
             0.25,
             0.05,
-            switch (driveWheelType) {
-              case RUBBER -> 1.55;
-              case TIRE -> 1.45;
-            },
+            driveWheelType.cof,
             Units.inchesToMeters(2),
             0.05);
   }
diff --git a/src/main/java/frc/robot/extras/util/AllianceFlipper.java b/src/main/java/frc/robot/extras/util/AllianceFlipper.java
index a1fe9e4..92ffe9c 100644
--- a/src/main/java/frc/robot/extras/util/AllianceFlipper.java
+++ b/src/main/java/frc/robot/extras/util/AllianceFlipper.java
@@ -12,10 +12,9 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.Constants.FieldConstants;
 
 public class AllianceFlipper {
-  private static final double FIELD_LENGTH = 16.58112;
-
   public static boolean isBlue() {
     return DriverStation.getAlliance().orElseGet(() -> Alliance.Blue).equals(Alliance.Blue);
   }
@@ -29,7 +28,8 @@ public static boolean isRed() {
    * @return Translation2d object with its x coordinate flipped over the y-axis
    */
   public static final Translation2d flipTranslation(Translation2d translation) {
-    return new Translation2d(FIELD_LENGTH - translation.getX(), translation.getY());
+    return new Translation2d(
+        FieldConstants.FIELD_LENGTH_METERS - translation.getX(), translation.getY());
   }
 
   /**
@@ -38,7 +38,9 @@ public static final Translation2d flipTranslation(Translation2d translation) {
    */
   public static final Translation3d flipTranslation(Translation3d translation) {
     return new Translation3d(
-        FIELD_LENGTH - translation.getX(), translation.getY(), translation.getZ());
+        FieldConstants.FIELD_LENGTH_METERS - translation.getX(),
+        translation.getY(),
+        translation.getZ());
   }
 
   /**
diff --git a/src/main/java/frc/robot/extras/util/GeomUtil.java b/src/main/java/frc/robot/extras/util/GeomUtil.java
index 3b52c44..017c98e 100644
--- a/src/main/java/frc/robot/extras/util/GeomUtil.java
+++ b/src/main/java/frc/robot/extras/util/GeomUtil.java
@@ -50,7 +50,38 @@ public static ChassisSpeeds toWpilibChassisSpeeds(
         dyn4jLinearVelocity.x, dyn4jLinearVelocity.y, angularVelocityRadPerSec);
   }
 
+  /**
+   * Gets the x and y velocities of a ChassisSpeeds
+   *
+   * @param chassisSpeeds the ChassisSpeeds to retrieve velocities from
+   * @return a Translation2d containing the velocities in the x and y direction in meters per second
+   */
   public static Translation2d getChassisSpeedsTranslationalComponent(ChassisSpeeds chassisSpeeds) {
     return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond);
   }
+
+  /**
+   * Checks if two translations are within a certain threshold in meters
+   *
+   * @param t1 Translation 1 as a Translation2d
+   * @param t2 Translation 2 as a Translation2d
+   * @param thresholdMeters the threshold between the two translations in meters
+   * @return true if the two translations are within thresholdMeters
+   */
+  public static boolean isTranslationWithinThreshold(
+      Translation2d t1, Translation2d t2, double thresholdMeters) {
+    return t1.getDistance(t2) <= thresholdMeters;
+  }
+
+  /**
+   * Checks if two rotations are within a certain threshold in degrees
+   *
+   * @param r1 Rotations 1 as a Rotation2d
+   * @param r2 Rotation 2 as a Rotation2d
+   * @param thresholdMeters the threshold between the two rotations in degrees
+   * @return true if the two rotations are within thresholdDegrees
+   */
+  public static boolean isRotationWithinThreshold(double r1, double r2, double thresholdDegrees) {
+    return Math.abs(r1 - r2) <= thresholdDegrees;
+  }
 }
diff --git a/src/main/java/frc/robot/extras/util/JoystickUtil.java b/src/main/java/frc/robot/extras/util/JoystickUtil.java
new file mode 100644
index 0000000..1ffc4bb
--- /dev/null
+++ b/src/main/java/frc/robot/extras/util/JoystickUtil.java
@@ -0,0 +1,102 @@
+package frc.robot.extras.util;
+
+import frc.robot.Constants.JoystickConstants;
+import java.util.function.DoubleSupplier;
+
+public class JoystickUtil {
+
+  /**
+   * Deadbands a value to 0
+   *
+   * @param value Value to input
+   * @param deadband If the the input is within this value, the function will return 0.
+   * @return 0.0 if the value is within specified deadband range
+   */
+  private static double deadband(double value, double deadband) {
+    if (Math.abs(value) > deadband) {
+      if (value > 0.0) {
+        return (value - deadband) / (1.0 - deadband);
+      } else {
+        return (value + deadband) / (1.0 - deadband);
+      }
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Function to modify a singular joystick axis.
+   *
+   * @param supplierValue Supplies a double, usually by a joystick.
+   * @param exponent Exponent to raise the supplied value to.
+   * @return Returns the modified value.
+   */
+  public static double modifyAxis(DoubleSupplier supplierValue, int exponent) {
+    double value = supplierValue.getAsDouble();
+
+    // Deadband
+    value = deadband(value, JoystickConstants.DEADBAND_VALUE);
+
+    // Raise value to specified exponent
+    value = Math.copySign(Math.pow(value, exponent), value);
+
+    return value;
+  }
+
+  public static double modifyAxisCubed(DoubleSupplier supplierValue) {
+    double value = supplierValue.getAsDouble();
+
+    // Deadband
+    value = JoystickUtil.deadband(value, JoystickConstants.DEADBAND_VALUE);
+
+    // Cube the axis
+    value = Math.copySign(value * value * value, value);
+
+    return value;
+  }
+
+  /**
+   * Converts the two axis coordinates to polar to get both the angle and radius, so they can work
+   * in a double[] list.
+   *
+   * @param xJoystick The supplied input of the xJoystick.
+   * @param yJoystick The supplied input of the yJoystick.
+   * @param exponent The exponent to raise the supplied value to.
+   * @return The modified axises of both joysticks in polar form.
+   */
+  public static double[] modifyAxisPolar(
+      DoubleSupplier xJoystick, DoubleSupplier yJoystick, int exponent) {
+    double xInput = deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
+    double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
+    if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) {
+      double theta = Math.atan(xInput / yInput);
+      double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput);
+      double raisedHypotenuse = Math.pow(hypotenuse, exponent);
+      xInput = Math.copySign(Math.sin(theta) * raisedHypotenuse, xInput);
+      yInput = Math.copySign(Math.cos(theta) * raisedHypotenuse, yInput);
+      return new double[] {xInput, yInput};
+    }
+    return new double[] {
+      Math.copySign(Math.pow(xInput, exponent), xInput),
+      Math.copySign(Math.pow(yInput, exponent), yInput)
+    };
+  }
+
+  public static double[] modifyAxisCubedPolar(DoubleSupplier xJoystick, DoubleSupplier yJoystick) {
+    double xInput =
+        JoystickUtil.deadband(xJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
+    double yInput = deadband(yJoystick.getAsDouble(), JoystickConstants.DEADBAND_VALUE);
+    if (Math.abs(xInput) > 0 && Math.abs(yInput) > 0) {
+      double theta = Math.atan(xInput / yInput);
+      double hypotenuse = Math.sqrt(xInput * xInput + yInput * yInput);
+      double cubedHypotenuse = Math.pow(hypotenuse, 3);
+      xInput = Math.copySign(Math.sin(theta) * cubedHypotenuse, xInput);
+      yInput = Math.copySign(Math.cos(theta) * cubedHypotenuse, yInput);
+      return new double[] {xInput, yInput};
+    }
+    return new double[] {
+      Math.copySign(xInput * xInput * xInput, xInput),
+      Math.copySign(yInput * yInput * yInput, yInput)
+    };
+  }
+}
diff --git a/src/main/java/frc/robot/extras/util/MathUtil.java b/src/main/java/frc/robot/extras/util/MathUtil.java
index 65acfdf..f67401a 100644
--- a/src/main/java/frc/robot/extras/util/MathUtil.java
+++ b/src/main/java/frc/robot/extras/util/MathUtil.java
@@ -1,7 +1,6 @@
 package frc.robot.extras.util;
 
 import edu.wpi.first.math.geometry.Rotation3d;
-import frc.robot.BuildConstants;
 import java.util.Random;
 
 public class MathUtil {
@@ -9,7 +8,7 @@ public class MathUtil {
    * random object that generates random variables the seed is the hash of GIT_SHA this way when you
    * do log-replay even the generated random numbers are the same
    */
-  private static final Random random = new Random(BuildConstants.GIT_SHA.hashCode());
+  private static final Random random = new Random((long) Math.random());
 
   public static double linearInterpretationWithBounding(
       double x1, double y1, double x2, double y2, double x) {
diff --git a/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java
new file mode 100644
index 0000000..7a47a72
--- /dev/null
+++ b/src/main/java/frc/robot/extras/util/ProceduralStructGenerator.java
@@ -0,0 +1,857 @@
+package frc.robot.extras.util;
+
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.lang.annotation.Documented;
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+import java.lang.reflect.AnnotatedElement;
+import java.lang.reflect.Field;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.lang.reflect.RecordComponent;
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalInt;
+import java.util.Set;
+import java.util.function.Supplier;
+import java.util.stream.BaseStream;
+
+/** A utility class for procedurally generating {@link Struct}s from records and enums. */
+public final class ProceduralStructGenerator {
+  private ProceduralStructGenerator() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * A functional interface representing a method that retrives a value from a {@link ByteBuffer}.
+   */
+  @FunctionalInterface
+  private interface Unpacker<T> {
+    T unpack(ByteBuffer buffer);
+  }
+
+  /** A functional interface representing a method that packs a value into a {@link ByteBuffer}. */
+  @FunctionalInterface
+  private interface Packer<T> {
+    ByteBuffer pack(ByteBuffer buffer, T value);
+
+    static <T> Packer<T> fromStruct(Struct<T> struct) {
+      return (buffer, value) -> {
+        struct.pack(buffer, value);
+        return buffer;
+      };
+    }
+  }
+
+  private record PrimType<T>(String name, int size, Unpacker<T> unpacker, Packer<T> packer) {}
+
+  /** A map of primitive types to their schema types. */
+  private static final HashMap<Class<?>, PrimType<?>> primitiveTypeMap = new HashMap<>();
+
+  private static <T> void addPrimType(
+      Class<T> boxedClass,
+      Class<T> primitiveClass,
+      String name,
+      int size,
+      Unpacker<T> unpacker,
+      Packer<T> packer) {
+    PrimType<T> primType = new PrimType<>(name, size, unpacker, packer);
+    primitiveTypeMap.put(boxedClass, primType);
+    primitiveTypeMap.put(primitiveClass, primType);
+  }
+
+  // Add primitive types to the map
+  static {
+    addPrimType(
+        Integer.class, int.class, "int32", Integer.BYTES, ByteBuffer::getInt, ByteBuffer::putInt);
+    addPrimType(
+        Double.class,
+        double.class,
+        "float64",
+        Double.BYTES,
+        ByteBuffer::getDouble,
+        ByteBuffer::putDouble);
+    addPrimType(
+        Float.class,
+        float.class,
+        "float32",
+        Float.BYTES,
+        ByteBuffer::getFloat,
+        ByteBuffer::putFloat);
+    addPrimType(
+        Boolean.class,
+        boolean.class,
+        "bool",
+        Byte.BYTES,
+        buffer -> buffer.get() != 0,
+        (buffer, value) -> buffer.put((byte) (value ? 1 : 0)));
+    addPrimType(
+        Character.class,
+        char.class,
+        "char",
+        Character.BYTES,
+        ByteBuffer::getChar,
+        ByteBuffer::putChar);
+    addPrimType(Byte.class, byte.class, "uint8", Byte.BYTES, ByteBuffer::get, ByteBuffer::put);
+    addPrimType(
+        Short.class, short.class, "int16", Short.BYTES, ByteBuffer::getShort, ByteBuffer::putShort);
+    addPrimType(
+        Long.class, long.class, "int64", Long.BYTES, ByteBuffer::getLong, ByteBuffer::putLong);
+  }
+
+  /**
+   * A map of types to their custom struct schemas.
+   *
+   * <p>This allows adding custom struct implementations for types that are not supported by
+   * default. Think of vendor-specific.
+   */
+  private static final HashMap<Class<?>, Struct<?>> customStructTypeMap = new HashMap<>();
+
+  /**
+   * Add a custom struct to the structifier.
+   *
+   * @param <T> The type the struct is for.
+   * @param clazz The class of the type.
+   * @param struct The struct to add.
+   * @param override Whether to override an existing struct. An existing struct could mean the type
+   *     already has a {@code struct} field and implemnts {@link StructSerializable} or that the
+   *     type is already in the custom struct map.
+   */
+  public static <T> void addCustomStruct(Class<T> clazz, Struct<T> struct, boolean override) {
+    if (override) {
+      customStructTypeMap.put(clazz, struct);
+    } else if (!StructSerializable.class.isAssignableFrom(clazz)) {
+      customStructTypeMap.putIfAbsent(clazz, struct);
+    }
+  }
+
+  /**
+   * Returns a {@link Struct} for the given {@link StructSerializable} marked class. Due to the
+   * non-contractual nature of the marker this can fail. If the {@code struct} field could not be
+   * accessed for any reason, an empty {@link Optional} is returned.
+   *
+   * @param <T> The type of the class.
+   * @param clazz The class object to extract the struct from.
+   * @return An optional containing the struct if it could be extracted.
+   */
+  @SuppressWarnings("unchecked")
+  public static <T extends StructSerializable> Optional<Struct<T>> extractClassStruct(
+      Class<? extends T> clazz) {
+    try {
+      var possibleField = Optional.ofNullable(clazz.getDeclaredField("struct"));
+      return possibleField.flatMap(
+          field -> {
+            field.setAccessible(true);
+            if (Struct.class.isAssignableFrom(field.getType())) {
+              try {
+                return Optional.ofNullable((Struct<T>) field.get(null));
+              } catch (IllegalAccessException e) {
+                return Optional.empty();
+              }
+            } else {
+              return Optional.empty();
+            }
+          });
+    } catch (NoSuchFieldException e) {
+      return Optional.empty();
+    }
+  }
+
+  /**
+   * Returns a {@link Struct} for the given class. This does not do compile time checking that the
+   * class is a {@link StructSerializable}. Whenever possible it is reccomended to use {@link
+   * #extractClassStruct(Class)}.
+   *
+   * @param clazz The class object to extract the struct from.
+   * @return An optional containing the struct if it could be extracted.
+   */
+  @SuppressWarnings("unchecked")
+  public static Optional<Struct<?>> extractClassStructDynamic(Class<?> clazz) {
+    if (StructSerializable.class.isAssignableFrom(clazz)) {
+      return extractClassStruct((Class<? extends StructSerializable>) clazz).map(struct -> struct);
+    } else {
+      return Optional.empty();
+    }
+  }
+
+  @Retention(RetentionPolicy.RUNTIME)
+  @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT})
+  @Documented
+  public @interface IgnoreStructField {}
+
+  @Retention(RetentionPolicy.RUNTIME)
+  @Target({ElementType.FIELD, ElementType.RECORD_COMPONENT})
+  @Documented
+  public @interface FixedSizeArray {
+    int size();
+  }
+
+  private static OptionalInt arraySize(AnnotatedElement field) {
+    return Optional.ofNullable(field.getAnnotation(FixedSizeArray.class))
+        .map(FixedSizeArray::size)
+        .map(OptionalInt::of)
+        .orElse(OptionalInt.empty());
+  }
+
+  private static boolean shouldIgnore(AnnotatedElement field) {
+    return field.isAnnotationPresent(IgnoreStructField.class);
+  }
+
+  private record StructField(
+      String name,
+      String type,
+      int size,
+      boolean immutable,
+      Set<Struct<?>> structsToLoad,
+      Unpacker<?> unpacker,
+      Packer<?> packer) {
+
+    public static StructField fromField(Field field) {
+      return StructField.fromNameAndClass(
+          field.getName(),
+          field.getType(),
+          arraySize(field),
+          Modifier.isFinal(field.getModifiers()));
+    }
+
+    public static StructField fromRecordComponent(RecordComponent component) {
+      return StructField.fromNameAndClass(
+          component.getName(), component.getType(), arraySize(component), true);
+    }
+
+    @SuppressWarnings("unchecked")
+    public static StructField fromNameAndClass(
+        String name, Class<?> clazz, OptionalInt arraySize, boolean isFinal) {
+      if (!isFixedSize(clazz, arraySize)) {
+        return null;
+      }
+      if (clazz.isArray() && arraySize.isPresent()) {
+        final Class<?> componentType = clazz.getComponentType();
+        final int size = arraySize.getAsInt();
+        final StructField componentField =
+            fromNameAndClass(
+                componentType.getSimpleName(), componentType, OptionalInt.empty(), false);
+        return new StructField(
+            name + "[" + size + "]",
+            componentField.type,
+            componentField.size * size,
+            isFinal,
+            componentField.structsToLoad,
+            buffer -> {
+              Object[] array = new Object[size];
+              for (int i = 0; i < size; i++) {
+                array[i] = componentField.unpacker.unpack(buffer);
+              }
+              return array;
+            },
+            (buffer, value) -> {
+              for (Object obj : (Object[]) value) {
+                ((Packer<Object>) componentField.packer).pack(buffer, obj);
+              }
+              return buffer;
+            });
+      } else if (Measure.class.isAssignableFrom(clazz)) {
+        return new StructField(
+            name,
+            "float64",
+            Double.BYTES,
+            isFinal,
+            Set.of(),
+            buffer -> {
+              throw new UnsupportedOperationException("Cannot unpack Measure");
+            },
+            (buffer, value) -> buffer.putDouble(((Measure<?>) value).baseUnitMagnitude()));
+      } else if (primitiveTypeMap.containsKey(clazz)) {
+        PrimType<?> primType = primitiveTypeMap.get(clazz);
+        return new StructField(
+            name,
+            primType.name,
+            primType.size,
+            isFinal,
+            Set.of(),
+            primType.unpacker,
+            primType.packer);
+      } else {
+        Struct<?> struct = null;
+        if (customStructTypeMap.containsKey(clazz)) {
+          struct = customStructTypeMap.get(clazz);
+        } else if (StructSerializable.class.isAssignableFrom(clazz)) {
+          struct = extractClassStructDynamic(clazz).orElse(null);
+        }
+        if (struct == null) {
+          return null;
+        }
+        Set<Struct<?>> structsToLoad = new HashSet<>(Set.of(struct.getNested()));
+        structsToLoad.add(struct);
+        return new StructField(
+            name,
+            struct.getTypeName(),
+            struct.getSize(),
+            struct.isImmutable() && isFinal,
+            structsToLoad,
+            struct::unpack,
+            Packer.fromStruct(struct));
+      }
+    }
+  }
+
+  /**
+   * Introspects a class to determine if it's a fixed size.
+   *
+   * <p>Fixed size means no collections, no strings, no arrays, etc.
+   *
+   * @param clazz The class to introspect.
+   * @return Whether the class is fixed size.
+   */
+  public static boolean isFixedSize(Class<?> clazz, OptionalInt arraySize) {
+    if (clazz.isArray()) {
+      if (arraySize.isEmpty()) {
+        return false;
+      } else {
+        Class<?> componentType = clazz.getComponentType();
+        return isFixedSize(componentType, OptionalInt.empty());
+      }
+    } else if (clazz.isRecord()) {
+      for (RecordComponent component : clazz.getRecordComponents()) {
+        if (!isFixedSize(component.getType(), arraySize(component))) {
+          return false;
+        }
+      }
+    } else {
+      for (Field field : clazz.getDeclaredFields()) {
+        Class<?> fieldClass = field.getType();
+        if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) {
+          continue;
+        }
+        if (Collection.class.isAssignableFrom(fieldClass)
+            || Iterator.class.isAssignableFrom(fieldClass)
+            || Iterable.class.isAssignableFrom(fieldClass)
+            || BaseStream.class.isAssignableFrom(fieldClass)
+            || fieldClass.isArray()
+            || fieldClass == String.class
+            || fieldClass == Optional.class) {
+          return false;
+        }
+        if (!primitiveTypeMap.containsKey(fieldClass)
+            && !isFixedSize(fieldClass, arraySize(field))) {
+          return false;
+        }
+      }
+    }
+    return true;
+  }
+
+  /**
+   * Introspects a class to determine if it's interiorly mutable.
+   *
+   * <p>Interior mutability means that the class has fields that are mutable.
+   *
+   * @param clazz The class to introspect.
+   * @return Whether the class is interiorly mutable.
+   */
+  public static boolean isInteriorlyMutable(Class<?> clazz) {
+    if (clazz.isArray()) {
+      return true;
+    } else if (clazz.isRecord()) {
+      for (RecordComponent component : clazz.getRecordComponents()) {
+        if (isInteriorlyMutable(component.getType())) {
+          return true;
+        }
+      }
+    } else {
+      for (Field field : clazz.getDeclaredFields()) {
+        if (field.isEnumConstant() || Modifier.isStatic(field.getModifiers())) {
+          continue;
+        }
+        if (!Modifier.isFinal(field.getModifiers())) {
+          return true;
+        }
+        if (!primitiveTypeMap.containsKey(field.getType())
+            && isInteriorlyMutable(field.getType())) {
+          return true;
+        }
+      }
+    }
+    return false;
+  }
+
+  /** A utility for building schema syntax in a procedural manner. */
+  @SuppressWarnings("PMD.AvoidStringBufferField")
+  public static class SchemaBuilder {
+    /** A utility for building enum fields in a procedural manner. */
+    public static final class EnumFieldBuilder {
+      private final StringBuilder m_builder = new StringBuilder();
+      private final String m_fieldName;
+      private boolean m_firstVariant = true;
+
+      /**
+       * Creates a new enum field builder.
+       *
+       * @param fieldName The name of the field.
+       */
+      public EnumFieldBuilder(String fieldName) {
+        this.m_fieldName = fieldName;
+        m_builder.append("enum {");
+      }
+
+      /**
+       * Adds a variant to the enum field.
+       *
+       * @param name The name of the variant.
+       * @param value The value of the variant.
+       * @return The builder for chaining.
+       */
+      public EnumFieldBuilder addVariant(String name, int value) {
+        if (!m_firstVariant) {
+          m_builder.append(',');
+        }
+        m_firstVariant = false;
+        m_builder.append(name).append('=').append(value);
+        return this;
+      }
+
+      /**
+       * Builds the enum field. If this object is being used with {@link SchemaBuilder#addEnumField}
+       * then {@link #build()} does not have to be called by the user.
+       *
+       * @return The built enum field.
+       */
+      public String build() {
+        m_builder.append("} int8 ").append(m_fieldName).append(';');
+        return m_builder.toString();
+      }
+    }
+
+    /** Creates a new schema builder. */
+    public SchemaBuilder() {}
+
+    private final StringBuilder m_builder = new StringBuilder();
+
+    /**
+     * Adds a field to the schema.
+     *
+     * @param name The name of the field.
+     * @param type The type of the field.
+     * @return The builder for chaining.
+     */
+    public SchemaBuilder addField(StructField field) {
+      m_builder.append(field.type).append(' ').append(field.name).append(';');
+      return this;
+    }
+
+    /**
+     * Adds an inline enum field to the schema.
+     *
+     * @param enumFieldBuilder The builder for the enum field.
+     * @return The builder for chaining.
+     */
+    public SchemaBuilder addEnumField(EnumFieldBuilder enumFieldBuilder) {
+      m_builder.append(enumFieldBuilder.build());
+      return this;
+    }
+
+    /**
+     * Builds the schema.
+     *
+     * @return The built schema.
+     */
+    public String build() {
+      return m_builder.toString();
+    }
+  }
+
+  public static <T> Struct<T> noopStruct(Class<T> cls) {
+    return new Struct<>() {
+      @Override
+      public Class<T> getTypeClass() {
+        return cls;
+      }
+
+      @Override
+      public String getTypeName() {
+        return cls.getSimpleName();
+      }
+
+      @Override
+      public String getSchema() {
+        return "";
+      }
+
+      @Override
+      public int getSize() {
+        return 0;
+      }
+
+      @Override
+      public void pack(ByteBuffer buffer, T value) {}
+
+      @Override
+      public T unpack(ByteBuffer buffer) {
+        return null;
+      }
+    };
+  }
+
+  private abstract static class ProcStruct<T> implements Struct<T> {
+    protected final Class<T> typeClass;
+    protected final List<StructField> fields;
+    private final String schema;
+
+    // stored values so we never recompute them
+    private final int size;
+    private final boolean isImmutable;
+    private final Struct<?>[] nested;
+
+    public ProcStruct(Class<T> typeClass, List<StructField> fields, String schema) {
+      this.typeClass = typeClass;
+      this.fields = fields;
+      this.schema = schema;
+
+      this.size = fields.stream().mapToInt(StructField::size).sum();
+      this.isImmutable = fields.stream().allMatch(StructField::immutable);
+      this.nested =
+          fields.stream()
+              .map(StructField::structsToLoad)
+              .flatMap(Collection::stream)
+              .toArray(Struct<?>[]::new);
+
+      ProceduralStructGenerator.customStructTypeMap.put(typeClass, this);
+    }
+
+    @Override
+    public Class<T> getTypeClass() {
+      return typeClass;
+    }
+
+    @Override
+    public String getTypeName() {
+      return typeClass.getSimpleName();
+    }
+
+    @Override
+    public String getSchema() {
+      return schema;
+    }
+
+    @Override
+    public int getSize() {
+      return size;
+    }
+
+    @Override
+    public boolean isCloneable() {
+      return Cloneable.class.isAssignableFrom(typeClass);
+    }
+
+    @Override
+    @SuppressWarnings("unchecked")
+    public T clone(T obj) throws CloneNotSupportedException {
+      if (isCloneable()) {
+        try {
+          return (T) typeClass.getMethod("clone").invoke(obj);
+        } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) {
+          throw new CloneNotSupportedException();
+        }
+      } else {
+        throw new CloneNotSupportedException();
+      }
+    }
+
+    @Override
+    public boolean isImmutable() {
+      return isImmutable;
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return nested;
+    }
+
+    @Override
+    public String toString() {
+      return this.getTypeName() + "<" + this.getSize() + ">" + " {" + this.schema + "}";
+    }
+  }
+
+  /**
+   * Generates a {@link Struct} for the given {@link Record} class. If a {@link Struct} cannot be
+   * generated from the {@link Record}, the errors encountered will be printed and a no-op {@link
+   * Struct} will be returned.
+   *
+   * @param <R> The type of the record.
+   * @param recordClass The class of the record.
+   * @return The generated struct.
+   */
+  @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"})
+  public static <R extends Record> Struct<R> genRecord(final Class<R> recordClass) {
+    final RecordComponent[] components = recordClass.getRecordComponents();
+    final SchemaBuilder schemaBuilder = new SchemaBuilder();
+    final ArrayList<StructField> fields = new ArrayList<>();
+
+    for (final RecordComponent component : components) {
+      if (shouldIgnore(component)) {
+        continue;
+      }
+      component.getAccessor().setAccessible(true);
+      fields.add(StructField.fromRecordComponent(component));
+    }
+
+    if (fields.stream().anyMatch(f -> f == null)) {
+      return noopStruct(recordClass);
+    }
+    fields.forEach(schemaBuilder::addField);
+
+    return new ProcStruct<>(recordClass, fields, schemaBuilder.build()) {
+      @Override
+      public void pack(ByteBuffer buffer, R value) {
+        boolean failed = false;
+        int startingPosition = buffer.position();
+        for (int i = 0; i < components.length; i++) {
+          Packer<Object> packer = (Packer<Object>) fields.get(i).packer();
+          try {
+            Object componentValue = components[i].getAccessor().invoke(value);
+            if (componentValue == null) {
+              throw new IllegalArgumentException("Component is null");
+            }
+            packer.pack(buffer, componentValue);
+          } catch (IllegalAccessException
+              | IllegalArgumentException
+              | InvocationTargetException e) {
+            failed = true;
+            break;
+          }
+        }
+        if (failed) {
+          buffer.put(startingPosition, new byte[this.getSize()]);
+        }
+      }
+
+      @Override
+      public R unpack(ByteBuffer buffer) {
+        try {
+          Object[] args = new Object[components.length];
+          Class<?>[] argTypes = new Class<?>[components.length];
+          for (int i = 0; i < components.length; i++) {
+            args[i] = fields.get(i).unpacker().unpack(buffer);
+            argTypes[i] = components[i].getType();
+          }
+          return recordClass.getConstructor(argTypes).newInstance(args);
+        } catch (InstantiationException
+            | IllegalAccessException
+            | InvocationTargetException
+            | NoSuchMethodException
+            | SecurityException e) {
+          System.err.println(
+              "Could not unpack record: "
+                  + recordClass.getSimpleName()
+                  + "\n    "
+                  + e.getMessage());
+          return null;
+        }
+      }
+    };
+  }
+
+  /**
+   * Generates a {@link Struct} for the given {@link Enum} class. If a {@link Struct} cannot be
+   * generated from the {@link Enum}, the errors encountered will be printed and a no-op {@link
+   * Struct} will be returned.
+   *
+   * @param <E> The type of the enum.
+   * @param enumClass The class of the enum.
+   * @return The generated struct.
+   */
+  @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"})
+  public static <E extends Enum<E>> Struct<E> genEnum(Class<E> enumClass) {
+    final E[] enumVariants = enumClass.getEnumConstants();
+    final Field[] allEnumFields = enumClass.getDeclaredFields();
+    final SchemaBuilder schemaBuilder = new SchemaBuilder();
+    final SchemaBuilder.EnumFieldBuilder enumFieldBuilder =
+        new SchemaBuilder.EnumFieldBuilder("variant");
+    final HashMap<Integer, E> enumMap = new HashMap<>();
+    final ArrayList<StructField> fields = new ArrayList<>();
+
+    if (enumVariants == null || enumVariants.length == 0) {
+      return noopStruct(enumClass);
+    }
+
+    for (final E constant : enumVariants) {
+      final String name = constant.name();
+      final int ordinal = constant.ordinal();
+
+      enumFieldBuilder.addVariant(name, ordinal);
+      enumMap.put(ordinal, constant);
+    }
+    schemaBuilder.addEnumField(enumFieldBuilder);
+    fields.add(
+        new StructField(
+            "variant",
+            "int8",
+            1,
+            true,
+            Set.of(),
+            ByteBuffer::get,
+            (buffer, value) -> buffer.put((byte) ((Enum<?>) value).ordinal())));
+
+    final List<Field> enumFields =
+        List.of(allEnumFields).stream()
+            .filter(
+                f ->
+                    !f.isEnumConstant() && !Modifier.isStatic(f.getModifiers()) && !shouldIgnore(f))
+            .toList();
+
+    for (final Field field : enumFields) {
+      field.setAccessible(true);
+      fields.add(StructField.fromField(field));
+    }
+    if (fields.stream().anyMatch(f -> f == null)) {
+      return noopStruct(enumClass);
+    }
+    for (int i = 1; i < fields.size(); i++) {
+      // do this to skip the variant field
+      schemaBuilder.addField(fields.get(i));
+    }
+
+    return new ProcStruct<>(enumClass, fields, schemaBuilder.build()) {
+      @Override
+      public void pack(ByteBuffer buffer, E value) {
+        boolean failed = false;
+        int startingPosition = buffer.position();
+        buffer.put((byte) value.ordinal());
+        for (int i = 0; i < enumFields.size(); i++) {
+          Packer<Object> packer = (Packer<Object>) fields.get(i + 1).packer();
+          Field field = enumFields.get(i);
+          try {
+            Object fieldValue = field.get(value);
+            if (fieldValue == null) {
+              throw new IllegalArgumentException("Field is null");
+            }
+            packer.pack(buffer, fieldValue);
+          } catch (IllegalArgumentException | IllegalAccessException e) {
+            System.err.println(
+                "Could not pack enum field: "
+                    + enumClass.getSimpleName()
+                    + "#"
+                    + field.getName()
+                    + "\n    "
+                    + e.getMessage());
+            failed = true;
+            break;
+          }
+        }
+        if (failed) {
+          buffer.put(startingPosition, new byte[this.getSize()]);
+        }
+      }
+
+      final byte[] m_spongeBuffer = new byte[this.getSize() - 1];
+
+      @Override
+      public E unpack(ByteBuffer buffer) {
+        int ordinal = buffer.get();
+        buffer.get(m_spongeBuffer);
+        return enumMap.getOrDefault(ordinal, null);
+      }
+
+      public boolean isCloneable() {
+        return true;
+      }
+      ;
+
+      public E clone(E obj) throws CloneNotSupportedException {
+        return obj;
+      }
+      ;
+    };
+  }
+
+  /**
+   * Generates a {@link Struct} for the given {@link Object} class. If a {@link Struct} cannot be
+   * generated from the {@link Object}, the errors encountered will be printed and a no-op {@link
+   * Struct} will be returned.
+   *
+   * @param <O> The type of the object.
+   * @param objectClass The class of the object.
+   * @param objectSupplier A supplier for the object.
+   * @return The generated struct.
+   */
+  @SuppressWarnings({"unchecked", "PMD.AvoidAccessibilityAlteration"})
+  public static <O> Struct<O> genObject(Class<O> objectClass, Supplier<O> objectSupplier) {
+    final SchemaBuilder schemaBuilder = new SchemaBuilder();
+    final Field[] allFields =
+        List.of(objectClass.getDeclaredFields()).stream()
+            .filter(f -> !shouldIgnore(f) && !Modifier.isStatic(f.getModifiers()))
+            .toArray(Field[]::new);
+    final ArrayList<StructField> fields = new ArrayList<>(allFields.length);
+
+    for (final Field field : allFields) {
+      field.setAccessible(true);
+      fields.add(StructField.fromField(field));
+    }
+
+    if (fields.stream().anyMatch(f -> f == null)) {
+      return noopStruct(objectClass);
+    }
+    fields.forEach(schemaBuilder::addField);
+
+    return new ProcStruct<>(objectClass, fields, schemaBuilder.build()) {
+      @Override
+      public void pack(ByteBuffer buffer, O value) {
+        boolean failed = false;
+        int startingPosition = buffer.position();
+        for (int i = 0; i < allFields.length; i++) {
+          Packer<Object> packer = (Packer<Object>) fields.get(i).packer();
+          try {
+            Object fieldValue = allFields[i].get(value);
+            if (fieldValue == null) {
+              throw new IllegalArgumentException("Field is null");
+            }
+            packer.pack(buffer, fieldValue);
+          } catch (IllegalArgumentException | IllegalAccessException e) {
+            System.err.println(
+                "Could not pack object field: "
+                    + objectClass.getSimpleName()
+                    + "#"
+                    + allFields[i].getName()
+                    + "\n    "
+                    + e.getMessage());
+            failed = true;
+            break;
+          }
+        }
+        if (failed) {
+          buffer.put(startingPosition, new byte[this.getSize()]);
+        }
+      }
+
+      @Override
+      public O unpack(ByteBuffer buffer) {
+        try {
+          O obj = objectSupplier.get();
+          for (int i = 0; i < allFields.length; i++) {
+            Object fieldValue = fields.get(i).unpacker().unpack(buffer);
+            allFields[i].set(obj, fieldValue);
+          }
+          return obj;
+        } catch (IllegalArgumentException | IllegalAccessException e) {
+          System.err.println(
+              "Could not unpack object: "
+                  + objectClass.getSimpleName()
+                  + "\n    "
+                  + e.getMessage());
+          return null;
+        }
+      }
+    };
+  }
+}
diff --git a/src/main/java/frc/robot/extras/util/TimeUtil.java b/src/main/java/frc/robot/extras/util/TimeUtil.java
index 754968a..d59cd95 100644
--- a/src/main/java/frc/robot/extras/util/TimeUtil.java
+++ b/src/main/java/frc/robot/extras/util/TimeUtil.java
@@ -3,28 +3,16 @@
 import org.littletonrobotics.junction.Logger;
 
 public class TimeUtil {
-  public static void delay(double seconds) {
-    try {
-      // Convert seconds to total milliseconds
-      long totalMillis = (long) (seconds * 1000);
-      // Calculate the remaining nanoseconds
-      int nanoPart = (int) ((seconds * 1000 - totalMillis) * 1000000);
-
-      // Pause the thread for the specified duration in milliseconds and nanoseconds
-      Thread.sleep(totalMillis, nanoPart);
-    } catch (InterruptedException e) {
-      // Restore the interrupted status
-      Thread.currentThread().interrupt();
-
-      // Optionally, handle the interruption, e.g. logging or throwing a runtime exception
-      System.err.println("The sleep was interrupted");
-    }
-  }
-
+  /**
+   * @return Gets the deterministic timestamp in seconds.
+   */
   public static double getLogTimeSeconds() {
     return Logger.getTimestamp() / 1_000_000.0;
   }
 
+  /**
+   * @return Gets the non-deterministic timestamp in seconds.
+   */
   public static double getRealTimeSeconds() {
     return Logger.getRealTimestamp() / 1_000_000.0;
   }
diff --git a/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java
new file mode 100644
index 0000000..04a7d7a
--- /dev/null
+++ b/src/main/java/frc/robot/extras/vision/MegatagPoseEstimate.java
@@ -0,0 +1,99 @@
+package frc.robot.extras.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructSerializable;
+import java.nio.ByteBuffer;
+
+public class MegatagPoseEstimate implements StructSerializable {
+  public static class MegatagPoseEstimateStruct implements Struct<MegatagPoseEstimate> {
+    public Pose2d fieldToCamera = Pose2d.kZero;
+    public double timestampSeconds;
+    public double latency;
+    public double avgTagArea;
+    public double avgTagDist;
+    public int tagCount;
+
+    @Override
+    public Class<MegatagPoseEstimate> getTypeClass() {
+      return MegatagPoseEstimate.class;
+    }
+
+    @Override
+    public String getTypeString() {
+      return "struct:MegatagPoseEstimate";
+    }
+
+    @Override
+    public int getSize() {
+      return Pose2d.struct.getSize() + kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+      return "Pose2d fieldToCamera;double timestampSeconds;double latency;double avgTagArea; int tagCount; double avgTagDist";
+    }
+
+    @Override
+    public Struct<?>[] getNested() {
+      return new Struct<?>[] {Pose2d.struct};
+    }
+
+    @Override
+    public MegatagPoseEstimate unpack(ByteBuffer bb) {
+      MegatagPoseEstimate rv = new MegatagPoseEstimate();
+      rv.fieldToCamera = Pose2d.struct.unpack(bb);
+      rv.timestampSeconds = bb.getDouble();
+      rv.latency = bb.getDouble();
+      rv.avgTagArea = bb.getDouble();
+      rv.fiducialIds = new int[0];
+      rv.avgTagDist = bb.getDouble();
+      rv.timestampSeconds = bb.getInt();
+      return rv;
+    }
+
+    @Override
+    public void pack(ByteBuffer bb, MegatagPoseEstimate value) {
+      Pose2d.struct.pack(bb, value.fieldToCamera);
+      bb.putDouble(value.timestampSeconds);
+      bb.putDouble(value.latency);
+      bb.putDouble(value.avgTagArea);
+      bb.putDouble(value.avgTagDist);
+    }
+
+    @Override
+    public String getTypeName() {
+      // TODO Auto-generated method stub
+      throw new UnsupportedOperationException("Unimplemented method 'getTypeName'");
+    }
+  }
+
+  public Pose2d fieldToCamera = Pose2d.kZero;
+  public double timestampSeconds;
+  public double latency;
+  public double avgTagArea;
+  public int[] fiducialIds;
+  public int tagCount;
+  public double avgTagDist;
+
+  public MegatagPoseEstimate() {}
+
+  public static MegatagPoseEstimate fromLimelight(LimelightHelpers.PoseEstimate poseEstimate) {
+    MegatagPoseEstimate rv = new MegatagPoseEstimate();
+    rv.fieldToCamera = poseEstimate.pose;
+    if (rv.fieldToCamera == null) rv.fieldToCamera = Pose2d.kZero;
+    rv.timestampSeconds = poseEstimate.timestampSeconds;
+    rv.latency = poseEstimate.latency;
+    rv.avgTagArea = poseEstimate.avgTagArea;
+    rv.avgTagDist = poseEstimate.avgTagDist;
+    rv.tagCount = poseEstimate.tagCount;
+    rv.fiducialIds = new int[poseEstimate.rawFiducials.length];
+    for (int i = 0; i < rv.fiducialIds.length; ++i) {
+      rv.fiducialIds[i] = poseEstimate.rawFiducials[i].id;
+    }
+
+    return rv;
+  }
+
+  public static final MegatagPoseEstimateStruct struct = new MegatagPoseEstimateStruct();
+}
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java
index d9c4fe1..ed08692 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java
@@ -1,17 +1,14 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// 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 frc.robot.subsystems.swerve;
 
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 
-/** Add your docs here. */
+/** Swerve Constants */
 public class SwerveConstants {
 
   public static final class DriveConstants {
@@ -22,9 +19,9 @@ public static final class DriveConstants {
     // Wheel base and track width are measured by the center of the swerve modules, not the frame of
     // the robot
     // Distance between centers of right and left wheels on robot
-    public static final double TRACK_WIDTH = Units.inchesToMeters(22);
+    public static final double TRACK_WIDTH = Units.inchesToMeters(21.25);
     // Distance between front and back wheels on robot
-    public static final double WHEEL_BASE = Units.inchesToMeters(22);
+    public static final double WHEEL_BASE = Units.inchesToMeters(21.25);
 
     public static final Translation2d[] MODULE_TRANSLATIONS =
         new Translation2d[] {
@@ -37,25 +34,25 @@ public static final class DriveConstants {
     public static final SwerveDriveKinematics DRIVE_KINEMATICS =
         new SwerveDriveKinematics(MODULE_TRANSLATIONS);
 
-    public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 0 - 9;
-    public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 0 - 9;
-    public static final int REAR_LEFT_DRIVE_MOTOR_ID = 0 - 9;
-    public static final int REAR_RIGHT_DRIVE_MOTOR_ID = 0 - 9;
+    public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 22;
+    public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 24;
+    public static final int REAR_LEFT_DRIVE_MOTOR_ID = 23;
+    public static final int REAR_RIGHT_DRIVE_MOTOR_ID = 21;
 
-    public static final int FRONT_LEFT_TURN_MOTOR_ID = 0 - 9;
-    public static final int FRONT_RIGHT_TURN_MOTOR_ID = 0 - 9;
-    public static final int REAR_LEFT_TURN_MOTOR_ID = 0 - 9;
-    public static final int REAR_RIGHT_TURN_MOTOR_ID = 0 - 9;
+    public static final int FRONT_LEFT_TURN_MOTOR_ID = 5;
+    public static final int FRONT_RIGHT_TURN_MOTOR_ID = 6;
+    public static final int REAR_LEFT_TURN_MOTOR_ID = 8;
+    public static final int REAR_RIGHT_TURN_MOTOR_ID = 7;
 
-    public static final int FRONT_LEFT_CANCODER_ID = 0 - 9;
-    public static final int FRONT_RIGHT_CANCODER_ID = 0 - 9;
-    public static final int REAR_LEFT_CANCODER_ID = 0 - 9;
-    public static final int REAR_RIGHT_CANCODER_ID = 0 - 9;
+    public static final int FRONT_LEFT_CANCODER_ID = 14;
+    public static final int FRONT_RIGHT_CANCODER_ID = 12;
+    public static final int REAR_LEFT_CANCODER_ID = 11;
+    public static final int REAR_RIGHT_CANCODER_ID = 13;
 
-    public static final double FRONT_LEFT_ZERO_ANGLE = 0;
-    public static final double FRONT_RIGHT_ZERO_ANGLE = 0;
-    public static final double REAR_LEFT_ZERO_ANGLE = 0;
-    public static final double REAR_RIGHT_ZERO_ANGLE = 0;
+    public static final double FRONT_LEFT_ZERO_ANGLE = 0.137939453125;
+    public static final double FRONT_RIGHT_ZERO_ANGLE = -0.420654296875;
+    public static final double REAR_LEFT_ZERO_ANGLE = -0.475341796875;
+    public static final double REAR_RIGHT_ZERO_ANGLE = -0.05078125;
 
     public static final SensorDirectionValue FRONT_LEFT_CANCODER_REVERSED =
         SensorDirectionValue.CounterClockwise_Positive;
@@ -67,34 +64,36 @@ public static final class DriveConstants {
         SensorDirectionValue.CounterClockwise_Positive;
 
     public static final InvertedValue FRONT_LEFT_TURN_MOTOR_REVERSED =
-        InvertedValue.Clockwise_Positive;
+        InvertedValue.CounterClockwise_Positive;
     public static final InvertedValue FRONT_RIGHT_TURN_MOTOR_REVERSED =
-        InvertedValue.Clockwise_Positive;
+        InvertedValue.CounterClockwise_Positive;
     public static final InvertedValue REAR_LEFT_TURN_MOTOR_REVERSED =
         InvertedValue.Clockwise_Positive;
     public static final InvertedValue REAR_RIGHT_TURN_MOTOR_REVERSED =
         InvertedValue.Clockwise_Positive;
 
     public static final InvertedValue FRONT_LEFT_DRIVE_ENCODER_REVERSED =
-        InvertedValue.Clockwise_Positive;
-    public static final InvertedValue FRONT_RIGHT_DRIVE_ENCODER_REVERSED =
         InvertedValue.CounterClockwise_Positive;
+    public static final InvertedValue FRONT_RIGHT_DRIVE_ENCODER_REVERSED =
+        InvertedValue.Clockwise_Positive;
     public static final InvertedValue REAR_LEFT_DRIVE_ENCODER_REVERSED =
         InvertedValue.Clockwise_Positive;
     public static final InvertedValue REAR_RIGHT_DRIVE_ENCODER_REVERSED =
         InvertedValue.CounterClockwise_Positive;
 
-    public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 0 - 9;
-    public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 0 - 9;
+    public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 20;
+    public static final double LOW_ANGULAR_SPEED_RADIANS_PER_SECOND = 5;
+
+    public static final double MAX_SPEED_METERS_PER_SECOND = 6.95;
+    public static final double MAX_SHOOT_SPEED_METERS_PER_SECOND = 3;
 
-    public static final double MAX_SPEED_METERS_PER_SECOND = 0 - 9;
+    public static final double HEADING_ACCEPTABLE_ERROR_RADIANS = Units.degreesToRadians(2.5);
+    public static final double HEADING_ACCEPTABLE_ERROR_MOVING_RADIANS = Units.degreesToRadians(4);
   }
 
   public class ModuleConstants {
-    public static final double DRIVE_GEAR_RATIO = 0 - 9;
-    public static final double TURN_GEAR_RATIO = 0 - 9;
-    // Use the wheel diamer characterization periodically to find this
-    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(4);
+    public static final double DRIVE_GEAR_RATIO = 4.59;
+    public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(3.774788522800778);
 
     public static final double WHEEL_CIRCUMFERENCE_METERS = WHEEL_DIAMETER_METERS * Math.PI;
     public static final double DRIVE_TO_METERS = WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO;
@@ -104,116 +103,71 @@ public class ModuleConstants {
     public static final double DRIVE_SUPPLY_LIMIT = 45.0;
     public static final double DRIVE_STATOR_LIMIT = 50.0;
 
-    public static final double TURN_P = 0 - 9;
-    public static final double TURN_I = 0 - 9;
-    public static final double TURN_D = 0 - 9;
+    public static final double TURN_P = 116;
+    public static final double TURN_I = 0.0;
+    public static final double TURN_D = 0.64;
 
-    public static final double TURN_S = 0 - 9;
-    public static final double TURN_V = 0 - 9;
-    public static final double TURN_A = 0 - 9;
+    public static final double TURN_S = 0.0;
+    public static final double TURN_V = 0.0;
+    public static final double TURN_A = 0.0;
 
-    public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 0 - 9;
-    public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 0 - 9;
+    public static final double MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND = 30;
+    public static final double MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED = 24;
 
-    public static final double DRIVE_P = 0 - 9;
-    public static final double DRIVE_I = 0 - 9;
-    public static final double DRIVE_D = 0 - 9;
-    // Use recalc to find the feedfoward values
+    public static final double DRIVE_P = 0.417;
+    public static final double DRIVE_I = 0.0;
+    public static final double DRIVE_D = 0.0;
+
+    public static final double DRIVE_S = 0.16;
+    // These values were gotten using recalc, then converted to the correct units & were confirmed
+    // through testing and characterization
     // https://www.reca.lc/drive?appliedVoltageRamp=%7B%22s%22%3A1200%2C%22u%22%3A%22V%2Fs%22%7D&batteryAmpHours=%7B%22s%22%3A18%2C%22u%22%3A%22A%2Ah%22%7D&batteryResistance=%7B%22s%22%3A0.018%2C%22u%22%3A%22Ohm%22%7D&batteryVoltageAtRest=%7B%22s%22%3A12.6%2C%22u%22%3A%22V%22%7D&efficiency=97&filtering=1&gearRatioMax=%7B%22magnitude%22%3A15%2C%22ratioType%22%3A%22Reduction%22%7D&gearRatioMin=%7B%22magnitude%22%3A3%2C%22ratioType%22%3A%22Reduction%22%7D&maxSimulationTime=%7B%22s%22%3A4%2C%22u%22%3A%22s%22%7D&maxSpeedAccelerationThreshold=%7B%22s%22%3A0.15%2C%22u%22%3A%22ft%2Fs2%22%7D&motor=%7B%22quantity%22%3A4%2C%22name%22%3A%22Kraken%20X60%2A%22%7D&motorCurrentLimit=%7B%22s%22%3A60%2C%22u%22%3A%22A%22%7D&numCyclesPerMatch=24&peakBatteryDischarge=20&ratio=%7B%22magnitude%22%3A4.59%2C%22ratioType%22%3A%22Reduction%22%7D&sprintDistance=%7B%22s%22%3A25%2C%22u%22%3A%22ft%22%7D&swerve=1&targetTimeToGoal=%7B%22s%22%3A2%2C%22u%22%3A%22s%22%7D&throttleResponseMax=0.99&throttleResponseMin=0.5&weightAuxilliary=%7B%22s%22%3A24%2C%22u%22%3A%22lbs%22%7D&weightDistributionFrontBack=0.5&weightDistributionLeftRight=0.5&weightInspected=%7B%22s%22%3A125%2C%22u%22%3A%22lbs%22%7D&wheelBaseLength=%7B%22s%22%3A27%2C%22u%22%3A%22in%22%7D&wheelBaseWidth=%7B%22s%22%3A20%2C%22u%22%3A%22in%22%7D&wheelCOFDynamic=0.9&wheelCOFLateral=1.1&wheelCOFStatic=1.1&wheelDiameter=%7B%22s%22%3A4%2C%22u%22%3A%22in%22%7D
-    public static final double DRIVE_S = 0 - 9;
-    public static final double DRIVE_V = 0 - 9;
-    public static final double DRIVE_A = 0 - 9;
+    public static final double DRIVE_V =
+        1.73 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.1203 V*s/m
+    public static final double DRIVE_A =
+        0.32 * WHEEL_CIRCUMFERENCE_METERS / DRIVE_GEAR_RATIO; // = 0.02225 V*s^2/m
   }
 
   public static final class TrajectoryConstants {
 
     public static final double DRIVE_BASE_DIAMETER =
         Math.sqrt(Math.pow(DriveConstants.TRACK_WIDTH, 2) + Math.pow(DriveConstants.WHEEL_BASE, 2));
-    public static final double MAX_SPEED = 0 - 9;
-    public static final double MAX_ACCELERATION = 0 - 9;
 
-    public static final double AUTO_TRANSLATION_P = 0 - 9;
-    public static final double AUTO_TRANSLATION_D = 0 - 9;
-    public static final double AUTO_THETA_P = 0 - 9;
-    public static final double AUTO_THETA_D = 0 - 9;
-  }
+    public static final double MAX_SPEED = 5.0;
+    public static final double MAX_ACCELERATION = 3;
+
+    public static final double AUTO_TRANSLATION_P = 1.5; // 1.7
+    public static final double AUTO_TRANSLATION_D = 0.2;
+    public static final double AUTO_THETA_P = 4.5; // 5
+    public static final double AUTO_THETA_D = 0.4;
+
+    public static final double AUTO_SHOOT_HEADING_OFFSET = 2;
+
+    public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND = 2;
+    public static final double MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED = 2;
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints THETA_CONTROLLER_CONSTRAINTS =
+        new TrapezoidProfile.Constraints(
+            MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, MAX_ANGULAR_SPEED_RADIANS_PER_SECOND_SQUARED);
+
+    public static final double X_TOLERANCE = 0.02;
+    public static final double Y_TOLERANCE = 0.02;
+    public static final double THETA_TOLERANCE = 1.25;
+
+    // Note Detection Driving Constants
+    public static final double AUTO_ALIGN_TRANSLATIONAL_P = 3;
+    public static final double AUTO_ALIGN_TRANSLATIONAL_I = 0;
+    public static final double AUTO_ALIGN_TRANSLATIONAL_D = 0;
+
+    public static Constraints AUTO_ALIGN_TRANSLATION_CONSTRAINTS = new Constraints(5, 2);
+
+    public static final double AUTO_ALIGN_ROTATIONAL_P = 3;
+    public static final double AUTO_ALIGN_ROTATIONAL_I = 0;
+    public static final double AUTO_ALIGN_ROTATIONAL_D = 0;
 
-  /**
-   * stores the constants and PID configs for chassis because we want an all-real simulation for the
-   * chassis, the numbers are required to be considerably precise
-   */
-  public class SimulationConstants {
-    /**
-     * numbers that needs to be changed to fit each robot TODO: change these numbers to match your
-     * robot
-     */
-    public static final double WHEEL_COEFFICIENT_OF_FRICTION = 0.95,
-        ROBOT_MASS_KG = 40; // with bumpers
-
-    /** TODO: change motor type to match your robot */
-    public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1),
-        STEER_MOTOR = DCMotor.getFalcon500(1);
-
-    public static final double WHEEL_RADIUS_METERS = ModuleConstants.WHEEL_DIAMETER_METERS / 2,
-        DRIVE_GEAR_RATIO = ModuleConstants.DRIVE_GEAR_RATIO,
-        STEER_GEAR_RATIO = 16.0,
-        TIME_ROBOT_STOP_ROTATING_SECONDS = 0.06,
-        STEER_FRICTION_VOLTAGE = 0.12,
-        DRIVE_FRICTION_VOLTAGE = ModuleConstants.DRIVE_S,
-        DRIVE_INERTIA = 0.01,
-        STEER_INERTIA = 0.01;
-
-    /* adjust current limit */
-    public static final double DRIVE_CURRENT_LIMIT = ModuleConstants.DRIVE_STATOR_LIMIT;
-    // public static final double STEER_CURRENT_LIMIT = ModuleConstants.ST;
-
-    /* equations that calculates some constants for the simulator (don't modify) */
-    private static final double GRAVITY_CONSTANT = 9.81;
-    public static final double DRIVE_BASE_RADIUS = DriveConstants.MODULE_TRANSLATIONS[0].getNorm(),
-        /* friction_force = normal_force * coefficient_of_friction */
-        MAX_FRICTION_ACCELERATION = GRAVITY_CONSTANT * WHEEL_COEFFICIENT_OF_FRICTION,
-        MAX_FRICTION_FORCE_PER_MODULE =
-            MAX_FRICTION_ACCELERATION * ROBOT_MASS_KG / DriveConstants.MODULE_TRANSLATIONS.length,
-        /* force = torque / distance */
-        MAX_PROPELLING_FORCE_NEWTONS =
-            DRIVE_MOTOR.getTorque(DRIVE_CURRENT_LIMIT) * DRIVE_GEAR_RATIO / WHEEL_RADIUS_METERS,
-        /* floor_speed = wheel_angular_velocity * wheel_radius */
-        CHASSIS_MAX_VELOCITY =
-            DRIVE_MOTOR.freeSpeedRadPerSec
-                / DRIVE_GEAR_RATIO
-                * WHEEL_RADIUS_METERS, // calculate using choreo
-        CHASSIS_MAX_ACCELERATION_MPS_SQ =
-            Math.min(
-                MAX_FRICTION_ACCELERATION, // cannot exceed max friction
-                MAX_PROPELLING_FORCE_NEWTONS / ROBOT_MASS_KG),
-        CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC = CHASSIS_MAX_VELOCITY / DRIVE_BASE_RADIUS,
-        CHASSIS_MAX_ANGULAR_ACCELERATION_RAD_PER_SEC_SQ =
-            CHASSIS_MAX_ACCELERATION_MPS_SQ / DRIVE_BASE_RADIUS,
-        CHASSIS_FRICTIONAL_ANGULAR_ACCELERATION =
-            CHASSIS_MAX_ANGULAR_VELOCITY_RAD_PER_SEC / TIME_ROBOT_STOP_ROTATING_SECONDS;
-
-    /* for collision detection in simulation */
-    public static final double BUMPER_WIDTH_METERS = Units.inchesToMeters(34.5),
-        BUMPER_LENGTH_METERS = Units.inchesToMeters(36),
-        /* https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction */
-        BUMPER_COEFFICIENT_OF_FRICTION = 0.75,
-        /* https://simple.wikipedia.org/wiki/Coefficient_of_restitution */
-        BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08;
-
-    /* Gyro Sim */
-    public static final double GYRO_ANGULAR_ACCELERATION_THRESHOLD_SKIDDING_RAD_PER_SEC_SQ = 100;
-    public static final double SKIDDING_AMOUNT_AT_THRESHOLD_RAD = Math.toRadians(1.2);
-    /*
-     * https://store.ctr-electronics.com/pigeon-2/
-     * for a well-installed one with vibration reduction, only 0.4 degree
-     * but most teams just install it directly on the rigid chassis frame (including my team :D)
-     * so at least 1.2 degrees of drifting in 1 minutes for an average angular velocity of 60 degrees/second
-     * which is the average velocity during normal swerve-circular-offense
-     * */
-    public static final double NORMAL_GYRO_DRIFT_IN_1_MIN_Std_Dev_RAD = Math.toRadians(1.2);
-    public static final double AVERAGE_VELOCITY_RAD_PER_SEC_DURING_TEST = Math.toRadians(60);
-
-    public static final int SIMULATION_TICKS_IN_1_PERIOD = 5;
+    public static Constraints AUTO_ALIGN_ROTATIONAL_CONSTRAINTS =
+        new Constraints(DriveConstants.MAX_ANGULAR_SPEED_RADIANS_PER_SECOND, 2);
   }
 
   public static final ModuleConfig[] moduleConfigs =
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
index 740bb8b..c66b0d9 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
@@ -8,19 +8,22 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 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.math.system.plant.DCMotor;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.extras.setpointGen.SwerveSetpoint;
+import frc.robot.extras.setpointGen.SwerveSetpointGenerator;
+import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
 import frc.robot.extras.util.DeviceCANBus;
 import frc.robot.extras.util.TimeUtil;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
-import frc.robot.subsystems.swerve.gyroIO.GyroInputsAutoLogged;
-import frc.robot.subsystems.swerve.gyroIO.GyroInterface;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
+import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
+import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged;
+import frc.robot.subsystems.swerve.gyro.GyroInterface;
+import frc.robot.subsystems.swerve.module.ModuleInterface;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThreadInputsAutoLogged;
 import frc.robot.subsystems.vision.VisionConstants;
@@ -38,12 +41,25 @@ public class SwerveDrive extends SubsystemBase {
   private final SwerveModulePosition[] lastModulePositions;
   private final SwerveDrivePoseEstimator poseEstimator;
 
+  private final SwerveSetpointGenerator setpointGenerator =
+      new SwerveSetpointGenerator(
+          DriveConstants.MODULE_TRANSLATIONS,
+          DCMotor.getKrakenX60(1).withReduction(ModuleConstants.DRIVE_GEAR_RATIO),
+          DCMotor.getFalcon500(1).withReduction(1),
+          60,
+          58,
+          7,
+          ModuleConstants.WHEEL_DIAMETER_METERS,
+          WHEEL_GRIP.TIRE_WHEEL.cof,
+          0.0);
+  private SwerveSetpoint setpoint = SwerveSetpoint.zeroed();
+
   private final OdometryThread odometryThread;
 
+  private Optional<DriverStation.Alliance> alliance;
+
   private final Alert gyroDisconnectedAlert =
       new Alert("Gyro Hardware Fault", Alert.AlertType.kError);
-  private SwerveDriveKinematics kinematics;
-  private boolean isTest;
 
   public SwerveDrive(
       GyroInterface gyroIO,
@@ -55,7 +71,6 @@ public SwerveDrive(
     this.gyroInputs = new GyroInputsAutoLogged();
     this.rawGyroRotation = new Rotation2d();
 
-    setKinematics(DriveConstants.DRIVE_KINEMATICS);
     swerveModules =
         new SwerveModule[] {
           new SwerveModule(frontLeftModuleIO, "FrontLeft"),
@@ -71,9 +86,9 @@ public SwerveDrive(
           new SwerveModulePosition(),
           new SwerveModulePosition()
         };
-    poseEstimator =
+    this.poseEstimator =
         new SwerveDrivePoseEstimator(
-            getKinematics(),
+            DriveConstants.DRIVE_KINEMATICS,
             rawGyroRotation,
             lastModulePositions,
             new Pose2d(),
@@ -91,30 +106,6 @@ public SwerveDrive(
     gyroDisconnectedAlert.set(false);
   }
 
-  public SwerveDriveKinematics getKinematics() {
-    return kinematics;
-  }
-
-  public void setKinematics(SwerveDriveKinematics newKinematics) {
-    kinematics = newKinematics;
-  }
-
-  /**
-   * Gets the current velocity of the gyro's yaw
-   *
-   * @return the yaw velocity
-   */
-  public double getGyroRate() {
-    return gyroInputs.yawVelocity;
-  }
-
-  /** Updates the pose estimator with the pose calculated from the swerve modules. */
-  public void addPoseEstimatorSwerveMeasurement() {
-    for (int timestampIndex = 0;
-        timestampIndex < odometryThreadInputs.measurementTimeStamps.length;
-        timestampIndex++) addPoseEstimatorSwerveMeasurement(timestampIndex);
-  }
-
   /*
    * Updates the pose estimator with the pose calculated from the april tags. How much it
    * contributes to the pose estimation is set by setPoseEstimatorVisionConfidence.
@@ -142,6 +133,7 @@ public void setPoseEstimatorVisionConfidence(
         VecBuilder.fill(xStandardDeviation, yStandardDeviation, thetaStandardDeviation));
   }
 
+  @Override
   public void periodic() {
     final double t0 = TimeUtil.getRealTimeSeconds();
     fetchOdometryInputs();
@@ -171,7 +163,7 @@ public double getCharacterizationVelocity() {
   }
 
   /** Processes odometry inputs */
-  void fetchOdometryInputs() {
+  private void fetchOdometryInputs() {
     odometryThread.lockOdometry();
     odometryThread.updateInputs(odometryThreadInputs);
     Logger.processInputs("Drive/OdometryThread", odometryThreadInputs);
@@ -185,6 +177,7 @@ void fetchOdometryInputs() {
     odometryThread.unlockOdometry();
   }
 
+  /** Runs the SwerveModules periodic methods */
   private void modulesPeriodic() {
     for (SwerveModule module : swerveModules) module.periodic();
   }
@@ -198,28 +191,82 @@ private void modulesPeriodic() {
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   public void drive(double xSpeed, double ySpeed, double rotationSpeed, boolean fieldRelative) {
-    SwerveModuleState[] swerveModuleStates =
-        getKinematics()
-            .toSwerveModuleStates(
-                fieldRelative
-                    ? ChassisSpeeds.fromFieldRelativeSpeeds(
-                        xSpeed, ySpeed, rotationSpeed, getPose().getRotation())
-                    : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed));
-    SwerveDriveKinematics.desaturateWheelSpeeds(
-        swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND);
-
-    setModuleStates(swerveModuleStates);
-    Logger.recordOutput("SwerveStates/SwerveModuleStates", swerveModuleStates);
+    ChassisSpeeds desiredSpeeds =
+        fieldRelative
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(
+                xSpeed, ySpeed, rotationSpeed, getOdometryAllianceRelativeRotation2d())
+            : new ChassisSpeeds(xSpeed, ySpeed, rotationSpeed);
+
+    setpoint = setpointGenerator.generateSetpoint(setpoint, desiredSpeeds, 0.02);
+
+    setModuleStates(setpoint.moduleStates());
+    Logger.recordOutput("SwerveStates/DesiredStates", setpoint.moduleStates());
+  }
+
+  /**
+   * Returns the heading of the robot in degrees from 0 to 360.
+   *
+   * @return Value is Counter-clockwise positive.
+   */
+  public double getHeading() {
+    return gyroInputs.yawDegrees;
+  }
+
+  /**
+   * Gets the rate of rotation of the NavX.
+   *
+   * @return The current rate in degrees per second.
+   */
+  public double getGyroRate() {
+    return gyroInputs.yawVelocity;
+  }
+
+  /** Returns a Rotation2d for the heading of the robot. */
+  public Rotation2d getGyroRotation2d() {
+    return Rotation2d.fromDegrees(getHeading());
+  }
+
+  /** Returns a Rotation2d for the heading of the robot. */
+  public Rotation2d getGyroFieldRelativeRotation2d() {
+    return Rotation2d.fromDegrees(getHeading() + getAllianceAngleOffset());
   }
 
   /** Returns 0 degrees if the robot is on the blue alliance, 180 if on the red alliance. */
   public double getAllianceAngleOffset() {
-    Optional<Alliance> alliance = DriverStation.getAlliance();
+    alliance = DriverStation.getAlliance();
     double offset =
         alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red ? 180.0 : 0.0;
     return offset;
   }
 
+  /** Zeroes the heading of the robot. */
+  public void zeroHeading() {
+    gyroIO.reset();
+  }
+
+  /**
+   * Returns the estimated field-relative pose of the robot. Positive x being forward, positive y
+   * being left.
+   */
+  @AutoLogOutput(key = "Odometry/Odometry")
+  public Pose2d getPose() {
+    return poseEstimator.getEstimatedPosition();
+  }
+
+  /** Returns a Rotation2d for the heading of the robot */
+  public Rotation2d getOdometryRotation2d() {
+    return getPose().getRotation();
+  }
+
+  /**
+   * Returns a Rotation2d for the heading of the robot relative to the field from the driver's
+   * perspective. This method is needed so that the drive command and poseEstimator don't fight each
+   * other. It uses odometry rotation.
+   */
+  public Rotation2d getOdometryAllianceRelativeRotation2d() {
+    return getPose().getRotation().plus(Rotation2d.fromDegrees(getAllianceAngleOffset()));
+  }
+
   /**
    * Sets the modules to the specified states.
    *
@@ -228,7 +275,7 @@ public double getAllianceAngleOffset() {
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
     for (int i = 0; i < 4; i++) {
-      swerveModules[i].runSetpoint(desiredStates[i]);
+      swerveModules[i].runSetPoint((desiredStates[i]));
     }
   }
 
@@ -237,38 +284,27 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
    *
    * @param timestampIndex index of the timestamp to sample the pose at
    */
-  private void addPoseEstimatorSwerveMeasurement(int timestampIndex) {
-    final SwerveModulePosition[] modulePositions = getModulesPosition(timestampIndex),
+  public void addPoseEstimatorSwerveMeasurement() { // int timestampIndex
+    final SwerveModulePosition[] modulePositions = getModulePositions(),
         moduleDeltas = getModulesDelta(modulePositions);
 
     if (gyroInputs.isConnected) {
-      rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex];
+      // rawGyroRotation = gyroInputs.odometryYawPositions[timestampIndex];
+      rawGyroRotation = getGyroRotation2d();
     } else {
-      Twist2d twist = getKinematics().toTwist2d(moduleDeltas);
+      Twist2d twist = DriveConstants.DRIVE_KINEMATICS.toTwist2d(moduleDeltas);
       rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta));
     }
 
     poseEstimator.updateWithTime(
-        odometryThreadInputs.measurementTimeStamps[timestampIndex],
-        rawGyroRotation,
-        modulePositions);
+        // odometryThreadInputs.measurementTimeStamps[timestampIndex],
+        Logger.getTimestamp(), rawGyroRotation, modulePositions);
   }
 
   /**
-   * Gets the modules positions, sampled at the indexed timestamp.
-   *
-   * @param timestampIndex the timestamp index to sample.
-   * @return a list of SwerveModulePosition, containing relative drive position and absolute turn
-   *     rotation at the sampled timestamp.
+   * @param freshModulesPosition Latest module positions
+   * @return The change of the module positions between the current and last update
    */
-  private SwerveModulePosition[] getModulesPosition(int timestampIndex) {
-    SwerveModulePosition[] swerveModulePositions = new SwerveModulePosition[swerveModules.length];
-    for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++)
-      swerveModulePositions[moduleIndex] =
-          swerveModules[moduleIndex].getOdometryPositions()[timestampIndex];
-    return swerveModulePositions;
-  }
-
   private SwerveModulePosition[] getModulesDelta(SwerveModulePosition[] freshModulesPosition) {
     SwerveModulePosition[] deltas = new SwerveModulePosition[swerveModules.length];
     for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) {
@@ -297,17 +333,6 @@ private SwerveModulePosition[] getModulePositions() {
     return positions;
   }
 
-  /** Gets the fused pose from the pose estimator. */
-  @AutoLogOutput(key = "Odometry/RobotPosition")
-  public Pose2d getPose() {
-    return poseEstimator.getEstimatedPosition();
-  }
-
-  /** Gets the current gyro yaw */
-  public Rotation2d getRawGyroYaw() {
-    return gyroInputs.yawDegreesRotation2d;
-  }
-
   /**
    * Sets the pose
    *
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
index 399ab4c..342ea77 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems.swerve;
 
-import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.*;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -10,8 +10,8 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
+import frc.robot.subsystems.swerve.module.ModuleInputsAutoLogged;
+import frc.robot.subsystems.swerve.module.ModuleInterface;
 import org.littletonrobotics.junction.Logger;
 
 public class SwerveModule extends SubsystemBase {
@@ -19,8 +19,6 @@ public class SwerveModule extends SubsystemBase {
   private final String name;
   private final ModuleInputsAutoLogged inputs = new ModuleInputsAutoLogged();
 
-  SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {};
-
   private final Alert hardwareFaultAlert;
 
   public SwerveModule(ModuleInterface io, String name) {
@@ -34,6 +32,7 @@ public SwerveModule(ModuleInterface io, String name) {
     CommandScheduler.getInstance().unregisterSubsystem(this);
   }
 
+  /** Updates the module's odometry inputs. */
   public void updateOdometryInputs() {
     io.updateInputs(inputs);
     Logger.processInputs("Drive/Module-" + name, inputs);
@@ -43,22 +42,30 @@ public void updateOdometryInputs() {
   @Override
   public void periodic() {}
 
+  /** Sets the drive voltage of the module. */
   public void setVoltage(Voltage volts) {
     io.setDriveVoltage(volts);
     io.setTurnVoltage(Volts.zero());
   }
 
+  /** Gets the drive voltage of the module. */
   public double getDriveVoltage() {
     return inputs.driveAppliedVolts;
   }
 
+  /** Sets the drive velocity of the module. */
   public double getCharacterizationVelocity() {
     return inputs.driveVelocity;
   }
 
-  /** Runs the module with the specified setpoint state. Returns the optimized state. */
-  public void runSetpoint(SwerveModuleState state) {
+  /** Runs the module with the specified setpoint state. Returns optimized setpoint */
+  public void runSetPoint(SwerveModuleState state) {
+    state.optimize(getTurnRotation());
+    if (state.speedMetersPerSecond < 0.01) {
+      io.stopModule();
+    }
     io.setDesiredState(state);
+    Logger.recordOutput("Drive/desired turn angle", state.angle.getRotations());
   }
 
   /** Returns the current turn angle of the module. */
@@ -72,12 +79,12 @@ public double getTurnVelocity() {
 
   /** Returns the current drive position of the module in meters. */
   public double getDrivePositionMeters() {
-    return ModuleConstants.DRIVE_TO_METERS * inputs.drivePosition;
+    return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.drivePosition;
   }
 
   /** Returns the current drive velocity of the module in meters per second. */
   public double getDriveVelocityMetersPerSec() {
-    return ModuleConstants.DRIVE_TO_METERS_PER_SECOND * inputs.driveVelocity;
+    return ModuleConstants.WHEEL_CIRCUMFERENCE_METERS * inputs.driveVelocity;
   }
 
   /** Returns the module state (turn angle and drive velocity). */
@@ -85,11 +92,6 @@ public SwerveModuleState getMeasuredState() {
     return new SwerveModuleState(getDriveVelocityMetersPerSec(), getTurnRotation());
   }
 
-  /** Returns the module positions received this cycle. */
-  public SwerveModulePosition[] getOdometryPositions() {
-    return odometryPositions;
-  }
-
   /**
    * Gets the module position consisting of the distance it has traveled and the angle it is
    * rotated.
@@ -97,8 +99,6 @@ public SwerveModulePosition[] getOdometryPositions() {
    * @return a SwerveModulePosition object containing position and rotation
    */
   public SwerveModulePosition getPosition() {
-    double position = ModuleConstants.DRIVE_TO_METERS * getDrivePositionMeters();
-    Rotation2d rotation = getTurnRotation();
-    return new SwerveModulePosition(position, rotation);
+    return new SwerveModulePosition(getDrivePositionMeters(), getTurnRotation());
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java
similarity index 86%
rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java
rename to src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java
index b2fa566..2d45260 100644
--- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java
+++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.swerve.gyroIO;
+package frc.robot.subsystems.swerve.gyro;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import org.littletonrobotics.junction.AutoLog;
@@ -22,4 +22,7 @@ public static class GyroInputs {
    * @param inputs inputs to update
    */
   default void updateInputs(GyroInputs inputs) {}
+
+  /** Resets the gyro yaw */
+  default void reset() {}
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java
similarity index 71%
rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java
rename to src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java
index fd107a0..0dfc028 100644
--- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/PhysicalGyro.java
+++ b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java
@@ -1,28 +1,29 @@
-package frc.robot.subsystems.swerve.gyroIO;
+package frc.robot.subsystems.swerve.gyro;
 
 import static edu.wpi.first.units.Units.*;
 
-import com.kauailabs.navx.frc.AHRS;
+import com.studica.frc.AHRS;
+import com.studica.frc.AHRS.NavXComType;
+import com.studica.frc.AHRS.NavXUpdateRate;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.wpilibj.SPI;
 import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
 import java.util.Queue;
 
 public class PhysicalGyro implements GyroInterface {
-  private final AHRS gyro = new AHRS(SPI.Port.kMXP, (byte) 250);
+  private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI, NavXUpdateRate.k200Hz);
   private final Queue<Angle> yawPositionInput;
 
   public PhysicalGyro() {
-    yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(-gyro.getAngle()));
+    yawPositionInput = OdometryThread.registerInput(() -> Degrees.of(gyro.getAngle()));
   }
 
   @Override
   public void updateInputs(GyroInputs inputs) {
     inputs.isConnected = gyro.isConnected();
     inputs.yawDegreesRotation2d = gyro.getRotation2d();
-    inputs.yawVelocity = -gyro.getRate();
-    inputs.yawDegrees = -gyro.getAngle();
+    inputs.yawVelocity = gyro.getRate();
+    inputs.yawDegrees = gyro.getAngle();
 
     // Handle odometry yaw positions
     if (!yawPositionInput.isEmpty()) {
@@ -35,4 +36,9 @@ public void updateInputs(GyroInputs inputs) {
       yawPositionInput.clear();
     }
   }
+
+  @Override
+  public void reset() {
+    gyro.reset();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java
similarity index 78%
rename from src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java
rename to src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java
index cea6b99..4197e45 100644
--- a/src/main/java/frc/robot/subsystems/swerve/gyroIO/SimulatedGyro.java
+++ b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java
@@ -1,8 +1,9 @@
-package frc.robot.subsystems.swerve.gyroIO;
+package frc.robot.subsystems.swerve.gyro;
 
 import static edu.wpi.first.units.Units.DegreesPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.extras.simulation.OdometryTimestampsSim;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
 
@@ -19,8 +20,14 @@ public void updateInputs(GyroInputs inputs) {
     inputs.odometryYawPositions = gyroSimulation.getCachedGyroReadings();
     inputs.odometryYawTimestamps = OdometryTimestampsSim.getTimestamps();
     inputs.yawDegreesRotation2d = gyroSimulation.getGyroReading();
+    inputs.yawDegrees = gyroSimulation.getGyroReading().getDegrees();
     inputs.yawVelocity =
         RadiansPerSecond.of(gyroSimulation.getMeasuredAngularVelocityRadPerSec())
             .in(DegreesPerSecond);
   }
+
+  @Override
+  public void reset() {
+    gyroSimulation.setRotation(Rotation2d.kZero);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java
similarity index 87%
rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java
rename to src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java
index 25b0bf9..7fdbbcd 100644
--- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java
+++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleInterface.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.swerve.moduleIO;
+package frc.robot.subsystems.swerve.module;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
@@ -23,7 +23,8 @@ class ModuleInputs {
 
     public double[] odometryTimestamps = new double[] {};
 
-    public double turnPosition = 0.0;
+    public double[] odometryDriveWheelRevolutions = new double[] {};
+    public Rotation2d[] odometrySteerPositions = new Rotation2d[] {};
   }
 
   /**
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java
similarity index 88%
rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java
rename to src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java
index 48da6ab..f89a9bf 100644
--- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/module/PhysicalModule.java
@@ -1,4 +1,4 @@
-package frc.robot.subsystems.swerve.moduleIO;
+package frc.robot.subsystems.swerve.module;
 
 import static edu.wpi.first.units.Units.*;
 
@@ -11,7 +11,6 @@
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -49,15 +48,13 @@ public class PhysicalModule implements ModuleInterface {
   private final BaseStatusSignal[] periodicallyRefreshedSignals;
 
   public PhysicalModule(ModuleConfig moduleConfig) {
-    driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.RIO.name);
-    turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.RIO.name);
-    turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.RIO.name);
+    driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name);
+    turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name);
+    turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name);
 
     CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration();
     turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero();
     turnEncoderConfig.MagnetSensor.SensorDirection = moduleConfig.encoderReversed();
-    turnEncoderConfig.MagnetSensor.AbsoluteSensorRange =
-        AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
     turnEncoder.getConfigurator().apply(turnEncoderConfig, HardwareConstants.TIMEOUT_S);
 
     TalonFXConfiguration driveConfig = new TalonFXConfiguration();
@@ -153,13 +150,9 @@ public void updateInputs(ModuleInputs inputs) {
       turnEncoderAbsolutePosition.clear();
     }
 
-    inputs.turnPosition = turnMotor.getPosition().getValueAsDouble();
-
     inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble();
     inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble();
 
-    inputs.turnPosition = turnMotor.getPosition().getValueAsDouble();
-
     inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble();
     inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble();
     inputs.turnCurrentAmps = turnMotorCurrent.getValueAsDouble();
@@ -177,29 +170,15 @@ public void setTurnVoltage(Voltage volts) {
 
   @Override
   public void setDesiredState(SwerveModuleState desiredState) {
-    double turnRotations = getTurnRotations();
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState setpoint =
-        new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
-
-    setpoint.optimize(Rotation2d.fromRotations(turnRotations));
-    setpoint.cosineScale(Rotation2d.fromRotations(turnRotations));
-
-    if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) {
-      driveMotor.set(0);
-      turnMotor.set(0);
-      return;
-    }
-
     // Converts meters per second to rotations per second
     double desiredDriveRPS =
-        setpoint.speedMetersPerSecond
+        desiredState.speedMetersPerSecond
             * ModuleConstants.DRIVE_GEAR_RATIO
             / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
 
     driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS)));
     turnMotor.setControl(
-        mmPositionRequest.withPosition(Rotations.of(setpoint.angle.getRotations())));
+        mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations())));
   }
 
   public double getTurnRotations() {
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
similarity index 58%
rename from src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
rename to src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
index ff474ca..44094b5 100644
--- a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
+++ b/src/main/java/frc/robot/subsystems/swerve/module/SimulatedModule.java
@@ -1,14 +1,12 @@
-package frc.robot.subsystems.swerve.moduleIO;
+package frc.robot.subsystems.swerve.module;
 
 import static edu.wpi.first.units.Units.*;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Voltage;
 import frc.robot.extras.simulation.OdometryTimestampsSim;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
@@ -18,14 +16,20 @@
 public class SimulatedModule implements ModuleInterface {
   private final SwerveModuleSimulation moduleSimulation;
 
-  private final PIDController drivePID = new PIDController(5, 0, 0);
-  private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(5, 0, 0);
-  private final Constraints turnConstraints = new Constraints(5, 0);
-  private final ProfiledPIDController turnPID = new ProfiledPIDController(5, 0, 0, turnConstraints);
-  private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(5, 0, 0);
+  private final PIDController drivePID = new PIDController(.27, 0, 0);
+  private final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(1, 1.5);
+
+  private final Constraints turnConstraints =
+      new Constraints(
+          ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND,
+          ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED);
+  private final ProfiledPIDController turnPID =
+      new ProfiledPIDController(18, 0, 0, turnConstraints);
+  private final SimpleMotorFeedforward turnFF = new SimpleMotorFeedforward(0, 0, 0);
 
   public SimulatedModule(SwerveModuleSimulation moduleSimulation) {
     this.moduleSimulation = moduleSimulation;
+    turnPID.enableContinuousInput(-Math.PI, Math.PI);
   }
 
   @Override
@@ -36,18 +40,19 @@ public void updateInputs(ModuleInputs inputs) {
         RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec())
             .in(RotationsPerSecond);
     inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts();
-    inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorSupplyCurrentAmps());
+    inputs.driveCurrentAmps = moduleSimulation.getDriveMotorSupplyCurrentAmps();
 
-    // inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition();
-    inputs.turnPosition =
-        Radians.of(moduleSimulation.getTurnRelativeEncoderPositionRad()).in(Rotations);
+    inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition();
     inputs.turnVelocity =
-        RadiansPerSecond.of(moduleSimulation.getTurnRelativeEncoderSpeedRadPerSec())
-            .in(RotationsPerSecond);
+        Radians.of(moduleSimulation.getTurnAbsoluteEncoderSpeedRadPerSec()).in(Rotations);
     inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts();
-    inputs.turnCurrentAmps = Math.abs(moduleSimulation.getTurnMotorSupplyCurrentAmps());
+    inputs.turnCurrentAmps = moduleSimulation.getTurnMotorSupplyCurrentAmps();
+
+    inputs.odometrySteerPositions = moduleSimulation.getCachedTurnAbsolutePositions();
 
     inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps();
+
+    inputs.isConnected = true;
   }
 
   @Override
@@ -60,48 +65,38 @@ public void setTurnVoltage(Voltage volts) {
     moduleSimulation.requestTurnVoltageOut(volts);
   }
 
+  @Override
   public void setDesiredState(SwerveModuleState desiredState) {
-    double turnRotations = getTurnRotations();
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState setpoint =
-        new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
-
-    setpoint.optimize(Rotation2d.fromRotations(turnRotations));
-    setpoint.cosineScale(Rotation2d.fromRotations(turnRotations));
-
-    if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) {
-      moduleSimulation.requestDriveVoltageOut(0);
-      moduleSimulation.requestTurnVoltageOut(0);
-      return;
-    }
-
     // Converts meters per second to rotations per second
     double desiredDriveRPS =
-        setpoint.speedMetersPerSecond
+        desiredState.speedMetersPerSecond
             * ModuleConstants.DRIVE_GEAR_RATIO
             / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
 
     moduleSimulation.requestDriveVoltageOut(
         Volts.of(
                 drivePID.calculate(
-                    Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()),
+                    RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec())
+                            .in(RotationsPerSecond)
+                        * ModuleConstants.WHEEL_CIRCUMFERENCE_METERS,
                     desiredDriveRPS))
-            .plus(driveFF.calculate(RotationsPerSecond.of(desiredDriveRPS))));
+            .plus(Volts.of(driveFF.calculate(desiredDriveRPS))));
     moduleSimulation.requestTurnVoltageOut(
         Volts.of(
                 turnPID.calculate(
                     moduleSimulation.getTurnAbsolutePosition().getRotations(),
                     desiredState.angle.getRotations()))
-            .plus(turnFF.calculate(RotationsPerSecond.of(turnPID.getSetpoint().velocity))));
+            .plus(Volts.of(turnFF.calculate(turnPID.getSetpoint().velocity))));
   }
 
+  @Override
   public double getTurnRotations() {
     return moduleSimulation.getTurnAbsolutePosition().getRotations();
   }
 
   @Override
   public void stopModule() {
-    moduleSimulation.requestDriveVoltageOut(Volts.zero());
-    moduleSimulation.requestTurnVoltageOut(Volts.zero());
+    moduleSimulation.requestDriveVoltageOut(Volts.of(0));
+    moduleSimulation.requestTurnVoltageOut(Volts.of(0));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java
index b65ad20..ebc9216 100644
--- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java
+++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThread.java
@@ -3,11 +3,12 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import edu.wpi.first.units.measure.Angle;
+import frc.robot.Constants;
 import frc.robot.Constants.HardwareConstants;
+import frc.robot.Constants.SimulationConstants;
 import frc.robot.Robot;
 import frc.robot.extras.util.DeviceCANBus;
 import frc.robot.extras.util.TimeUtil;
-import frc.robot.subsystems.swerve.SwerveConstants.SimulationConstants;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Queue;
@@ -47,7 +48,7 @@ static Queue<Angle> registerInput(Supplier<Angle> supplier) {
   }
 
   static OdometryThread createInstance(DeviceCANBus canBus) {
-    return switch (Robot.CURRENT_ROBOT_MODE) {
+    return switch (Constants.CURRENT_MODE) {
       case REAL ->
           new OdometryThreadReal(
               canBus,
diff --git a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java
index 1b8f17c..879f431 100644
--- a/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/odometryThread/OdometryThreadReal.java
@@ -55,7 +55,8 @@ private void odometryPeriodic() {
   private void refreshSignalsAndBlockThread() {
     switch (canBus) {
       case RIO -> {
-        TimeUtil.delay(1.0 / HardwareConstants.SIGNAL_FREQUENCY);
+        BaseStatusSignal.setUpdateFrequencyForAll(
+            1.0 / HardwareConstants.SIGNAL_FREQUENCY, statusSignals);
         BaseStatusSignal.refreshAll();
       }
       case CANIVORE -> BaseStatusSignal.waitForAll(HardwareConstants.TIMEOUT_S, statusSignals);
diff --git a/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java
new file mode 100644
index 0000000..2c07913
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/PhysicalVision.java
@@ -0,0 +1,415 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.extras.util.GeomUtil;
+import frc.robot.extras.vision.LimelightHelpers;
+import frc.robot.extras.vision.LimelightHelpers.PoseEstimate;
+import frc.robot.extras.vision.MegatagPoseEstimate;
+import frc.robot.subsystems.vision.VisionConstants.Limelight;
+// import frc.robot.subsystems.vision.VisionInterface.VisionInputs;
+import java.util.concurrent.ConcurrentHashMap;
+// import java.util.concurrent.ExecutorService;
+// import java.util.concurrent.Executors;
+import java.util.concurrent.atomic.AtomicReference;
+
+public class PhysicalVision implements VisionInterface {
+
+  private Pose2d lastSeenPose = new Pose2d();
+  private double headingDegrees = 0;
+  private double headingRateDegreesPerSecond = 0;
+  private final ConcurrentHashMap<Limelight, AtomicReference<VisionInputs>> limelightThreads =
+      new ConcurrentHashMap<>();
+  // private final ExecutorService executorService = Executors.newFixedThreadPool(3);
+  private final AtomicReference<VisionInputs> latestInputs =
+      new AtomicReference<>(new VisionInputs());
+  private final ThreadManager threadManager = new ThreadManager(Limelight.values().length);
+
+  /**
+   * The pose estimates from the limelights in the following order {shooterLimelight,
+   * frontLeftLimelight, frontRightLimelight}
+   */
+  private MegatagPoseEstimate[] limelightEstimates =
+      new MegatagPoseEstimate[] {
+        new MegatagPoseEstimate(), new MegatagPoseEstimate(), new MegatagPoseEstimate()
+      };
+
+  public PhysicalVision() {
+    for (Limelight limelight : Limelight.values()) {
+      limelightThreads.put(limelight, new AtomicReference<>(latestInputs.get()));
+
+      // Start a vision input task for each Limelight
+      threadManager.startVisionInputTask(
+          limelight.getName(), latestInputs.get(), () -> visionThreadTask(latestInputs.get()));
+    }
+  }
+
+  @Override
+  public void updateInputs(VisionInputs inputs) {
+    // Combine inputs into the main inputs object
+    synchronized (inputs) {
+      for (Limelight limelight : Limelight.values()) {
+        inputs.isLimelightConnected[limelight.getId()] = isLimelightConnected(limelight);
+        inputs.limelightLatencies[limelight.getId()] = getLatencySeconds(limelight);
+        inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight);
+        inputs.limelightSeesAprilTags[limelight.getId()] = canSeeAprilTags(limelight);
+        inputs.limelightMegatagPoses[limelight.getId()] = enabledPoseUpdate(limelight);
+        inputs.limelightAprilTagDistances[limelight.getId()] =
+            getLimelightAprilTagDistance(limelight);
+        inputs.limelightCalculatedPoses[limelight.getId()] = getPoseFromAprilTags(limelight);
+        inputs.limelightTimestamps[limelight.getId()] = getTimeStampSeconds(limelight);
+        inputs.limelightLastSeenPose = getLastSeenPose();
+        inputs.limelightAprilTagDistances[limelight.getId()] =
+            getLimelightAprilTagDistance(limelight);
+
+        latestInputs.set(inputs);
+        limelightThreads.get(limelight).set(latestInputs.get());
+      }
+    }
+  }
+
+  /**
+   * Checks if the specified limelight can fully see one or more April Tag.
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return true if the limelight can fully see one or more April Tag
+   */
+  @Override
+  public boolean canSeeAprilTags(Limelight limelight) {
+    // First checks if it can see an april tag, then checks if it is fully in frame
+    // Different Limelights have different FOVs
+    if (getNumberOfAprilTags(limelight) > VisionConstants.MIN_APRIL_TAG_ID
+        && getNumberOfAprilTags(limelight) <= VisionConstants.MAX_APRIL_TAG_ID) {
+      if (limelight.getName().equals(Limelight.SHOOTER.getName())) {
+        return Math.abs(LimelightHelpers.getTX(limelight.getName()))
+            <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR;
+      } else {
+        // return false;
+        return Math.abs(LimelightHelpers.getTX(limelight.getName()))
+            <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR;
+      }
+    }
+    // return latestInputs.get().limelightSeesAprilTags[limelight.getId()] = false;
+    // return  LimelightHelpers.getTV(limelight.getName());
+    return false;
+    // latestInputs.get().limelightSeesAprilTags[limelight.getId()] =
+  }
+
+  /**
+   * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which
+   * is then placed into its corresponding spot in the limelightEstimates array.
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   */
+  public MegatagPoseEstimate enabledPoseUpdate(Limelight limelight) {
+    PoseEstimate megatag1Estimate = getMegaTag1PoseEstimate(limelight);
+    PoseEstimate megatag2Estimate = getMegaTag2PoseEstimate(limelight);
+
+    if (canSeeAprilTags(limelight)
+        && isValidPoseEstimate(limelight, megatag1Estimate, megatag2Estimate)) {
+      if (isLargeDiscrepancyBetweenMegaTag1And2(limelight, megatag1Estimate, megatag2Estimate)
+          && getLimelightAprilTagDistance(limelight)
+              < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
+        return limelightEstimates[limelight.getId()] =
+            MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight));
+      } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) {
+        LimelightHelpers.SetRobotOrientation(limelight.getName(), headingDegrees, 0, 0, 0, 0, 0);
+        return limelightEstimates[limelight.getId()] =
+            MegatagPoseEstimate.fromLimelight(getMegaTag2PoseEstimate(limelight));
+      } else {
+        return limelightEstimates[limelight.getId()] =
+            MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight));
+      }
+    }
+    return limelightEstimates[limelight.getId()] = new MegatagPoseEstimate();
+  }
+
+  /**
+   * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link
+   * #enabledPoseUpdate(int)}
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   */
+  public void updatePoseEstimate(Limelight limelight, VisionInputs inputs) {
+    synchronized (inputs) {
+      limelightEstimates[limelight.getId()] =
+          DriverStation.isEnabled()
+              ? enabledPoseUpdate(limelight)
+              : MegatagPoseEstimate.fromLimelight(getMegaTag1PoseEstimate(limelight));
+    }
+  }
+
+  /**
+   * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates.
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return true if the discrepancy is larger than the defined threshold, false otherwise
+   */
+  public boolean isLargeDiscrepancyBetweenMegaTag1And2(
+      Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) {
+    return !GeomUtil.isTranslationWithinThreshold(
+            mt1.pose.getTranslation(),
+            mt2.pose.getTranslation(),
+            VisionConstants.MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD)
+        || !GeomUtil.isRotationWithinThreshold(
+            mt1.pose.getRotation().getDegrees(),
+            mt2.pose.getRotation().getDegrees(),
+            VisionConstants.MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD);
+    // return true;
+  }
+
+  /**
+   * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it
+   * sees
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will
+   *     return 0 for x, y, and theta
+   */
+  public PoseEstimate getMegaTag1PoseEstimate(Limelight limelight) {
+    return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelight.getName());
+  }
+
+  /**
+   * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it
+   * sees
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will
+   *     return 0 for x, y, and theta
+   */
+  public PoseEstimate getMegaTag2PoseEstimate(Limelight limelight) {
+    return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelight.getName());
+  }
+
+  /**
+   * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return true if the pose estimate exists within the field and the pose estimate is not null
+   */
+  public boolean isValidPoseEstimate(Limelight limelight, PoseEstimate mt1, PoseEstimate mt2) {
+    return LimelightHelpers.isValidPoseEstimate(mt1)
+        && LimelightHelpers.isValidPoseEstimate(mt2)
+        && isWithinFieldBounds(mt1, mt2);
+  }
+
+  /**
+   * Checks whether the pose estimate for MT1 and MT2 is within the field
+   *
+   * @param megaTag1Estimate the MT1 pose estimate to check
+   * @param megaTag2Estimate the MT2 pose estimate to check
+   */
+  private boolean isWithinFieldBounds(
+      PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) {
+    return (megaTag1Estimate.pose.getX() > 0
+            && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
+        && (megaTag1Estimate.pose.getY() > 0
+            && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)
+        && (megaTag2Estimate.pose.getX() > 0
+            && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
+        && (megaTag2Estimate.pose.getY() > 0
+            && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS);
+  }
+
+  /**
+   * Gets the pose of the robot calculated by specified limelight via any April Tags it sees
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for
+   *     x, y, and theta
+   */
+  @Override
+  public Pose2d getPoseFromAprilTags(Limelight limelight) {
+    return limelightEstimates[limelight.getId()].fieldToCamera;
+  }
+
+  // public Pose2d getAprilTagPositionToLimelight(Limelight limelight) {
+  //   return LimelightHelpers.getTargetPose_CameraSpace(limelight.getName());
+  // }
+
+  // public Pose2d getAprilTagPositionToRobot(Limelight limelight) {
+  //   return latestInputs.get().limelightRobotToTargetPose[limelight.getId()]
+  // =LimelightHelpers.getTargetPose_RobotSpace(limelight.getName());
+  // }
+
+  /** Returns how many april tags the limelight that is being used for pose estimation can see. */
+  @Override
+  public int getNumberOfAprilTags(Limelight limelight) {
+    return limelightEstimates[limelight.getId()].tagCount;
+    // latestInputs.get().limelightMegatagPose[limelight.getId()].fiducialIds.length;
+    // return limelightEstimates[limelight.getId()].tagCount;
+  }
+
+  /**
+   * Returns the timestamp for the vision measurement from the limelight that is being used for pose
+   * estimation.
+   */
+  @Override
+  public double getTimeStampSeconds(Limelight limelight) {
+    return limelightEstimates[limelight.getId()].timestampSeconds / 1000.0;
+  }
+
+  /**
+   * Returns the latency in seconds of when the limelight that is being used for pose estimation
+   * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing
+   * latency.
+   */
+  @Override
+  public double getLatencySeconds(Limelight limelight) {
+    return (limelightEstimates[limelight.getId()].latency) / 1000.0;
+  }
+
+  /** Gets the pose calculated the last time a limelight saw an April Tag */
+  @Override
+  public Pose2d getLastSeenPose() {
+    return lastSeenPose;
+  }
+
+  /**
+   * Gets the average distance between the specified limelight and the April Tags it sees
+   *
+   * @param limelight a limelight (SHOOTER, FRONT_LEFT, FRONT_RIGHT).
+   * @return the average distance between the robot and the April Tag(s) in meters
+   */
+  @Override
+  public double getLimelightAprilTagDistance(Limelight limelight) {
+    if (canSeeAprilTags(limelight)) {
+      return limelightEstimates[limelight.getId()].avgTagDist;
+    }
+    // To be safe returns a big distance from the april tags if it can't see any
+    return Double.MAX_VALUE;
+  }
+
+  /**
+   * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and
+   * 2 for pose estimation.
+   *
+   * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red
+   *     alliance)
+   * @param headingRateDegrees the rate the robot is rotating, CCW positive
+   */
+  @Override
+  public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
+    this.headingDegrees = headingDegrees;
+    this.headingRateDegreesPerSecond = headingRateDegrees;
+  }
+
+  /**
+   * Checks if the specified limelight is connected
+   *
+   * @param limelight The limelight to check
+   * @return True if the limelight network table contains the key "tv"
+   */
+  public boolean isLimelightConnected(Limelight limelight) {
+    NetworkTable limelightTable = LimelightHelpers.getLimelightNTTable(limelight.getName());
+    return limelightTable.containsKey("tv");
+  }
+
+  /**
+   * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate
+   *
+   * @param limelight the limelight number
+   */
+  public void checkAndUpdatePose(Limelight limelight, VisionInputs inputs) {
+    double last_TX = 0;
+    double last_TY = 0;
+
+    // Syncronization block to ensure thread safety during the critical section where pose
+    // information is read and compared.
+    // This helps prevent race conditions, where one limelight may be updating an object that
+    // another limelight is reading.
+    // A race condition could cause unpredictable things to happen. Such as causing a limelight to
+    // be unable to reference an
+    // object, as its reference was modified earlier.
+    synchronized (this) {
+      try {
+        double current_TX = LimelightHelpers.getTX(limelight.getName());
+        double current_TY = LimelightHelpers.getTY(limelight.getName());
+
+        // This checks if the limelight reading is new. The reasoning being that if the TX and TY
+        // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way,
+        // because to get the timestamp of the reading, you need to parse the JSON dump which can be
+        // very demanding whereas this only has to get the Network Table entries for TX and TY.
+        if (current_TX != last_TX || current_TY != last_TY) {
+
+          updatePoseEstimate(limelight, inputs);
+          latestInputs.set(inputs);
+
+          limelightThreads.computeIfPresent(limelight, (key, value) -> latestInputs);
+          // // Handle threading for Limelight (start or stop threads if needed)
+          // Check if this Limelight thread exists in limelightThreads
+          if (limelightThreads.get(limelight) != null) {
+            // Update thread inputs or restart the thread if needed
+            limelightThreads.get(limelight).set(latestInputs.get());
+          }
+
+          // This is to keep track of the last valid pose calculated by the limelights
+          // it is used when the driver resets the robot odometry to the limelight calculated
+          // position
+          if (canSeeAprilTags(limelight)) {
+            lastSeenPose = getMegaTag1PoseEstimate(limelight).pose;
+          }
+        } else {
+          // // Retrieve the AtomicReference for the given limelight number
+          AtomicReference<VisionInputs> isThreadRunning =
+              limelightThreads.getOrDefault(limelight, latestInputs);
+          // Only stop the thread if it's currently running
+          if (isThreadRunning.get() != null) {
+            // stop the thread for the specified limelight
+            stopLimelightThread(limelight);
+          }
+        }
+        last_TX = current_TX;
+        last_TY = current_TY;
+      } catch (Exception e) {
+        System.err.println(
+            "Error communicating with the: " + limelight.getName() + ": " + e.getMessage());
+      }
+    }
+  }
+
+  /**
+   * Starts a separate thread dedicated to updating the pose estimate for a specified limelight.
+   * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the
+   * JSON dump for each limelight sequentially within a single scheduler loop.
+   *
+   * <p>To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage
+   * the lifecycle of these threads.
+   *
+   * <p>Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the
+   * corresponding limelight's thread is marked as "running". This ensures that pose estimates are
+   * updated in real-time, leveraging the parallel processing capabilities of the executor service.
+   *
+   * @param limelight the limelight number
+   */
+  public void visionThreadTask(VisionInputs inputs) { // Limelight limelight
+    try {
+      synchronized (inputs) {
+        for (Limelight limelight : Limelight.values()) {
+          checkAndUpdatePose(limelight, inputs);
+        }
+      }
+
+    } catch (Exception e) {
+      e.printStackTrace();
+    }
+  }
+
+  /**
+   * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread
+   * for the specified limelight.
+   *
+   * @param limelight the limelight number
+   */
+  public void stopLimelightThread(Limelight limelight) {
+    threadManager.stopThread(limelight.getName());
+  }
+
+  /** Shuts down all the threads. */
+  public void endAllThreads() {
+    threadManager.shutdownAllThreads();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java
new file mode 100644
index 0000000..3a82e3b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/SimulatedVision.java
@@ -0,0 +1,213 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.networktables.NetworkTable;
+import frc.robot.extras.vision.LimelightHelpers;
+import frc.robot.subsystems.vision.VisionConstants.Limelight;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+// Simulate the vision system.
+// Please see the following link for example code
+// https://github.com/PhotonVision/photonvision/blob/2a6fa1b6ac81f239c59d724da5339f608897c510/photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
+public class SimulatedVision extends PhysicalVision {
+  PhotonCameraSim shooterCameraSim;
+  PhotonCameraSim frontLeftCameraSim;
+  PhotonCameraSim frontRightCameraSim;
+  private final VisionSystemSim visionSim;
+  private final Supplier<Pose2d> robotSimulationPose;
+  private Pose2d lastSeenPose = new Pose2d();
+
+  private final int kResWidth = 1280;
+  private final int kResHeight = 800;
+
+  private int[] tagCount = new int[Limelight.values().length];
+  private double[] apriltagDist = new double[Limelight.values().length];
+
+  public SimulatedVision(Supplier<Pose2d> robotSimulationPose) {
+    super();
+    this.robotSimulationPose = robotSimulationPose;
+    // Create the vision system simulation which handles cameras and targets on the
+    // field.
+    visionSim = new VisionSystemSim("main");
+
+    // Add all the AprilTags inside the tag layout as visible targets to this
+    // simulated field.
+    visionSim.addAprilTags(VisionConstants.FIELD_LAYOUT);
+    // visionSim.addVisionTargets(TargetModel.kAprilTag36h11);
+
+    // Create simulated camera properties. These can be set to mimic your actual
+    // camera.
+    var cameraProperties = new SimCameraProperties();
+    cameraProperties.setCalibration(kResWidth, kResHeight, Rotation2d.fromDegrees(97.7));
+    cameraProperties.setCalibError(0.35, 0.10);
+    cameraProperties.setFPS(15);
+    cameraProperties.setAvgLatencyMs(20);
+    cameraProperties.setLatencyStdDevMs(5);
+
+    // Create a PhotonCameraSim which will update the linked PhotonCamera's values
+    // with visible
+    // targets.
+    // Instance variables
+    shooterCameraSim =
+        new PhotonCameraSim(getSimulationCamera(Limelight.SHOOTER), cameraProperties);
+    frontLeftCameraSim =
+        new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_LEFT), cameraProperties);
+    frontRightCameraSim =
+        new PhotonCameraSim(getSimulationCamera(Limelight.FRONT_RIGHT), cameraProperties);
+
+    visionSim.addCamera(
+        shooterCameraSim, VisionConstants.SHOOTER_TRANSFORM); // check inverse things
+    visionSim.addCamera(frontLeftCameraSim, VisionConstants.FRONT_LEFT_TRANSFORM);
+    visionSim.addCamera(frontRightCameraSim, VisionConstants.FRONT_RIGHT_TRANSFORM);
+
+    // Enable the raw and processed streams. (http://localhost:1181 / 1182)
+    // shooterCameraSim.enableRawStream(true);
+    // shooterCameraSim.enableProcessedStream(true);
+    // frontLeftCameraSim.enableRawStream(true);
+    // frontLeftCameraSim.enableProcessedStream(true);
+    // frontRightCameraSim.enableRawStream(true);
+    // frontRightCameraSim.enableProcessedStream(true);
+
+    // // Enable drawing a wireframe visualization of the field to the camera streams.
+    // // This is extremely resource-intensive and is disabled by default.
+    // shooterCameraSim.enableDrawWireframe(true);
+    // frontLeftCameraSim.enableDrawWireframe(true);
+    // frontRightCameraSim.enableDrawWireframe(true);
+  }
+
+  @Override
+  public void updateInputs(VisionInputs inputs) {
+    // Abuse the updateInputs periodic call to update the sim
+
+    // Move the vision sim robot on the field
+    if (robotSimulationPose.get() != null) {
+      visionSim.update(robotSimulationPose.get());
+      Logger.recordOutput("Vision/SimIO/updateSimPose", robotSimulationPose.get());
+    }
+    super.updateInputs(inputs);
+
+    for (Limelight limelight : Limelight.values()) {
+      writeToTable(
+          getSimulationCamera(limelight).getAllUnreadResults(),
+          getLimelightTable(limelight),
+          limelight);
+      // inputs.limelightTargets[limelight.getId()] = getNumberOfAprilTags(limelight);
+      // inputs.limelightAprilTagDistance[limelight.getId()] =
+      // getLimelightAprilTagDistance(limelight);
+    }
+  }
+
+  /**
+   * Writes photonvision results to the limelight network table
+   *
+   * @param results The simulated photonvision pose estimation results
+   * @param table The Limelight table to write to
+   * @param limelight The Limelight to write for
+   */
+  private void writeToTable(
+      List<PhotonPipelineResult> results, NetworkTable table, Limelight limelight) {
+    // write to ll table
+    for (PhotonPipelineResult result : results) {
+      if (result.getMultiTagResult().isPresent()) {
+        Transform3d best = result.getMultiTagResult().get().estimatedPose.best;
+        List<Double> pose_data =
+            new ArrayList<>(
+                Arrays.asList(
+                    best.getX(), // 0: X
+                    best.getY(), // 1: Y
+                    best.getZ(), // 2: Z,
+                    best.getRotation().getX(), // 3: roll
+                    best.getRotation().getY(), // 4: pitch
+                    best.getRotation().getZ(), // 5: yaw
+                    result.metadata.getLatencyMillis(), // 6: latency ms,
+                    (double)
+                        result.getMultiTagResult().get().fiducialIDsUsed.size(), // 7: tag count
+                    0.0, // 8: tag span
+                    0.0, // 9: tag dist
+                    result.getBestTarget().getArea() // 10: tag area
+                    ));
+        // Add RawFiducials
+        // This is super inefficient but it's sim only, who cares.
+        for (var target : result.targets) {
+          pose_data.add((double) target.getFiducialId()); // 0: id
+          pose_data.add(target.getYaw()); // 1: txnc
+          pose_data.add(target.getPitch()); // 2: tync
+          pose_data.add(0.0); // 3: ta
+          pose_data.add(0.0); // 4: distToCamera
+          pose_data.add(0.0); // 5: distToRobot
+          pose_data.add(0.5); // 6: ambiguity
+        }
+
+        table
+            .getEntry("botpose_wpiblue")
+            .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray());
+        table
+            .getEntry("botpose_orb_wpiblue")
+            .setDoubleArray(pose_data.stream().mapToDouble(Double::doubleValue).toArray());
+
+        table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0);
+        table.getEntry("cl").setDouble(result.metadata.getLatencyMillis());
+        lastSeenPose =
+            new Pose2d(
+                result.getMultiTagResult().get().estimatedPose.best.getTranslation().getX(),
+                result.getMultiTagResult().get().estimatedPose.best.getY(),
+                new Rotation2d(
+                    result.getMultiTagResult().get().estimatedPose.best.getRotation().getAngle()));
+      }
+    }
+  }
+
+  /**
+   * Gets the PhotonCamera for the given Limelight
+   *
+   * @param limelight The Limelight to get the camera for
+   * @return A PhotonCamera object for the given Limelight
+   */
+  private PhotonCamera getSimulationCamera(Limelight limelight) {
+    return switch (limelight) {
+      case SHOOTER -> VisionConstants.SHOOTER_CAMERA;
+      case FRONT_LEFT -> VisionConstants.FRONT_LEFT_CAMERA;
+      case FRONT_RIGHT -> VisionConstants.FRONT_RIGHT_CAMERA;
+      default -> throw new IllegalArgumentException("Invalid limelight camera " + limelight);
+    };
+  }
+
+  /**
+   * Gets the Limelight network table
+   *
+   * @param limelight The Limelight to get the table for
+   * @return The network table of the Limelight
+   */
+  private NetworkTable getLimelightTable(Limelight limelight) {
+    return switch (limelight) {
+      case SHOOTER -> LimelightHelpers.getLimelightNTTable(Limelight.SHOOTER.getName());
+      case FRONT_LEFT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_LEFT.getName());
+      case FRONT_RIGHT -> LimelightHelpers.getLimelightNTTable(Limelight.FRONT_RIGHT.getName());
+      default -> throw new IllegalArgumentException("Invalid limelight " + limelight);
+    };
+  }
+
+  // @Override
+  // public boolean canSeeAprilTags(Limelight limelight) {
+  //   table.getEntry("tv").setInteger(result.hasTargets() ? 1 : 0);
+
+  // }
+  @Override
+  public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
+    super.setHeadingInfo(headingDegrees, headingRateDegrees);
+  }
+
+  @Override
+  public Pose2d getLastSeenPose() {
+    return lastSeenPose;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/ThreadManager.java b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java
new file mode 100644
index 0000000..f0113e8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/ThreadManager.java
@@ -0,0 +1,80 @@
+package frc.robot.subsystems.vision;
+
+import frc.robot.subsystems.vision.VisionInterface.VisionInputs;
+import java.util.Map;
+import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.ExecutorService;
+import java.util.concurrent.Executors;
+import java.util.concurrent.Future;
+import java.util.concurrent.TimeUnit;
+
+public class ThreadManager {
+  private final ExecutorService executorService;
+  private final Map<String, Future<?>> threadFutures = new ConcurrentHashMap<>();
+
+  public ThreadManager(int maxThreads) {
+    executorService = Executors.newFixedThreadPool(maxThreads);
+  }
+
+  // Start a thread for a specific task
+  public void startThread(String threadName, Runnable task) {
+    if (threadFutures.containsKey(threadName)) {
+      System.err.println("Thread " + threadName + " is already running.");
+      return;
+    }
+
+    Future<?> future =
+        executorService.submit(
+            () -> {
+              try {
+                task.run();
+              } catch (Exception e) {
+                System.err.println("Error in thread " + threadName + ": " + e.getMessage());
+              }
+            });
+
+    threadFutures.put(threadName, future);
+  }
+
+  // Stop a specific thread
+  public void stopThread(String threadName) {
+    Future<?> future = threadFutures.get(threadName);
+    if (future != null) {
+      future.cancel(true);
+      threadFutures.remove(threadName);
+    } else {
+      System.err.println("Thread " + threadName + " is not running.");
+    }
+  }
+
+  // Submit a vision input task
+  public void startVisionInputTask(String threadName, VisionInputs inputs, Runnable visionTask) {
+    startThread(
+        threadName,
+        () -> {
+          try {
+            while (!Thread.currentThread().isInterrupted()) {
+              visionTask.run();
+              Thread.sleep(VisionConstants.THREAD_SLEEP_MS); // Sleep to avoid excessive updates
+            }
+          } catch (InterruptedException e) {
+            Thread.currentThread().interrupt();
+          }
+        });
+  }
+
+  // Shut down all threads
+  public void shutdownAllThreads() {
+    threadFutures.values().forEach(future -> future.cancel(true));
+    threadFutures.clear();
+    executorService.shutdown();
+    try {
+      if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) {
+        executorService.shutdownNow();
+      }
+    } catch (InterruptedException e) {
+      executorService.shutdownNow();
+      Thread.currentThread().interrupt();
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
deleted file mode 100644
index 72a4df2..0000000
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ /dev/null
@@ -1,54 +0,0 @@
-package frc.robot.subsystems.vision;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.littletonrobotics.junction.AutoLogOutput;
-import org.littletonrobotics.junction.Logger;
-
-public class Vision extends SubsystemBase {
-  private final VisionIO visionIO;
-  private final VisionIOInputsAutoLogged inputs = new VisionIOInputsAutoLogged();
-
-  public Vision(VisionIO visionIO) {
-    // Initializing Fields
-    this.visionIO = visionIO;
-  }
-
-  @Override
-  public void periodic() {
-    // Updates limelight inputs
-    visionIO.updateInputs(inputs);
-    Logger.processInputs(visionIO.getLimelightName(0), inputs);
-  }
-
-  // Add methods to support DriveCommand
-  public int getNumberOfAprilTags(int limelightNumber) {
-    return visionIO.getNumberOfAprilTags(limelightNumber);
-  }
-
-  public double getLimelightAprilTagDistance(int limelightNumber) {
-    return visionIO.getLimelightAprilTagDistance(limelightNumber);
-  }
-
-  public double getTimeStampSeconds(int limelightNumber) {
-    return visionIO.getTimeStampSeconds(limelightNumber);
-  }
-
-  public double getLatencySeconds(int limelightNumber) {
-    return visionIO.getLatencySeconds(limelightNumber);
-  }
-
-  public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
-    visionIO.setHeadingInfo(headingDegrees, headingRateDegrees);
-  }
-
-  @AutoLogOutput(key = "Vision/Has Targets")
-  public boolean canSeeAprilTags(int limelightNumber) {
-    return visionIO.canSeeAprilTags(
-        limelightNumber); // Assuming we're checking the shooter limelight
-  }
-
-  public Pose2d getPoseFromAprilTags(int limelightNumber) {
-    return visionIO.getPoseFromAprilTags(limelightNumber);
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
index ac54ce9..b4ceef6 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java
@@ -1,9 +1,66 @@
 package frc.robot.subsystems.vision;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import org.photonvision.PhotonCamera;
 
 public final class VisionConstants {
+  public enum Limelight {
+    SHOOTER(0, VisionConstants.SHOOTER_LIMELIGHT_NAME),
+    FRONT_LEFT(1, VisionConstants.FRONT_LEFT_LIMELIGHT_NAME),
+    FRONT_RIGHT(2, VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME);
 
+    private final int id;
+    private final String name;
+
+    Limelight(int id, String name) {
+      this.id = id;
+      this.name = name;
+    }
+
+    public int getId() {
+      return id;
+    }
+
+    public String getName() {
+      return name;
+    }
+
+    public static Limelight fromId(int id) {
+      return switch (id) {
+        case 0 -> SHOOTER;
+        case 1 -> FRONT_LEFT;
+        case 2 -> FRONT_RIGHT;
+        default -> throw new IllegalArgumentException("Invalid Limelight ID: " + id);
+      };
+    }
+  }
+
+  public static final Transform3d SHOOTER_TRANSFORM =
+      new Transform3d(
+          new Translation3d(-0.3119324724, 0.0, 0.1865472012), new Rotation3d(0.0, 35, 180.0));
+  public static final Transform3d FRONT_LEFT_TRANSFORM =
+      new Transform3d(
+          new Translation3d(0.2749477356, -0.269958439, 0.2318054546),
+          new Rotation3d(0.0, 25, -35));
+  public static final Transform3d FRONT_RIGHT_TRANSFORM =
+      new Transform3d(
+          new Translation3d(0.2816630892, 0.2724405524, 0.232156), new Rotation3d(0.0, 25, 35));
+
+  public static final PhotonCamera SHOOTER_CAMERA = new PhotonCamera(Limelight.SHOOTER.getName());
+  public static final PhotonCamera FRONT_LEFT_CAMERA =
+      new PhotonCamera(Limelight.FRONT_LEFT.getName());
+  public static final PhotonCamera FRONT_RIGHT_CAMERA =
+      new PhotonCamera(Limelight.FRONT_RIGHT.getName());
+
+  public static final int THREAD_SLEEP_MS = 20;
+
+  public static final AprilTagFieldLayout FIELD_LAYOUT =
+      AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
   public static final double VISION_X_POS_TRUST = 0.5; // meters
   public static final double VISION_Y_POS_TRUST = 0.5; // meters
   public static final double VISION_ANGLE_TRUST = Units.degreesToRadians(50); // radians
@@ -17,6 +74,9 @@ public final class VisionConstants {
 
   public static final double MEGA_TAG_2_DISTANCE_THRESHOLD = 5; // TODO: Tune
 
+  public static final double MEGA_TAG_TRANSLATION_DISCREPANCY_THRESHOLD = 0.5; // TODO: tune
+  public static final double MEGA_TAG_ROTATION_DISCREPANCY_THREASHOLD = 45;
+
   public static final String SHOOTER_LIMELIGHT_NAME = "limelight-shooter";
   public static final int SHOOTER_LIMELIGHT_NUMBER = 0;
   public static final String FRONT_LEFT_LIMELIGHT_NAME = "limelight-left";
@@ -24,53 +84,8 @@ public final class VisionConstants {
   public static final String FRONT_RIGHT_LIMELIGHT_NAME = "limelight-right";
   public static final int FRONT_RIGHT_LIMELIGHT_NUMBER = 2;
 
-  public static final double[][] APRIL_TAG_POSITIONS = {
-    // {x, y, z, rotation (degrees)}
-    {
-      Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 120
-    }, // 1
-    {
-      Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 120
-    }, // 2
-    {
-      Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 180
-    }, // 3
-    {
-      Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 180
-    }, // 4
-    {
-      Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270
-    }, // 5
-    {
-      Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38), 270
-    }, // 6
-    {-Units.inchesToMeters(1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13), 0}, // 7
-    {-Units.inchesToMeters(1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13), 0}, // 8
-    {
-      Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38), 60
-    }, // 9
-    {
-      Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38), 60
-    }, // 10
-    {
-      Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 300
-    }, // 11
-    {
-      Units.inchesToMeters(468.69), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 60
-    }, // 12
-    {
-      Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 180
-    }, // 13
-    {
-      Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0), 0
-    }, // 14
-    {
-      Units.inchesToMeters(182.73), Units.inchesToMeters(177.1), Units.inchesToMeters(52.0), 120
-    }, // 15
-    {
-      Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0), 240
-    }, // 16
-  };
+  public static final int MIN_APRIL_TAG_ID = 1;
+  public static final int MAX_APRIL_TAG_ID = 16;
 
   public static final double[][] ONE_APRIL_TAG_LOOKUP_TABLE = {
     // {distance in meters, x std deviation, y std deviation, r (in degrees) std deviation}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
deleted file mode 100644
index ad9c125..0000000
--- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package frc.robot.subsystems.vision;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import org.littletonrobotics.junction.AutoLog;
-
-public interface VisionIO {
-  @AutoLog
-  class VisionIOInputs {
-    public boolean cameraConnected = false;
-    public double latency = 0.0;
-    public double fiducialMarksID = 0.0;
-
-    public int camerasAmount = 0;
-    public int targetsCount = 0;
-  }
-
-  void updateInputs(VisionIOInputs inputs);
-
-  String getLimelightName(int limelightNumber);
-
-  double getLatencySeconds(int limelightNumber);
-
-  double getTimeStampSeconds(int limelightNumber);
-
-  boolean canSeeAprilTags(int limelightNumber);
-
-  double getLimelightAprilTagDistance(int limelightNumber);
-
-  int getNumberOfAprilTags(int limelightNumber);
-
-  Pose2d getPoseFromAprilTags(int limelightNumber);
-
-  void setHeadingInfo(double headingDegrees, double headingRateDegrees);
-}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java b/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
deleted file mode 100644
index aa4405d..0000000
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOReal.java
+++ /dev/null
@@ -1,439 +0,0 @@
-package frc.robot.subsystems.vision;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Constants.FieldConstants;
-import frc.robot.extras.vision.LimelightHelpers;
-import frc.robot.extras.vision.LimelightHelpers.PoseEstimate;
-import java.util.Map;
-import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.ExecutorService;
-import java.util.concurrent.Executors;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-public class VisionIOReal implements VisionIO {
-
-  private Pose2d lastSeenPose = new Pose2d();
-  private double headingDegrees = 0;
-  private double headingRateDegreesPerSecond = 0;
-  private final Map<Integer, AtomicBoolean> limelightThreads = new ConcurrentHashMap<>();
-  private final ExecutorService executorService = Executors.newFixedThreadPool(3);
-
-  /**
-   * The pose estimates from the limelights in the following order {shooterLimelight,
-   * frontLeftLimelight, frontRightLimelight}
-   */
-  private PoseEstimate[] limelightEstimates;
-
-  public VisionIOReal() {
-    limelightEstimates = new PoseEstimate[3];
-    for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) {
-      limelightThreads.put(limelightNumber, new AtomicBoolean(true));
-      limelightEstimates[limelightNumber] = new PoseEstimate();
-    }
-  }
-
-  @Override
-  public void updateInputs(VisionIOInputs inputs) {
-    inputs.camerasAmount = limelightEstimates.length;
-    inputs.cameraConnected = true;
-
-    for (int limelightNumber = 0; limelightNumber < limelightEstimates.length; limelightNumber++) {
-      // Update camera connection status
-      inputs.cameraConnected = true;
-
-      // Add number of April tags seen by this limelight
-      inputs.targetsCount += getNumberOfAprilTags(limelightNumber);
-
-      // Add fiducial mark ID
-      inputs.fiducialMarksID = LimelightHelpers.getFiducialID(getLimelightName(limelightNumber));
-
-      // Add latency for this limelight
-      inputs.latency += getLatencySeconds(limelightNumber) / 1000.0;
-    }
-
-    // Calculate average latency
-    inputs.latency /= limelightEstimates.length;
-
-    periodic();
-  }
-
-  /**
-   * Checks if the specified limelight can fully see one or more April Tag.
-   *
-   * @param limelightNumber the number of the limelight
-   * @return true if the limelight can fully see one or more April Tag
-   */
-  @Override
-  public boolean canSeeAprilTags(int limelightNumber) {
-    // First checks if it can see an april tag, then checks if it is fully in frame
-    // Different Limelights have different FOVs
-    if (getNumberOfAprilTags(limelightNumber) > 0
-        && getNumberOfAprilTags(limelightNumber) <= VisionConstants.APRIL_TAG_POSITIONS.length) {
-      if (getLimelightName(limelightNumber).equals(VisionConstants.SHOOTER_LIMELIGHT_NAME)) {
-        return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber)))
-            <= VisionConstants.LL3G_FOV_MARGIN_OF_ERROR;
-      } else {
-        return Math.abs(LimelightHelpers.getTX(getLimelightName(limelightNumber)))
-            <= VisionConstants.LL3_FOV_MARGIN_OF_ERROR;
-      }
-    }
-    return false;
-  }
-
-  /**
-   * Gets the JSON dump from the specified limelight and puts it into a PoseEstimate object, which
-   * is then placed into its corresponding spot in the limelightEstimates array.
-   *
-   * @param limelightNumber the number of the limelight
-   */
-  public PoseEstimate enabledPoseUpdate(int limelightNumber) {
-    if (canSeeAprilTags(limelightNumber) && isValidPoseEstimate(limelightNumber)) {
-      if (isLargeDiscrepancyBetweenMegaTag1And2(limelightNumber)
-          && getLimelightAprilTagDistance(limelightNumber)
-              < VisionConstants.MEGA_TAG_2_DISTANCE_THRESHOLD) {
-        return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
-      } else if (headingRateDegreesPerSecond < VisionConstants.MEGA_TAG_2_MAX_HEADING_RATE) {
-        LimelightHelpers.SetRobotOrientation(
-            getLimelightName(limelightNumber), headingDegrees, 0, 0, 0, 0, 0);
-        return limelightEstimates[limelightNumber] = getMegaTag2PoseEstimate(limelightNumber);
-      } else {
-        return limelightEstimates[limelightNumber] = getMegaTag1PoseEstimate(limelightNumber);
-      }
-    }
-    return limelightEstimates[limelightNumber] = new PoseEstimate();
-  }
-
-  /**
-   * If the robot is not enabled, update the pose using MegaTag1 and after it is enabled, run {@link
-   * #enabledPoseUpdate(int)}
-   *
-   * @param limelightNumber the number of the limelight
-   */
-  public void updatePoseEstimate(int limelightNumber) {
-    limelightEstimates[limelightNumber] =
-        DriverStation.isEnabled()
-            ? enabledPoseUpdate(limelightNumber)
-            : getMegaTag1PoseEstimate(limelightNumber);
-  }
-
-  /**
-   * Checks if there is a large discrepancy between the MegaTag1 and MegaTag2 estimates.
-   *
-   * @param limelightNumber the number of the limelight
-   * @return true if the discrepancy is larger than the defined threshold, false otherwise
-   */
-  public boolean isLargeDiscrepancyBetweenMegaTag1And2(int limelightNumber) {
-    PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber);
-    PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);
-
-    // Extract the positions of the two poses
-    Translation2d megaTag1TranslationMeters = megaTag1Estimate.pose.getTranslation();
-    Translation2d megaTag2TranslationMeters = megaTag2Estimate.pose.getTranslation();
-
-    double megaTag1RotationDegrees = megaTag1Estimate.pose.getRotation().getDegrees();
-    double megaTag2RotationDegrees = megaTag2Estimate.pose.getRotation().getDegrees();
-
-    // Calculate the discrepancy between the two MegaTag translations in meters
-    double megaTagTranslationDiscrepancyMeters =
-        megaTag1TranslationMeters.getDistance(megaTag2TranslationMeters);
-    double megaTagRotationDiscrepancyDegrees =
-        Math.abs(megaTag1RotationDegrees - megaTag2RotationDegrees);
-
-    // Define a threshold (meters) for what constitutes a "large" discrepancy
-    // This value should be determined based on your testing
-    double thresholdMeters = 0.5;
-    double thresholdDegrees = 45;
-
-    // Check if the discrepancy is larger than the threshold (meters)
-    return megaTagTranslationDiscrepancyMeters > thresholdMeters
-        || megaTagRotationDiscrepancyDegrees > thresholdDegrees;
-  }
-
-  /**
-   * Gets the MegaTag1 pose of the robot calculated by specified limelight via any April Tags it
-   * sees
-   *
-   * @param limelightNumber the number of the limelight
-   * @return the MegaTag1 pose of the robot, if the limelight can't see any April Tags, it will
-   *     return 0 for x, y, and theta
-   */
-  public PoseEstimate getMegaTag1PoseEstimate(int limelightNumber) {
-    return LimelightHelpers.getBotPoseEstimate_wpiBlue(getLimelightName(limelightNumber));
-  }
-
-  /**
-   * Gets the MegaTag2 pose of the robot calculated by specified limelight via any April Tags it
-   * sees
-   *
-   * @param limelightNumber the number of the limelight
-   * @return the MegaTag2 pose of the robot, if the limelight can't see any April Tags, it will
-   *     return 0 for x, y, and theta
-   */
-  public PoseEstimate getMegaTag2PoseEstimate(int limelightNumber) {
-    return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(getLimelightName(limelightNumber));
-  }
-
-  /**
-   * Checks if the MT1 and MT2 pose estimate exists and whether it is within the field
-   *
-   * @param limelightNumber the number of the limelight
-   * @return true if the pose estimate exists within the field and the pose estimate is not null
-   */
-  public boolean isValidPoseEstimate(int limelightNumber) {
-    PoseEstimate megaTag1Estimate = getMegaTag1PoseEstimate(limelightNumber);
-    PoseEstimate megaTag2Estimate = getMegaTag2PoseEstimate(limelightNumber);
-
-    return LimelightHelpers.isValidPoseEstimate(megaTag1Estimate)
-        && LimelightHelpers.isValidPoseEstimate(megaTag2Estimate)
-        && isWithinFieldBounds(megaTag1Estimate, megaTag2Estimate);
-  }
-
-  /**
-   * Checks whether the pose estimate for MT1 and MT2 is within the field
-   *
-   * @param megaTag1Estimate the MT1 pose estimate to check
-   * @param megaTag2Estimate the MT2 pose estimate to check
-   */
-  private boolean isWithinFieldBounds(
-      PoseEstimate megaTag1Estimate, PoseEstimate megaTag2Estimate) {
-    return (megaTag1Estimate.pose.getX() > 0
-            && megaTag1Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
-        && (megaTag1Estimate.pose.getY() > 0
-            && megaTag1Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS)
-        && (megaTag2Estimate.pose.getX() > 0
-            && megaTag2Estimate.pose.getX() <= FieldConstants.FIELD_WIDTH_METERS)
-        && (megaTag2Estimate.pose.getY() > 0
-            && megaTag2Estimate.pose.getY() <= FieldConstants.FIELD_WIDTH_METERS);
-  }
-
-  /**
-   * Gets the pose of the robot calculated by specified limelight via any April Tags it sees
-   *
-   * @param limelightNumber the number of the limelight
-   * @return the pose of the robot, if the limelight can't see any April Tags, it will return 0 for
-   *     x, y, and theta
-   */
-  @Override
-  public Pose2d getPoseFromAprilTags(int limelightNumber) {
-    return limelightEstimates[limelightNumber].pose;
-  }
-
-  /** Returns how many april tags the limelight that is being used for pose estimation can see. */
-  @Override
-  public int getNumberOfAprilTags(int limelightNumber) {
-    return limelightEstimates[limelightNumber].tagCount;
-  }
-
-  /**
-   * Returns the timestamp for the vision measurement from the limelight that is being used for pose
-   * estimation.
-   */
-  @Override
-  public double getTimeStampSeconds(int limelightNumber) {
-    return limelightEstimates[limelightNumber].timestampSeconds / 1000.0;
-  }
-
-  /**
-   * Returns the latency in seconds of when the limelight that is being used for pose estimation
-   * calculated the robot's pose. It adds the pipeline latency, capture latency, and json parsing
-   * latency.
-   */
-  @Override
-  public double getLatencySeconds(int limelightNumber) {
-    return (limelightEstimates[limelightNumber].latency) / 1000.0;
-  }
-
-  /** Gets the pose calculated the last time a limelight saw an April Tag */
-  public Pose2d getLastSeenPose() {
-    return lastSeenPose;
-  }
-
-  /**
-   * Gets the average distance between the specified limelight and the April Tags it sees
-   *
-   * @param limelightNumber the number of the limelight
-   * @return the average distance between the robot and the April Tag(s) in meters
-   */
-  public double getLimelightAprilTagDistance(int limelightNumber) {
-    if (canSeeAprilTags(limelightNumber)) {
-      return limelightEstimates[limelightNumber].avgTagDist;
-    }
-    // To be safe returns a big distance from the april tags if it can't see any
-    return Double.MAX_VALUE;
-  }
-
-  /**
-   * Sets the heading and heading rate of the robot, this is used for deciding between MegaTag 1 and
-   * 2 for pose estimation.
-   *
-   * @param headingDegrees the angle the robot is facing in degrees (0 degrees facing the red
-   *     alliance)
-   * @param headingRateDegrees the rate the robot is rotating, CCW positive
-   */
-  public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
-    this.headingDegrees = headingDegrees;
-    this.headingRateDegreesPerSecond = headingRateDegrees;
-  }
-
-  /**
-   * Gets the limelight name associated with the specified limelight number/index
-   *
-   * @param limelightNumber the limelight number
-   * @return 0 = limelight-shooter, 1 = limelight-left, 2 = limelight-right
-   */
-  public String getLimelightName(int limelightNumber) {
-    return switch (limelightNumber) {
-      case 0 -> VisionConstants.SHOOTER_LIMELIGHT_NAME;
-      case 1 -> VisionConstants.FRONT_LEFT_LIMELIGHT_NAME;
-      case 2 -> VisionConstants.FRONT_RIGHT_LIMELIGHT_NAME;
-      default ->
-          throw new IllegalArgumentException("You entered a number for a non-existent limelight");
-    };
-  }
-
-  /**
-   * This checks is there is new pose detected by a limelight, and if so, updates the pose estimate
-   *
-   * @param limelightNumber the limelight number
-   */
-  public void checkAndUpdatePose(int limelightNumber) {
-    double last_TX = 0;
-    double last_TY = 0;
-
-    // Syncronization block to ensure thread safety during the critical section where pose
-    // information is read and compared.
-    // This helps prevent race conditions, where one limelight may be updating an object that
-    // another limelight is reading.
-    // A race condition could cause unpredictable things to happen. Such as causing a limelight to
-    // be unable to reference an
-    // object, as its reference was modified earlier.
-    synchronized (this) {
-      try {
-        double current_TX = LimelightHelpers.getTX(getLimelightName(limelightNumber));
-        double current_TY = LimelightHelpers.getTY(getLimelightName(limelightNumber));
-
-        // This checks if the limelight reading is new. The reasoning being that if the TX and TY
-        // are EXACTLY the same, it hasn't updated yet with a new reading. We are doing it this way,
-        // because to get the timestamp of the reading, you need to parse the JSON dump which can be
-        // very demanding whereas this only has to get the Network Table entries for TX and TY.
-        if (current_TX != last_TX || current_TY != last_TY) {
-          updatePoseEstimate(limelightNumber);
-          limelightThreads.computeIfPresent(
-              limelightNumber, (key, value) -> new AtomicBoolean(true));
-          // This is to keep track of the last valid pose calculated by the limelights
-          // it is used when the driver resets the robot odometry to the limelight calculated
-          // position
-          if (canSeeAprilTags(limelightNumber)) {
-            lastSeenPose = getMegaTag1PoseEstimate(limelightNumber).pose;
-          }
-        } else {
-          // Retrieve the AtomicBoolean for the given limelight number
-          AtomicBoolean isThreadRunning =
-              limelightThreads.getOrDefault(limelightNumber, new AtomicBoolean());
-          // Only stop the thread if it's currently running
-          if (isThreadRunning.get()) {
-            // stop the thread for the specified limelight
-            stopThread(limelightNumber);
-          }
-        }
-        last_TX = current_TX;
-        last_TY = current_TY;
-      } catch (Exception e) {
-        System.err.println(
-            "Error communicating with the: "
-                + getLimelightName(limelightNumber)
-                + ": "
-                + e.getMessage());
-      }
-    }
-  }
-
-  /**
-   * Starts a separate thread dedicated to updating the pose estimate for a specified limelight.
-   * This approach is adopted to prevent loop overruns that would occur if we attempted to parse the
-   * JSON dump for each limelight sequentially within a single scheduler loop.
-   *
-   * <p>To achieve efficient and safe parallel execution, an ExecutorService is utilized to manage
-   * the lifecycle of these threads.
-   *
-   * <p>Each thread continuously runs the {@link #checkAndUpdatePose(int)} method as long as the
-   * corresponding limelight's thread is marked as "running". This ensures that pose estimates are
-   * updated in real-time, leveraging the parallel processing capabilities of the executor service.
-   *
-   * @param limelightNumber the limelight number
-   */
-  public void visionThread(int limelightNumber) {
-
-    executorService.submit(
-        () -> {
-          try {
-            // while (limelightThreads.get(limelightNumber).get()) {
-            checkAndUpdatePose(limelightNumber);
-            // }
-          } catch (Exception e) {
-            System.err.println(
-                "Error executing task for the: "
-                    + getLimelightName(limelightNumber)
-                    + ": "
-                    + e.getMessage());
-          }
-        });
-  }
-
-  /**
-   * Sets the AtomicBoolean 'runningThreads' to false for the specified limelight. Stops the thread
-   * for the specified limelight.
-   *
-   * @param limelightNumber the limelight number
-   */
-  public void stopThread(int limelightNumber) {
-    try {
-      // Since we can't see an April Tag, set the estimate for the specified limelight to an empty
-      // PoseEstimate()
-      limelightEstimates[limelightNumber] = new PoseEstimate();
-      limelightThreads.get(limelightNumber).set(false);
-    } catch (Exception e) {
-      System.err.println(
-          "Error stopping thread for the: "
-              + getLimelightName(limelightNumber)
-              + ": "
-              + e.getMessage());
-    }
-  }
-
-  /** Shuts down all the threads. */
-  public void endAllThreads() {
-    // Properly shut down the executor service when the subsystem ends
-    executorService.shutdown(); // Prevents new tasks from being submitted
-    try {
-      // Wait for existing tasks to finish
-      if (!executorService.awaitTermination(5, TimeUnit.SECONDS)) {
-        executorService.shutdownNow();
-        // Wait a bit longer for tasks to respond to being cancelled
-        if (!executorService.awaitTermination(5, TimeUnit.SECONDS))
-          System.err.println("ExecutorService did not terminate");
-      }
-    } catch (InterruptedException e) {
-      // (Re-)Cancel if current thread also interrupted
-      executorService.shutdownNow();
-      // Preserve interrupt status
-      Thread.currentThread().interrupt();
-    }
-  }
-
-  // Override periodic method to start the vision threads at the beginning of each subsystem tick
-  public void periodic() {
-    visionThread(VisionConstants.SHOOTER_LIMELIGHT_NUMBER);
-    visionThread(VisionConstants.FRONT_LEFT_LIMELIGHT_NUMBER);
-    visionThread(VisionConstants.FRONT_RIGHT_LIMELIGHT_NUMBER);
-    SmartDashboard.putNumber("april tag dist", getLimelightAprilTagDistance(0));
-    SmartDashboard.putString("shooter ll odom", getPoseFromAprilTags(0).toString());
-    SmartDashboard.putString("left ll odom", getPoseFromAprilTags(1).toString());
-
-    SmartDashboard.putString("right ll odom", getPoseFromAprilTags(2).toString());
-  }
-}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java
deleted file mode 100644
index 343d4b4..0000000
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOSim.java
+++ /dev/null
@@ -1,44 +0,0 @@
-// package frc.robot.subsystems.vision;
-
-// import edu.wpi.first.apriltag.AprilTagFieldLayout;
-// import edu.wpi.first.math.geometry.Pose2d;
-// import org.photonvision.simulation.PhotonCameraSim;
-// import org.photonvision.simulation.VisionSystemSim;
-
-// import java.util.List;
-// import java.util.function.Supplier;
-
-// public class VisionIOSim extends VisionIOPhoton {
-//     private final VisionSystemSim visionSystemSim;
-//     private final PhotonCameraSim[] camerasSim;
-//     private final Supplier<Pose2d> robotActualPoseInSimulationSupplier;
-//     public VisionIOSim(List<PhotonCameraProperties> cameraProperties, AprilTagFieldLayout
-// aprilTagFieldLayout, Supplier<Pose2d> robotActualPoseInSimulationSupplier) {
-//         super(cameraProperties);
-
-//         this.robotActualPoseInSimulationSupplier = robotActualPoseInSimulationSupplier;
-//         this.visionSystemSim = new VisionSystemSim("main");
-//         visionSystemSim.addAprilTags(aprilTagFieldLayout);
-//         camerasSim = new PhotonCameraSim[cameraProperties.size()];
-
-//         for (int i = 0; i < cameraProperties.size(); i++) {
-//             final PhotonCameraSim cameraSim = new PhotonCameraSim(
-//                     super.cameras[i],
-//                     cameraProperties.get(i).getSimulationProperties()
-//             );
-//             cameraSim.enableRawStream(true);
-//             cameraSim.enableProcessedStream(true);
-//             cameraSim.enableDrawWireframe(true);
-//             visionSystemSim.addCamera(
-//                     camerasSim[i] = cameraSim,
-//                     cameraProperties.get(i).robotToCamera
-//             );
-//         }
-//     }
-
-//     @Override
-//     public void updateInputs(VisionIOInputs inputs) {
-//         visionSystemSim.update(robotActualPoseInSimulationSupplier.get());
-//         super.updateInputs(inputs);
-//     }
-// }
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionInterface.java b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java
new file mode 100644
index 0000000..08c77cb
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/VisionInterface.java
@@ -0,0 +1,95 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.extras.vision.MegatagPoseEstimate;
+import frc.robot.subsystems.vision.VisionConstants.Limelight;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface VisionInterface {
+  @AutoLog
+  class VisionInputs {
+    public boolean[] isLimelightConnected = new boolean[Limelight.values().length];
+
+    public MegatagPoseEstimate[] limelightMegatagPoses =
+        new MegatagPoseEstimate[Limelight.values().length];
+    public double[] limelightLatencies = new double[Limelight.values().length];
+    public int[] limelightTargets = new int[Limelight.values().length];
+    public boolean[] limelightSeesAprilTags = new boolean[Limelight.values().length];
+
+    public Pose2d[] limelightCalculatedPoses = new Pose2d[Limelight.values().length];
+    public Pose2d limelightLastSeenPose = new Pose2d();
+    public double[] limelightAprilTagDistances = new double[Limelight.values().length];
+
+    public double[] limelightTimestamps = new double[Limelight.values().length];
+  }
+
+  /**
+   * Updates the inputs for the vision subsystem using VisionInputs
+   *
+   * @param inputs the inputs to update
+   */
+  default void updateInputs(VisionInputs inputs) {}
+
+  /**
+   * @param limelight The Limelight to retrieve the latency data from
+   * @return The current latency of the Limelight
+   */
+  default double getLatencySeconds(Limelight limelight) {
+    return 0.0;
+  }
+
+  /**
+   * @param limelight The Limelight to retrieve the timestamp data from
+   * @return The current timestamp of the Limelight
+   */
+  default double getTimeStampSeconds(Limelight limelight) {
+    return 0.0;
+  }
+
+  /**
+   * @param limelight The Limelight to retrieve the target data from
+   * @return Whether the Limelight can see any targets, and if it is within its field of view
+   */
+  default boolean canSeeAprilTags(Limelight limelight) {
+    return false;
+  }
+
+  /**
+   * @param limelight The Limelight to retrieve the distance data from
+   * @return The current April Tag distance from the Limelight
+   */
+  default double getLimelightAprilTagDistance(Limelight limelight) {
+    return 0.0;
+  }
+
+  /**
+   * @param limelight The Limelight to retrieve the data from
+   * @return The current number of April Tags of the Limelight
+   */
+  default int getNumberOfAprilTags(Limelight limelight) {
+    return 0;
+  }
+
+  /**
+   * @param limelight The Limelight to retrieve the pose data from
+   * @return The current pose of the Limelight
+   */
+  default Pose2d getPoseFromAprilTags(Limelight limelight) {
+    return null;
+  }
+
+  /**
+   * Sets the heading information of the robot, used with MT2
+   *
+   * @param headingDegrees The heading of the robot in degrees
+   * @param headingRateDegrees The rate of change of the heading of the robot in degrees
+   */
+  default void setHeadingInfo(double headingDegrees, double headingRateDegrees) {}
+
+  /**
+   * @return The last seen pose of any Limelight that most recently saw a target
+   */
+  default Pose2d getLastSeenPose() {
+    return null;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
new file mode 100644
index 0000000..28b95d4
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -0,0 +1,60 @@
+// All praise 254 lib
+
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.vision.VisionConstants.Limelight;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class VisionSubsystem extends SubsystemBase {
+  private final VisionInterface visionInterface;
+  private final VisionInputsAutoLogged inputs = new VisionInputsAutoLogged();
+
+  public VisionSubsystem(VisionInterface visionInterface) {
+    // Initializing Fields
+    this.visionInterface = visionInterface;
+  }
+
+  @Override
+  public void periodic() {
+    // Updates limelight inputs
+    visionInterface.updateInputs(inputs);
+    Logger.processInputs("Vision/", inputs);
+  }
+
+  // Add methods to support DriveCommand
+  public int getNumberOfAprilTags(Limelight limelight) {
+    return inputs.limelightTargets[limelight.getId()];
+  }
+
+  public double getLimelightAprilTagDistance(Limelight limelight) {
+    return inputs.limelightAprilTagDistances[limelight.getId()];
+  }
+
+  public double getTimeStampSeconds(Limelight limelight) {
+    return inputs.limelightTimestamps[limelight.getId()];
+  }
+
+  public double getLatencySeconds(Limelight limelight) {
+    return inputs.limelightLatencies[limelight.getId()];
+  }
+
+  public void setHeadingInfo(double headingDegrees, double headingRateDegrees) {
+    visionInterface.setHeadingInfo(headingDegrees, headingRateDegrees);
+  }
+
+  @AutoLogOutput(key = "Vision/Has Targets")
+  public boolean canSeeAprilTags(Limelight limelight) {
+    return inputs.limelightSeesAprilTags[limelight.getId()];
+  }
+
+  public Pose2d getPoseFromAprilTags(Limelight limelight) {
+    return inputs.limelightCalculatedPoses[limelight.getId()];
+  }
+
+  public Pose2d getLastSeenPose() {
+    return visionInterface.getLastSeenPose();
+  }
+}
diff --git a/src/test/java/SwerveDriveTest.java b/src/test/java/SwerveDriveTest.java
deleted file mode 100644
index 718fa7f..0000000
--- a/src/test/java/SwerveDriveTest.java
+++ /dev/null
@@ -1,83 +0,0 @@
-import static org.junit.jupiter.api.Assertions.*;
-import static org.mockito.ArgumentMatchers.any;
-import static org.mockito.ArgumentMatchers.eq;
-import static org.mockito.Mockito.*;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import frc.robot.subsystems.swerve.*;
-import frc.robot.subsystems.swerve.gyroIO.GyroInterface;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
-import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.mockito.Mock;
-import org.mockito.MockitoAnnotations;
-
-class SwerveDriveTest {
-
-  @Mock private GyroInterface gyroIO;
-
-  @Mock
-  private ModuleInterface frontLeftModuleIO,
-      frontRightModuleIO,
-      backLeftModuleIO,
-      backRightModuleIO;
-  @Mock private SwerveModule frontLeftModule, frontRightModule, backLeftModule, backRightModule;
-
-  @Mock private OdometryThread mockOdometryThread;
-
-  @Mock private SwerveDriveKinematics swerveDriveKinematics;
-
-  private SwerveDrive swerveDrive;
-
-  @BeforeEach
-  void setUp() {
-    // Initialize the mocks
-    MockitoAnnotations.openMocks(this);
-
-    swerveDrive =
-        spy(
-            new SwerveDrive(
-                gyroIO,
-                frontLeftModuleIO,
-                frontRightModuleIO,
-                backLeftModuleIO,
-                backRightModuleIO));
-
-    swerveDrive.setKinematics(swerveDriveKinematics);
-  }
-
-  @Test
-  void testDrive() {
-    // Prepare mock module states
-    SwerveModuleState[] mockStates = new SwerveModuleState[4];
-    for (int i = 0; i < 4; i++) {
-      mockStates[i] = mock(SwerveModuleState.class); // Mock each SwerveModuleState
-    }
-
-    // Mock the toSwerveModuleStates method to return mock states
-    when(swerveDriveKinematics.toSwerveModuleStates(any(ChassisSpeeds.class)))
-        .thenReturn(mockStates);
-
-    // Now when the drive method calls toSwerveModuleStates, it will return mockStates
-    swerveDrive.drive(1.0, 1.0, 0.5, true);
-
-    // Verify that setModuleStates was called with mockStates
-    verify(swerveDrive).setModuleStates(mockStates);
-    verify(swerveDrive.getKinematics()).toSwerveModuleStates(any(ChassisSpeeds.class));
-  }
-
-  @Test
-  void testSetPose() {
-    // Mock pose setter
-    Pose2d newPose = new Pose2d(3.0, 4.0, Rotation2d.fromDegrees(90));
-    swerveDrive.setPose(newPose);
-
-    // Verify that the setPose method was called with the new pose
-    verify(swerveDrive).setPose(eq(newPose));
-  }
-}
diff --git a/src/test/java/SwerveModuleTest.java b/src/test/java/SwerveModuleTest.java
deleted file mode 100644
index 08535c9..0000000
--- a/src/test/java/SwerveModuleTest.java
+++ /dev/null
@@ -1,39 +0,0 @@
-import static edu.wpi.first.units.Units.*;
-import static org.junit.jupiter.api.Assertions.*;
-import static org.mockito.Mockito.*;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
-import frc.robot.subsystems.swerve.*;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInputsAutoLogged;
-import frc.robot.subsystems.swerve.moduleIO.ModuleInterface;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.mockito.Mock;
-import org.mockito.MockitoAnnotations;
-
-class SwerveModuleTest {
-
-  private SwerveModule swerveModule;
-  @Mock private ModuleInterface mockModuleInterface;
-  @Mock private ModuleInputsAutoLogged mockInputs;
-
-  @BeforeEach
-  void setUp() {
-    MockitoAnnotations.openMocks(this);
-
-    // Initialize SwerveModule with the mocked interface
-    swerveModule = spy(new SwerveModule(mockModuleInterface, "Mocked Module"));
-    mockInputs = spy(mockInputs);
-  }
-
-  @Test
-  void testRunSetpoint() {
-    // Prepare a mock state for the module
-    SwerveModuleState mockState = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45));
-
-    // Call runSetPoint and verify that the setDesiredState method is called on the module interface
-    swerveModule.runSetpoint(mockState);
-    verify(mockModuleInterface).setDesiredState(mockState);
-  }
-}
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index 498e436..69538ea 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,42 +1,32 @@
 {
-    "fileName": "AdvantageKit.json",
-    "name": "AdvantageKit",
-    "version": "4.0.0-alpha-1",
-    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
-    "frcYear": "2025",
-    "mavenUrls": [],
-    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
-    "javaDependencies": [        
-        {
-            "groupId": "org.littletonrobotics.akit.junction",
-            "artifactId": "wpilib-shim",
-            "version": "4.0.0-alpha-1"
-        },        
-        {
-            "groupId": "org.littletonrobotics.akit.junction",
-            "artifactId": "junction-core",
-            "version": "4.0.0-alpha-1"
-        },        
-        {
-            "groupId": "org.littletonrobotics.akit.conduit",
-            "artifactId": "conduit-api",
-            "version": "4.0.0-alpha-1"
-        }
-    ],
-    "jniDependencies": [        
-        {
-            "groupId": "org.littletonrobotics.akit.conduit",
-            "artifactId": "conduit-wpilibio",
-            "version": "4.0.0-alpha-1",
-            "skipInvalidPlatforms": false,
-            "isJar": false,
-            "validPlatforms": [
-                "linuxathena",
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ]
-        }
-    ],
-    "cppDependencies": []
-}
\ No newline at end of file
+  "fileName": "AdvantageKit.json",
+  "name": "AdvantageKit",
+  "version": "4.0.0-beta-1",
+  "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+  "frcYear": "2025",
+  "mavenUrls": [],
+  "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+  "javaDependencies": [
+    {
+      "groupId": "org.littletonrobotics.akit",
+      "artifactId": "akit-java",
+      "version": "4.0.0-beta-1"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "groupId": "org.littletonrobotics.akit",
+      "artifactId": "akit-wpilibio",
+      "version": "4.0.0-beta-1",
+      "skipInvalidPlatforms": false,
+      "isJar": false,
+      "validPlatforms": [
+        "linuxathena",
+        "windowsx86-64",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "cppDependencies": []
+}
diff --git a/vendordeps/ChoreoLib2025Beta.json b/vendordeps/ChoreoLib2025Beta.json
index 1f6b2b3..a912da9 100644
--- a/vendordeps/ChoreoLib2025Beta.json
+++ b/vendordeps/ChoreoLib2025Beta.json
@@ -1,44 +1,44 @@
 {
-    "fileName": "ChoreoLib2025Beta.json",
-    "name": "ChoreoLib",
-    "version": "2025.0.0-beta-6",
-    "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
-    "frcYear": "2025",
-    "mavenUrls": [
-        "https://SleipnirGroup.github.io/ChoreoLib/dep",
-        "https://repo1.maven.org/maven2"
-    ],
-    "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json",
-    "javaDependencies": [
-        {
-            "groupId": "choreo",
-            "artifactId": "ChoreoLib-java",
-            "version": "2025.0.0-beta-6"
-        },
-        {
-            "groupId": "com.google.code.gson",
-            "artifactId": "gson",
-            "version": "2.11.0"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "choreo",
-            "artifactId": "ChoreoLib-cpp",
-            "version": "2025.0.0-beta-6",
-            "libName": "ChoreoLib",
-            "headerClassifier": "headers",
-            "sharedLibrary": false,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal",
-                "linuxathena",
-                "linuxarm32",
-                "linuxarm64"
-            ]
-        }
-    ]
-}
\ No newline at end of file
+  "fileName": "ChoreoLib2025Beta.json",
+  "name": "ChoreoLib",
+  "version": "2025.0.0-beta-6",
+  "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca",
+  "frcYear": "2025",
+  "mavenUrls": [
+    "https://SleipnirGroup.github.io/ChoreoLib/dep",
+    "https://repo1.maven.org/maven2"
+  ],
+  "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib2025Beta.json",
+  "javaDependencies": [
+    {
+      "groupId": "choreo",
+      "artifactId": "ChoreoLib-java",
+      "version": "2025.0.0-beta-6"
+    },
+    {
+      "groupId": "com.google.code.gson",
+      "artifactId": "gson",
+      "version": "2.11.0"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "choreo",
+      "artifactId": "ChoreoLib-cpp",
+      "version": "2025.0.0-beta-6",
+      "libName": "ChoreoLib",
+      "headerClassifier": "headers",
+      "sharedLibrary": false,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "osxuniversal",
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64"
+      ]
+    }
+  ]
+}
diff --git a/vendordeps/Dyn4j.json b/vendordeps/Dyn4j.json
index 7bbdce5..b537d76 100644
--- a/vendordeps/Dyn4j.json
+++ b/vendordeps/Dyn4j.json
@@ -1,23 +1,23 @@
 {
-    "fileName": "Dyn4j.json",
-    "name": "MapleSim",
-    "version": "0.0.1",
-    "uuid": "123e4567-e89b-12d3-a456-426614174000",
-    "frcYear": "2025",
-    "mavenUrls": [],
-    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
-    "javaDependencies": [
-      {
-        "groupId": "junit",
-        "artifactId": "junit",
-        "version": "4.13.1"
-      },
-      {
-        "groupId": "org.dyn4j",
-        "artifactId": "dyn4j",
-        "version": "5.0.2"
-      }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": []
-  }
\ No newline at end of file
+  "fileName": "Dyn4j.json",
+  "name": "MapleSim",
+  "version": "0.0.1",
+  "uuid": "123e4567-e89b-12d3-a456-426614174000",
+  "frcYear": "2025",
+  "mavenUrls": [],
+  "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+  "javaDependencies": [
+    {
+      "groupId": "junit",
+      "artifactId": "junit",
+      "version": "4.13.1"
+    },
+    {
+      "groupId": "org.dyn4j",
+      "artifactId": "dyn4j",
+      "version": "5.0.2"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": []
+}
diff --git a/vendordeps/NavX-2025.1.1-beta-1.json b/vendordeps/NavX-2025.1.1-beta-1.json
deleted file mode 100644
index 1bb5610..0000000
--- a/vendordeps/NavX-2025.1.1-beta-1.json
+++ /dev/null
@@ -1,39 +0,0 @@
-{
-    "fileName": "NavX-2025.1.1-beta-1.json",
-    "name": "navx_frc",
-    "version": "2025.1.1-beta-1",
-    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
-    "frcYear": "2025",
-    "mavenUrls": [
-        "https://dev.studica.com/maven/release/2025/"
-    ],
-    "jsonUrl": "https://dev.studica.com/releases/2025/NavX-2025.1.1-beta-1.json",
-    "javaDependencies": [
-        {
-            "groupId": "com.kauailabs.navx.frc",
-            "artifactId": "navx_frc-java",
-            "version": "2025.1.1-beta-1"
-        }
-    ],
-    "jniDependencies": [],
-    "cppDependencies": [
-        {
-            "groupId": "com.kauailabs.navx.frc",
-            "artifactId": "navx_frc-cpp",
-            "version": "2025.1.1-beta-1",
-            "headerClassifier": "headers",
-            "sourcesClassifier": "sources",
-            "sharedLibrary": false,
-            "libName": "navx_frc",
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena",
-                "linuxarm32",
-                "linuxarm64",
-                "linuxx86-64",
-                "osxuniversal",
-                "windowsx86-64"
-            ]
-        }
-    ]
-}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6-25.0.0-beta-4.json b/vendordeps/Phoenix6-25.0.0-beta-4.json
new file mode 100644
index 0000000..5a588aa
--- /dev/null
+++ b/vendordeps/Phoenix6-25.0.0-beta-4.json
@@ -0,0 +1,389 @@
+{
+  "fileName": "Phoenix6-25.0.0-beta-4.json",
+  "name": "CTRE-Phoenix (v6)",
+  "version": "25.0.0-beta-4",
+  "frcYear": "2025",
+  "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+  "mavenUrls": [
+    "https://maven.ctr-electronics.com/release/"
+  ],
+  "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-beta-latest.json",
+  "conflictsWith": [
+    {
+      "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
+      "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
+      "offlineFileName": "Phoenix6-replay-frc2025-beta-latest.json"
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "com.ctre.phoenix6",
+      "artifactId": "wpiapi-java",
+      "version": "25.0.0-beta-4"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "groupId": "com.ctre.phoenix6",
+      "artifactId": "api-cpp",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "linuxathena"
+      ],
+      "simMode": "hwsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6",
+      "artifactId": "tools",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "linuxathena"
+      ],
+      "simMode": "hwsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "api-cpp-sim",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "tools-sim",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simTalonSRX",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simVictorSPX",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simPigeonIMU",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simCANCoder",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFX",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProCANcoder",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProCANrange",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProPigeon2",
+      "version": "25.0.0-beta-4",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    }
+  ],
+  "cppDependencies": [
+    {
+      "groupId": "com.ctre.phoenix6",
+      "artifactId": "wpiapi-cpp",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_Phoenix6_WPI",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "linuxathena"
+      ],
+      "simMode": "hwsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6",
+      "artifactId": "tools",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_PhoenixTools",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "linuxathena"
+      ],
+      "simMode": "hwsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "wpiapi-cpp-sim",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_Phoenix6_WPISim",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "tools-sim",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_PhoenixTools_Sim",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simTalonSRX",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimTalonSRX",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simVictorSPX",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimVictorSPX",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simPigeonIMU",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimPigeonIMU",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simCANCoder",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimCANCoder",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProTalonFX",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimProTalonFX",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProCANcoder",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimProCANcoder",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProCANrange",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimProCANrange",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    },
+    {
+      "groupId": "com.ctre.phoenix6.sim",
+      "artifactId": "simProPigeon2",
+      "version": "25.0.0-beta-4",
+      "libName": "CTRE_SimProPigeon2",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxx86-64",
+        "linuxarm64",
+        "osxuniversal"
+      ],
+      "simMode": "swsim"
+    }
+  ]
+}
diff --git a/vendordeps/Phoenix6-replay-25.0.0-beta-1.json b/vendordeps/Phoenix6-replay-25.0.0-beta-1.json
deleted file mode 100644
index e29d5c4..0000000
--- a/vendordeps/Phoenix6-replay-25.0.0-beta-1.json
+++ /dev/null
@@ -1,386 +0,0 @@
-{
-    "fileName": "Phoenix6-replay-25.0.0-beta-1.json",
-    "name": "CTRE-Phoenix (v6) Replay",
-    "version": "25.0.0-beta-1",
-    "frcYear": "2025",
-    "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-replay-frc2025-beta-latest.json",
-    "conflictsWith": [
-        {
-            "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a",
-            "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.",
-            "offlineFileName": "Phoenix6And5.json"
-        },
-        {
-            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
-            "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.",
-            "offlineFileName": "Phoenix6-frc2025-beta-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "wpiapi-java",
-            "version": "25.0.0-beta-1"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "api-cpp",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "tools",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.replay",
-            "artifactId": "api-cpp-replay",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.replay",
-            "artifactId": "tools-replay",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "tools-sim",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simTalonSRX",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simVictorSPX",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simCANCoder",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFX",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANcoder",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
-            "version": "25.0.0-beta-1",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "wpiapi-cpp",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_Phoenix6_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6",
-            "artifactId": "tools",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_PhoenixTools",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.replay",
-            "artifactId": "wpiapi-cpp-replay",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_Phoenix6_WPIReplay",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.replay",
-            "artifactId": "tools-replay",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_PhoenixTools_Replay",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_Phoenix6_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "tools-sim",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_PhoenixTools_Sim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simTalonSRX",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimTalonSRX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simVictorSPX",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimVictorSPX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simPigeonIMU",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimPigeonIMU",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simCANCoder",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimCANCoder",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProTalonFX",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimProTalonFX",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProCANcoder",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimProCANcoder",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix6.sim",
-            "artifactId": "simProPigeon2",
-            "version": "25.0.0-beta-1",
-            "libName": "CTRE_SimProPigeon2",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}
\ No newline at end of file
diff --git a/vendordeps/Studica-2025.1.1-beta-4.json b/vendordeps/Studica-2025.1.1-beta-4.json
new file mode 100644
index 0000000..facfc72
--- /dev/null
+++ b/vendordeps/Studica-2025.1.1-beta-4.json
@@ -0,0 +1,71 @@
+{
+  "fileName": "Studica-2025.1.1-beta-4.json",
+  "name": "Studica",
+  "version": "2025.1.1-beta-4",
+  "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+  "frcYear": "2025",
+  "mavenUrls": [
+    "https://dev.studica.com/maven/release/2025/"
+  ],
+  "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.1.1-beta-4.json",
+  "cppDependencies": [
+    {
+      "artifactId": "Studica-cpp",
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "groupId": "com.studica.frc",
+      "headerClassifier": "headers",
+      "libName": "Studica",
+      "sharedLibrary": false,
+      "skipInvalidPlatforms": true,
+      "version": "2025.1.1-beta-4"
+    },
+    {
+      "artifactId": "Studica-driver",
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "groupId": "com.studica.frc",
+      "headerClassifier": "headers",
+      "libName": "StudicaDriver",
+      "sharedLibrary": false,
+      "skipInvalidPlatforms": true,
+      "version": "2025.1.1-beta-4"
+    }
+  ],
+  "javaDependencies": [
+    {
+      "artifactId": "Studica-java",
+      "groupId": "com.studica.frc",
+      "version": "2025.1.1-beta-4"
+    }
+  ],
+  "jniDependencies": [
+    {
+      "artifactId": "Studica-driver",
+      "groupId": "com.studica.frc",
+      "isJar": false,
+      "skipInvalidPlatforms": true,
+      "validPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "linuxx86-64",
+        "osxuniversal",
+        "windowsx86-64"
+      ],
+      "version": "2025.1.1-beta-4"
+    }
+  ]
+}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..2e8e6b0
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,71 @@
+{
+  "fileName": "photonlib.json",
+  "name": "photonlib",
+  "version": "v2025.0.0-beta-5",
+  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+  "frcYear": "2025",
+  "mavenUrls": [
+    "https://maven.photonvision.org/repository/internal",
+    "https://maven.photonvision.org/repository/snapshots"
+  ],
+  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+  "jniDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "v2025.0.0-beta-5",
+      "skipInvalidPlatforms": true,
+      "isJar": false,
+      "validPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "cppDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-cpp",
+      "version": "v2025.0.0-beta-5",
+      "libName": "photonlib",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "v2025.0.0-beta-5",
+      "libName": "photontargeting",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-java",
+      "version": "v2025.0.0-beta-5"
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-java",
+      "version": "v2025.0.0-beta-5"
+    }
+  ]
+}