diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644
index 0000000..2aaab01
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/tuning.auto b/src/main/deploy/pathplanner/autos/tuning.auto
new file mode 100644
index 0000000..a4de33a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/tuning.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..bab0da9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..9b00b5a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.5,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 3.5,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/java/org/robolancers321/BuildConstants.java b/src/main/java/org/robolancers321/BuildConstants.java
index c0cec9e..f888ee8 100644
--- a/src/main/java/org/robolancers321/BuildConstants.java
+++ b/src/main/java/org/robolancers321/BuildConstants.java
@@ -7,13 +7,13 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "321-Crescendo-2024";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 26;
-  public static final String GIT_SHA = "98ec876acf210c51a7146e70cdaf9765448c192a";
-  public static final String GIT_DATE = "2024-01-29 15:02:18 EST";
-  public static final String GIT_BRANCH = "dev-launcher-4";
-  public static final String BUILD_DATE = "2024-01-29 15:36:21 EST";
-  public static final long BUILD_UNIX_TIME = 1706560581324L;
-  public static final int DIRTY = 0;
+  public static final int GIT_REVISION = 19;
+  public static final String GIT_SHA = "ff080c1eff567a1ee3b838e24c1805a5aa804045";
+  public static final String GIT_DATE = "2024-02-12 16:11:49 EST";
+  public static final String GIT_BRANCH = "dev-drivetrain";
+  public static final String BUILD_DATE = "2024-02-13 15:19:49 EST";
+  public static final long BUILD_UNIX_TIME = 1707855589176L;
+  public static final int DIRTY = 1;
 
   private BuildConstants(){}
 }
diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java
index 3bb9cd1..24f5908 100644
--- a/src/main/java/org/robolancers321/RobotContainer.java
+++ b/src/main/java/org/robolancers321/RobotContainer.java
@@ -1,29 +1,82 @@
 /* (C) Robolancers 2024 */
 package org.robolancers321;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 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.Trigger;
+import org.robolancers321.subsystems.drivetrain.Drivetrain;
 import org.robolancers321.subsystems.intake.Intake;
 import org.robolancers321.subsystems.launcher.Launcher;
 
 public class RobotContainer {
+  Drivetrain drivetrain;
   Intake intake;
   Launcher launcher;
 
+  XboxController driverController;
+  XboxController manipulatorController;
+
+  SendableChooser<Command> autoChooser;
+
   public RobotContainer() {
+    this.drivetrain = Drivetrain.getInstance();
     this.intake = Intake.getInstance();
     this.launcher = Launcher.getInstance();
-    
-    this.intake.retractor.setDefaultCommand(this.intake.retractor.tuneControllers());
-    this.launcher.pivot.setDefaultCommand(this.launcher.pivot.tuneControllers());
-    this.launcher.flywheel.setDefaultCommand(this.launcher.flywheel.tuneController());
 
-    configureBindings();
+    this.driverController = new XboxController(0);
+    this.manipulatorController = new XboxController(1);
+
+    this.autoChooser = AutoBuilder.buildAutoChooser();
+
+    this.configureBindings();
+    this.configureAutoChooser();
+  }
+
+  private void configureBindings() {
+    this.drivetrain.setDefaultCommand(this.drivetrain.teleopDrive(driverController, true));
+
+    new Trigger(this.driverController::getAButton).onTrue(this.drivetrain.zeroYaw());
+
+    new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8)
+        .onTrue(this.intake.retractor.moveToIntake());
+
+    new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8)
+        .onFalse(this.intake.retractor.moveToRetracted());
+
+    new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8)
+        .onTrue(this.launcher.pivot.aimAtAmp());
+
+    new Trigger(() -> this.manipulatorController.getRightTriggerAxis() > 0.8)
+        .onFalse(this.launcher.pivot.moveToRetracted());
   }
 
-  private void configureBindings() {}
+  private void configureAutoChooser() {
+    // NamedCommands.registerCommand("Say Hello", new PrintCommand("Hello"));
+
+    // this.autoChooser.addOption("Do Nothing", new InstantCommand());
+  }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return new InstantCommand();
+
+    // return new PathPlannerAuto("tuning");
+
+    // var curr = drivetrain.getPose();
+
+    // var path =
+    //     new PathPlannerPath(
+    //         PathPlannerPath.bezierFromPoses(
+    //             curr,
+    //             new Pose2d(1.2, 5.3, Rotation2d.fromDegrees(0)),
+    //             new Pose2d(1, 5.3, Rotation2d.fromDegrees(0))),
+    //         Drivetrain.kAutoConstraints,
+    //         new GoalEndState(0, Rotation2d.fromDegrees(0)));
+
+    // path.preventFlipping = true;
+
+    // return AutoBuilder.followPath(path);
   }
 }
diff --git a/src/main/java/org/robolancers321/commands/Mate.java b/src/main/java/org/robolancers321/commands/Mate.java
index 964286b..a161fae 100644
--- a/src/main/java/org/robolancers321/commands/Mate.java
+++ b/src/main/java/org/robolancers321/commands/Mate.java
@@ -1,8 +1,6 @@
 /* (C) Robolancers 2024 */
 package org.robolancers321.commands;
 
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import java.util.function.DoubleSupplier;
 import org.robolancers321.subsystems.intake.Intake;
diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java
new file mode 100644
index 0000000..752f55b
--- /dev/null
+++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java
@@ -0,0 +1,399 @@
+/* (C) Robolancers 2024 */
+package org.robolancers321.subsystems.drivetrain;
+
+import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.Optional;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class Drivetrain extends SubsystemBase {
+  /*
+   * Singleton
+   */
+
+  private static Drivetrain instance = null;
+
+  public static Drivetrain getInstance() {
+    if (instance == null) instance = new Drivetrain();
+
+    return instance;
+  }
+
+  /*
+   * Constants
+   */
+
+  private static final String kMainCameraName = "MainCamera";
+  private static final String kNoteCameraName = "NoteCamera";
+
+  private static final AprilTagFieldLayout kAprilTagFieldLayout =
+      AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+
+  // TODO: find this transform
+  private static final Transform3d kRobotToCameraTransform =
+      new Transform3d(0, 0, 0, new Rotation3d(0, Math.PI, 0));
+
+  private static final double kNoteCameraMountHeight =
+      Units.inchesToMeters(11.0); // rough estimate of camera height while mounted on crate
+  private static final double kNoteCameraMountPitch =
+      0.0; // degrees w/r to the horizontal, above horizontal is positive
+
+  private static final double kTrackWidthMeters = Units.inchesToMeters(17.5);
+  private static final double kWheelBaseMeters = Units.inchesToMeters(17.5);
+
+  private static final double kMaxSpeedMetersPerSecond = 4.0;
+  private static final double kMaxOmegaRadiansPerSecond = 1.5 * Math.PI;
+
+  public static final PathConstraints kAutoConstraints =
+      new PathConstraints(4.0, 1.0, 270 * Math.PI / 180, 360 * Math.PI / 180);
+
+  private static final SwerveDriveKinematics kSwerveKinematics =
+      new SwerveDriveKinematics(
+          new Translation2d(0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters), // front left
+          new Translation2d(0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters), // front right
+          new Translation2d(-0.5 * kTrackWidthMeters, -0.5 * kWheelBaseMeters), // back right
+          new Translation2d(-0.5 * kTrackWidthMeters, 0.5 * kWheelBaseMeters) // back left
+          );
+
+  private static double kSecondOrderKinematicsDt = 0.2;
+
+  private static final double kTranslationP = 2.0;
+  private static final double kTranslationI = 0.0;
+  private static final double kTranslationD = 0.0;
+
+  private static final double kRotationP = 4.0;
+  private static final double kRotationI = 0.0;
+  private static final double kRotationD = 0.0;
+
+  /*
+   * Implementation
+   */
+
+  private SwerveModule frontLeft;
+  private SwerveModule frontRight;
+  private SwerveModule backRight;
+  private SwerveModule backLeft;
+
+  private AHRS gyro;
+
+  private PhotonCamera mainCamera;
+  private PhotonPoseEstimator visionEstimator;
+
+  private SwerveDrivePoseEstimator odometry;
+
+  private PhotonCamera noteCamera;
+
+  private Field2d field;
+
+  private Drivetrain() {
+    this.frontLeft = SwerveModule.getFrontLeft();
+    this.frontRight = SwerveModule.getFrontRight();
+    this.backRight = SwerveModule.getBackRight();
+    this.backLeft = SwerveModule.getBackLeft();
+
+    this.gyro = new AHRS(SPI.Port.kMXP);
+    this.zeroYaw();
+
+    this.mainCamera = new PhotonCamera(kMainCameraName);
+
+    this.visionEstimator =
+        new PhotonPoseEstimator(
+            kAprilTagFieldLayout,
+            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+            mainCamera,
+            kRobotToCameraTransform);
+
+    this.odometry =
+        new SwerveDrivePoseEstimator(
+            kSwerveKinematics, this.gyro.getRotation2d(), this.getModulePositions(), new Pose2d());
+
+    this.noteCamera = new PhotonCamera(kNoteCameraName);
+
+    this.field = new Field2d();
+
+    SmartDashboard.putData(this.field);
+
+    AutoBuilder.configureHolonomic(
+        this::getPose,
+        this::resetPose,
+        this::getChassisSpeeds,
+        this::drive,
+        new HolonomicPathFollowerConfig(
+            new PIDConstants(kTranslationP, kTranslationI, kTranslationD),
+            new PIDConstants(kRotationP, kRotationI, kRotationD),
+            kMaxSpeedMetersPerSecond,
+            0.5 * Math.hypot(kTrackWidthMeters, kWheelBaseMeters),
+            new ReplanningConfig()),
+        () -> {
+          var myAlliance = DriverStation.getAlliance();
+
+          if (myAlliance.isPresent()) return myAlliance.get() == DriverStation.Alliance.Red;
+
+          return false;
+        },
+        this);
+
+    PathPlannerLogging.setLogActivePathCallback(
+        poses -> this.field.getObject("pathplanner path poses").setPoses(poses));
+    PathPlannerLogging.setLogTargetPoseCallback(
+        targ -> this.field.getObject("pathplanner targ pose").setPose(targ));
+    PathPlannerLogging.setLogCurrentPoseCallback(
+        curr -> this.field.getObject("pathplanner curr pose").setPose(curr));
+  }
+
+  public Pose2d getPose() {
+    return this.odometry.getEstimatedPosition();
+  }
+
+  public void resetPose(Pose2d pose) {
+    this.odometry.resetPosition(
+        Rotation2d.fromDegrees(this.gyro.getAngle()), // .plus(Rotation2d.fromDegrees(180)),
+        this.getModulePositions(),
+        pose);
+  }
+
+  private SwerveModuleState[] getModuleStates() {
+    return new SwerveModuleState[] {
+      this.frontLeft.getState(),
+      this.frontRight.getState(),
+      this.backRight.getState(),
+      this.backLeft.getState()
+    };
+  }
+
+  public ChassisSpeeds getChassisSpeeds() {
+    return kSwerveKinematics.toChassisSpeeds(this.getModuleStates());
+  }
+
+  private SwerveModulePosition[] getModulePositions() {
+    return new SwerveModulePosition[] {
+      this.frontLeft.getPosition(),
+      this.frontRight.getPosition(),
+      this.backRight.getPosition(),
+      this.backLeft.getPosition()
+    };
+  }
+
+  public double getYawDeg() {
+    return this.gyro.getYaw();
+  }
+
+  private void updateModules(SwerveModuleState[] states) {
+    this.frontLeft.update(states[0]);
+    this.frontRight.update(states[1]);
+    this.backRight.update(states[2]);
+    this.backLeft.update(states[3]);
+  }
+
+  private void drive(
+      double desiredThrottleMPS,
+      double desiredStrafeMPS,
+      double desiredOmegaRadPerSec,
+      boolean fieldRelative) {
+    double correctedOmega =
+        MathUtil.clamp(
+            desiredOmegaRadPerSec, -kMaxOmegaRadiansPerSecond, kMaxOmegaRadiansPerSecond);
+
+    // apply corrective pose logarithm
+    double dt = kSecondOrderKinematicsDt;
+    double angularDisplacement =
+        -correctedOmega * dt; // TODO: why is this negative, maybe gyro orientation
+
+    double sin = Math.sin(0.5 * angularDisplacement);
+    double cos = Math.cos(0.5 * angularDisplacement);
+
+    double correctedThrottle = desiredStrafeMPS * sin + desiredThrottleMPS * cos;
+    double correctedStrafe = desiredStrafeMPS * cos - desiredThrottleMPS * sin;
+
+    ChassisSpeeds speeds =
+        fieldRelative
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(
+                correctedStrafe, correctedThrottle, correctedOmega, this.gyro.getRotation2d())
+            : new ChassisSpeeds(correctedStrafe, correctedThrottle, correctedOmega);
+
+    this.drive(speeds);
+  }
+
+  private void drive(ChassisSpeeds speeds) {
+    SwerveModuleState[] states = kSwerveKinematics.toSwerveModuleStates(speeds);
+
+    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeedMetersPerSecond);
+
+    this.updateModules(states);
+  }
+
+  private void fuseVision() {
+    Optional<EstimatedRobotPose> visionEstimate = visionEstimator.update();
+
+    if (visionEstimate.isEmpty()) return;
+
+    this.odometry.addVisionMeasurement(
+        visionEstimate.get().estimatedPose.toPose2d(), visionEstimate.get().timestampSeconds);
+  }
+
+  private Translation2d getRelativeNoteLocation() {
+    PhotonPipelineResult latestResult = this.noteCamera.getLatestResult();
+
+    if (!latestResult.hasTargets()) return new Translation2d();
+
+    PhotonTrackedTarget bestTarget = latestResult.getBestTarget();
+
+    // TODO: plus or minus mount pitch?
+    double dz =
+        kNoteCameraMountHeight
+            / Math.tan((-bestTarget.getPitch() + kNoteCameraMountPitch) * Math.PI / 180.0);
+    double dx = dz * Math.tan(bestTarget.getYaw() * Math.PI / 180.0);
+
+    return new Translation2d(dx, dz);
+  }
+
+  private void doSendables() {
+    SmartDashboard.putNumber("Drive Heading", this.getYawDeg());
+
+    Pose2d odometryPose = this.getPose();
+
+    SmartDashboard.putNumber("Odometry Pos X (m)", odometryPose.getX());
+    SmartDashboard.putNumber("Odometry Pos Y (m)", odometryPose.getY());
+    SmartDashboard.putNumber("Odometry Angle (deg)", odometryPose.getRotation().getDegrees());
+
+    Translation2d note = this.getRelativeNoteLocation();
+
+    SmartDashboard.putNumber("note x pffset", note.getX());
+    SmartDashboard.putNumber("note z offset", note.getY());
+  }
+
+  @Override
+  public void periodic() {
+    this.odometry.update(this.gyro.getRotation2d(), this.getModulePositions());
+
+    this.fuseVision();
+
+    this.field.setRobotPose(this.getPose());
+
+    this.doSendables();
+
+    this.frontLeft.doSendables();
+    this.frontRight.doSendables();
+    this.backRight.doSendables();
+    this.backLeft.doSendables();
+  }
+
+  public Command zeroYaw() {
+    return runOnce(
+        () -> {
+          this.gyro.zeroYaw();
+          this.gyro.setAngleAdjustment(0.0); // TODO: change this depending on navx orientation
+        });
+  }
+
+  private Command stop() {
+    return runOnce(() -> this.drive(0.0, 0.0, 0.0, false));
+  }
+
+  private Command feedForwardDrive(
+      DoubleSupplier throttleSupplier,
+      DoubleSupplier strafeSupplier,
+      DoubleSupplier omegaSupplier,
+      BooleanSupplier fieldCentricSupplier) {
+    return run(() -> {
+          double desiredThrottle = throttleSupplier.getAsDouble();
+          double desiredStrafe = strafeSupplier.getAsDouble();
+          double desiredOmega = omegaSupplier.getAsDouble();
+          boolean desiredFieldCentic = fieldCentricSupplier.getAsBoolean();
+
+          this.drive(desiredThrottle, desiredStrafe, desiredOmega, desiredFieldCentic);
+        })
+        .finallyDo(() -> this.stop());
+  }
+
+  public Command teleopDrive(XboxController controller, boolean fieldCentric) {
+    return this.feedForwardDrive(
+        () -> 0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftY(), 0.05),
+        () -> -0.1 * kMaxSpeedMetersPerSecond * MathUtil.applyDeadband(controller.getLeftX(), 0.05),
+        () ->
+            -0.3 * kMaxOmegaRadiansPerSecond * MathUtil.applyDeadband(controller.getRightX(), 0.05),
+        () -> fieldCentric);
+  }
+
+  public Command tuneModules() {
+    SmartDashboard.putNumber("target module angle (deg)", 0.0);
+    SmartDashboard.putNumber("target module vel (m/s)", 0.0);
+
+    SwerveModule.initTuning();
+
+    return run(() -> {
+          double theta = SmartDashboard.getNumber("target module angle (deg)", 0.0);
+          double vel = SmartDashboard.getNumber("target module vel (m/s)", 0.0);
+
+          SwerveModuleState[] states = new SwerveModuleState[4];
+          for (int i = 0; i < 4; i++)
+            states[i] = new SwerveModuleState(vel, Rotation2d.fromDegrees(theta));
+
+          this.frontLeft.tune();
+          this.frontRight.tune();
+          this.backRight.tune();
+          this.backLeft.tune();
+
+          this.updateModules(states);
+        })
+        .finallyDo(() -> this.stop());
+  }
+
+  public Command dangerouslyRunDrive(double speed) {
+    return run(
+        () -> {
+          this.frontLeft.dangerouslyRunDrive(speed);
+          this.frontRight.dangerouslyRunDrive(speed);
+          this.backRight.dangerouslyRunDrive(speed);
+          this.backLeft.dangerouslyRunDrive(speed);
+        });
+  }
+
+  public Command dangerouslyRunTurn(double speed) {
+    return run(
+        () -> {
+          this.frontLeft.dangerouslyRunTurn(speed);
+          this.frontRight.dangerouslyRunTurn(speed);
+          this.backRight.dangerouslyRunTurn(speed);
+          this.backLeft.dangerouslyRunTurn(speed);
+        });
+  }
+
+  public Command followPath(PathPlannerPath path) {
+    return AutoBuilder.followPath(path);
+  }
+}
diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java
new file mode 100644
index 0000000..62a9b0e
--- /dev/null
+++ b/src/main/java/org/robolancers321/subsystems/drivetrain/SwerveModule.java
@@ -0,0 +1,267 @@
+/* (C) Robolancers 2024 */
+package org.robolancers321.subsystems.drivetrain;
+
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.CANSparkBase.ControlType;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SwerveModule {
+  /*
+   * Singletons
+   */
+
+  private static SwerveModule frontLeft = null;
+
+  public static SwerveModule getFrontLeft() {
+    if (frontLeft == null)
+      frontLeft = new SwerveModule("Front Left", 2, 1, 9, false, true, false, -0.457520);
+
+    return frontLeft;
+  }
+
+  private static SwerveModule frontRight = null;
+
+  public static SwerveModule getFrontRight() {
+    if (frontRight == null)
+      frontRight = new SwerveModule("Front Right", 4, 3, 10, false, true, false, 0.140137);
+
+    return frontRight;
+  }
+
+  private static SwerveModule backRight = null;
+
+  public static SwerveModule getBackRight() {
+    if (backRight == null)
+      backRight = new SwerveModule("Back Right", 6, 5, 11, true, true, false, -0.128906);
+
+    return backRight;
+  }
+
+  private static SwerveModule backLeft = null;
+
+  public static SwerveModule getBackLeft() {
+    if (backLeft == null)
+      backLeft = new SwerveModule("Back Left", 8, 7, 12, false, true, false, -0.083984);
+
+    return backLeft;
+  }
+
+  /*
+   * Constants
+   */
+
+  private static final CANcoderConfiguration kCANCoderConfig = new CANcoderConfiguration();
+
+  private static final double kWheelRadiusMeters = Units.inchesToMeters(2.0);
+  private static final double kGearRatio = 6.12;
+  private static final double kDrivePositionConversionFactor =
+      2 * Math.PI * kWheelRadiusMeters / kGearRatio;
+  private static final double kDriveVelocityConversionFactor =
+      2 * Math.PI * kWheelRadiusMeters / (kGearRatio * 60.0);
+  private static final double kTurnPositionConversionFactor = 7.0 / 150.0;
+
+  private static final double kDriveP = 0.00;
+  private static final double kDriveI = 0.00;
+  private static final double kDriveD = 0.00;
+  private static final double kDriveFF = 0.198;
+
+  private static final double kTurnP = 10; // 20
+  private static final double kTurnI = 0.00;
+  private static final double kTurnD = 0;
+
+  /*
+   * Implementation
+   */
+
+  private String id;
+
+  private CANSparkMax driveMotor;
+  private CANSparkMax turnMotor;
+
+  private RelativeEncoder driveEncoder;
+  private CANcoder absoluteTurnEncoder;
+  private RelativeEncoder turnEncoder;
+
+  private SparkPIDController driveController;
+  private SparkPIDController turnController;
+
+  private SwerveModule(
+      String id,
+      int driveMotorPort,
+      int turnMotorPort,
+      int turnEncoderPort,
+      boolean invertDriveMotor,
+      boolean invertTurnMotor,
+      boolean invertTurnEncoder,
+      double turnEncoderOffset) {
+    this.id = id;
+
+    this.configDrive(driveMotorPort, invertDriveMotor);
+    this.configTurn(
+        turnMotorPort, turnEncoderPort, invertTurnMotor, invertTurnEncoder, turnEncoderOffset);
+  }
+
+  private void configDrive(int driveMotorPort, boolean invertDriveMotor) {
+    this.driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless);
+    this.driveMotor.setInverted(invertDriveMotor);
+    this.driveMotor.setIdleMode(IdleMode.kBrake);
+    this.driveMotor.setSmartCurrentLimit(40);
+    this.driveMotor.enableVoltageCompensation(12);
+
+    this.driveEncoder = this.driveMotor.getEncoder();
+    this.driveEncoder.setPosition(0.0);
+    this.driveEncoder.setPositionConversionFactor(kDrivePositionConversionFactor);
+    this.driveEncoder.setVelocityConversionFactor(kDriveVelocityConversionFactor);
+
+    this.driveController = this.driveMotor.getPIDController();
+    this.driveController.setP(kDriveP);
+    this.driveController.setI(kDriveI);
+    this.driveController.setD(kDriveD);
+    this.driveController.setFF(kDriveFF);
+
+    this.driveMotor.burnFlash();
+  }
+
+  private void configTurn(
+      int turnMotorPort,
+      int turnEncoderPort,
+      boolean invertTurnMotor,
+      boolean invertTurnEncoder,
+      double turnEncoderOffset) {
+
+    this.turnMotor = new CANSparkMax(turnMotorPort, MotorType.kBrushless);
+    this.turnMotor.setInverted(invertTurnMotor);
+    this.turnMotor.setIdleMode(IdleMode.kBrake);
+    this.turnMotor.setSmartCurrentLimit(40);
+    this.turnMotor.enableVoltageCompensation(12);
+
+    this.absoluteTurnEncoder = new CANcoder(turnEncoderPort);
+    CANcoderConfiguration config = kCANCoderConfig;
+    config.MagnetSensor.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Signed_PlusMinusHalf);
+    config.MagnetSensor.withMagnetOffset(turnEncoderOffset);
+    config.MagnetSensor.withSensorDirection(
+        invertTurnEncoder
+            ? SensorDirectionValue.Clockwise_Positive
+            : SensorDirectionValue.CounterClockwise_Positive);
+    this.absoluteTurnEncoder.getConfigurator().apply(config);
+
+    this.turnEncoder = this.turnMotor.getEncoder();
+    this.turnEncoder.setPositionConversionFactor(kTurnPositionConversionFactor);
+    this.turnEncoder.setPosition(
+        this.absoluteTurnEncoder
+            .getAbsolutePosition()
+            .getValueAsDouble()); // TODO: do we need to delay this call?
+
+    this.turnController = this.turnMotor.getPIDController();
+    this.turnController.setPositionPIDWrappingMinInput(-Math.PI);
+    this.turnController.setPositionPIDWrappingMaxInput(Math.PI);
+    this.turnController.setPositionPIDWrappingEnabled(true);
+    this.turnController.setP(kTurnP);
+    this.turnController.setI(kTurnI);
+    this.turnController.setD(kTurnD);
+
+    this.turnMotor.burnFlash();
+  }
+
+  public double getDriveVelocityMPS() {
+    return this.driveEncoder.getVelocity();
+  }
+
+  public double getTurnAngleRotations() {
+    return this.turnEncoder.getPosition();
+  }
+
+  public double getTurnAngleRad() {
+    return 2 * Math.PI * this.turnEncoder.getPosition();
+  }
+
+  public double getTurnAngleDeg() {
+    return 360.0 * this.getTurnAngleRotations();
+  }
+
+  public SwerveModulePosition getPosition() {
+    return new SwerveModulePosition(
+        this.driveEncoder.getPosition(), Rotation2d.fromRadians(this.getTurnAngleRad()));
+  }
+
+  public SwerveModuleState getState() {
+    return new SwerveModuleState(
+        this.getDriveVelocityMPS(), Rotation2d.fromRadians(this.getTurnAngleRad()));
+  }
+
+  protected void dangerouslyRunDrive(double speed) {
+    this.driveMotor.set(speed);
+  }
+
+  protected void dangerouslyRunTurn(double speed) {
+    this.turnMotor.set(speed);
+  }
+
+  protected void update(SwerveModuleState desiredState) {
+    SwerveModuleState optimized =
+        SwerveModuleState.optimize(desiredState, Rotation2d.fromRadians(this.getTurnAngleRad()));
+
+    SmartDashboard.putNumber(this.id + " ref angle", optimized.angle.getDegrees());
+
+    this.driveController.setReference(optimized.speedMetersPerSecond, ControlType.kVelocity);
+    this.turnController.setReference(optimized.angle.getRotations(), ControlType.kPosition);
+  }
+
+  protected void doSendables() {
+    SmartDashboard.putNumber(this.id + " position m", this.getPosition().distanceMeters);
+
+    SmartDashboard.putNumber(this.id + " Drive Vel (m/s)", this.getDriveVelocityMPS());
+    SmartDashboard.putNumber(this.id + " Turn Angle (deg)", this.getTurnAngleDeg());
+
+    SmartDashboard.putNumber(
+        this.id + " abs enc deg",
+        360 * this.absoluteTurnEncoder.getAbsolutePosition().getValueAsDouble());
+  }
+
+  protected static void initTuning() {
+    SmartDashboard.putNumber(
+        "module drive kp", SmartDashboard.getNumber("module drive kp", kDriveP));
+    SmartDashboard.putNumber(
+        "module drive ki", SmartDashboard.getNumber("module drive ki", kDriveI));
+    SmartDashboard.putNumber(
+        "module drive kd", SmartDashboard.getNumber("module drive kd", kDriveD));
+    SmartDashboard.putNumber(
+        "module drive kff", SmartDashboard.getNumber("module drive kff", kDriveFF));
+
+    SmartDashboard.putNumber("module turn kp", SmartDashboard.getNumber("module turn kp", kTurnP));
+    SmartDashboard.putNumber("module turn ki", SmartDashboard.getNumber("module turn ki", kTurnI));
+    SmartDashboard.putNumber("module turn kd", SmartDashboard.getNumber("module turn kd", kTurnD));
+  }
+
+  protected void tune() {
+    double tunedDriveP = SmartDashboard.getNumber("module drive kp", kDriveP);
+    double tunedDriveI = SmartDashboard.getNumber("module drive ki", kDriveI);
+    double tunedDriveD = SmartDashboard.getNumber("module drive kd", kDriveD);
+    double tunedDriveFF = SmartDashboard.getNumber("module drive kff", kDriveFF);
+
+    this.driveController.setP(tunedDriveP);
+    this.driveController.setI(tunedDriveI);
+    this.driveController.setD(tunedDriveD);
+    this.driveController.setFF(tunedDriveFF);
+
+    double tunedTurnP = SmartDashboard.getNumber("module turn kp", kTurnP);
+    double tunedTurnI = SmartDashboard.getNumber("module turn ki", kTurnI);
+    double tunedTurnD = SmartDashboard.getNumber("module turn kd", kTurnD);
+
+    this.turnController.setP(tunedTurnP);
+    this.turnController.setI(tunedTurnI);
+    this.turnController.setD(tunedTurnD);
+  }
+}
diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java
index 7c1f086..ca99eb3 100644
--- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java
+++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java
@@ -8,7 +8,6 @@
 import com.revrobotics.CANSparkBase.SoftLimitDirection;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.SparkAbsoluteEncoder.Type;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -41,7 +40,7 @@ public static Retractor getInstance() {
   private static final boolean kInvertEncoder = true;
 
   private static final int kCurrentLimit = 40;
-  
+
   private static final double kGearRatio = 360.0;
 
   private static final float kMinAngle = -20.0f;
@@ -58,12 +57,12 @@ public static Retractor getInstance() {
   private static final double kMaxVelocityDeg = 90.0;
   private static final double kMaxAccelerationDeg = 60.0;
 
-  private static final double kToleranceDeg = 2.5;
+  private static final double kToleranceDeg = 5.0;
 
   private enum RetractorSetpoint {
-    kRetracted(0),
+    kRetracted(160),
     kMating(0),
-    kIntake(0);
+    kIntake(-10.0);
 
     public final double angle;
 
@@ -105,7 +104,7 @@ private void configureMotor() {
     // this.motor.setSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, (float) kMinAngle);
     // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true);
     // this.motor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true);
-    
+
     this.motor.enableSoftLimit(SoftLimitDirection.kForward, false);
     this.motor.enableSoftLimit(SoftLimitDirection.kReverse, false);
   }
@@ -126,14 +125,16 @@ private void configureController() {
 
     this.m_enabled = true;
   }
-  
+
   @Override
   public double getMeasurement() {
     return this.getPositionDeg();
   }
 
   public double getPositionDeg() {
-    return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition();
+    return this.encoder.getPosition() > 180
+        ? this.encoder.getPosition() - 360.0
+        : this.encoder.getPosition();
   }
 
   public double getVelocityDeg() {
@@ -147,7 +148,8 @@ private boolean atGoal() {
   @Override
   protected void useOutput(double output, TrapezoidProfile.State setpoint) {
     double feedforwardOutput =
-        this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0);
+        this.feedforwardController.calculate(
+            setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0);
 
     SmartDashboard.putNumber("retractor position setpoint mp (deg)", setpoint.position);
     SmartDashboard.putNumber("retractor velocity setpoint mp (deg)", setpoint.velocity);
@@ -170,7 +172,7 @@ private void doSendables() {
     SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg());
     SmartDashboard.putNumber("retractor velocity (deg)", this.getVelocityDeg());
   }
-  
+
   @Override
   public void periodic() {
     super.periodic();
@@ -187,8 +189,12 @@ private void initTuning() {
     SmartDashboard.putNumber("retractor kv", SmartDashboard.getNumber("retractor kv", kV));
     SmartDashboard.putNumber("retractor kg", SmartDashboard.getNumber("retractor kg", kG));
 
-    SmartDashboard.putNumber("retractor max vel (deg)", SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg));
-    SmartDashboard.putNumber("retractor max acc (deg)", SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg));
+    SmartDashboard.putNumber(
+        "retractor max vel (deg)",
+        SmartDashboard.getNumber("retractor max vel (deg)", kMaxVelocityDeg));
+    SmartDashboard.putNumber(
+        "retractor max acc (deg)",
+        SmartDashboard.getNumber("retractor max acc (deg)", kMaxAccelerationDeg));
 
     SmartDashboard.putNumber("retractor target position (deg)", this.getPositionDeg());
   }
@@ -211,11 +217,16 @@ private void tune() {
 
     this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc));
 
-    this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("retractor target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle));
+    this.setGoal(
+        MathUtil.clamp(
+            SmartDashboard.getNumber("retractor target position (deg)", this.getPositionDeg()),
+            kMinAngle,
+            kMaxAngle));
   }
-  
+
   private Command moveToAngle(double angleDeg) {
-    return run(() -> this.setGoal(MathUtil.clamp(angleDeg, kMinAngle, kMaxAngle))).until(this::atGoal);
+    return run(() -> this.setGoal(MathUtil.clamp(angleDeg, kMinAngle, kMaxAngle)))
+        .until(this::atGoal);
   }
 
   public Command moveToRetracted() {
diff --git a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java
index cb11f6c..cec7c07 100644
--- a/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java
+++ b/src/main/java/org/robolancers321/subsystems/launcher/AimTable.java
@@ -42,12 +42,12 @@ private static AimCharacteristic calculateSpeakerAimCharacteristic(double distan
     double lowerBound = keys.get(0);
     double upperBound = keys.get(keys.size() - 1);
 
-    if (distance < lowerBound){
+    if (distance < lowerBound) {
       AimTable.AimCharacteristic lowerValue = table.get(lowerBound);
 
       return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm);
     }
-    
+
     if (distance > upperBound) {
       AimTable.AimCharacteristic upperValue = table.get(upperBound);
 
diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java
index 744df0e..bb1d127 100644
--- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java
+++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java
@@ -5,8 +5,6 @@
 
 import com.revrobotics.*;
 import com.revrobotics.CANSparkBase.ControlType;
-
-import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -40,7 +38,7 @@ private enum FlywheelSetpoint {
 
     public final double rpm;
 
-    private FlywheelSetpoint(double rpm){
+    private FlywheelSetpoint(double rpm) {
       this.rpm = rpm;
     }
   }
diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java
index 8cf2bdc..7dee95f 100644
--- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java
+++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java
@@ -5,7 +5,6 @@
 
 import com.revrobotics.*;
 import com.revrobotics.CANSparkBase.ControlType;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java
index 6d4f18a..0c1e3d5 100644
--- a/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java
+++ b/src/main/java/org/robolancers321/subsystems/launcher/Launcher.java
@@ -1,17 +1,7 @@
 /* (C) Robolancers 2024 */
 package org.robolancers321.subsystems.launcher;
 
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
-import java.util.function.DoubleSupplier;
 
 public class Launcher extends SubsystemBase {
   /*
diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java
index 6094587..6306f63 100644
--- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java
+++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java
@@ -6,7 +6,6 @@
 import com.revrobotics.*;
 import com.revrobotics.CANSparkBase.SoftLimitDirection;
 import com.revrobotics.SparkAbsoluteEncoder.Type;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -56,12 +55,12 @@ public static Pivot getInstance() {
   private static final double kMaxVelocityDeg = 240;
   private static final double kMaxAccelerationDeg = 360;
 
-  private static final double kToleranceDeg = 2.5;
+  private static final double kToleranceDeg = 5.0;
 
   private enum PivotSetpoint {
     kRetracted(0.0),
     kMating(0.0),
-    kAmp(0.0);
+    kAmp(85.0);
 
     public final double angle;
 
@@ -129,9 +128,11 @@ private void configureController() {
   public double getMeasurement() {
     return this.getPositionDeg();
   }
-  
+
   public double getPositionDeg() {
-    return this.encoder.getPosition() > 180 ? this.encoder.getPosition() - 360.0 : this.encoder.getPosition();
+    return this.encoder.getPosition() > 180
+        ? this.encoder.getPosition() - 360.0
+        : this.encoder.getPosition();
   }
 
   public double getVelocityDeg() {
@@ -145,7 +146,8 @@ private boolean atGoal() {
   @Override
   protected void useOutput(double output, TrapezoidProfile.State setpoint) {
     double feedforwardOutput =
-        this.feedforwardController.calculate(setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0);
+        this.feedforwardController.calculate(
+            setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0);
 
     SmartDashboard.putNumber("pivot position setpoint mp (deg)", setpoint.position);
     SmartDashboard.putNumber("pivot velocity setpoint mp (deg)", setpoint.velocity);
@@ -184,8 +186,11 @@ private void initTuning() {
     SmartDashboard.putNumber("pivot kg", SmartDashboard.getNumber("pivot kg", kG));
     SmartDashboard.putNumber("pivot kv", SmartDashboard.getNumber("pivot kv", kV));
 
-    SmartDashboard.putNumber("pivot max vel (deg)", SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg));
-    SmartDashboard.putNumber("pivot max acc (deg)", SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg));
+    SmartDashboard.putNumber(
+        "pivot max vel (deg)", SmartDashboard.getNumber("pivot max vel (deg)", kMaxVelocityDeg));
+    SmartDashboard.putNumber(
+        "pivot max acc (deg)",
+        SmartDashboard.getNumber("pivot max acc (deg)", kMaxAccelerationDeg));
 
     SmartDashboard.putNumber("pivot target position (deg)", this.getPositionDeg());
   }
@@ -208,11 +213,17 @@ private void tune() {
 
     this.m_controller.setConstraints(new Constraints(tunedMaxVel, tunedMaxAcc));
 
-    this.setGoal(MathUtil.clamp(SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()), kMinAngle, kMaxAngle));
+    this.setGoal(
+        MathUtil.clamp(
+            SmartDashboard.getNumber("pivot target position (deg)", this.getPositionDeg()),
+            kMinAngle,
+            kMaxAngle));
   }
 
   private Command moveToAngle(DoubleSupplier angleDegSupplier) {
-    return run(() -> this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle))).until(this::atGoal);
+    return run(() ->
+            this.setGoal(MathUtil.clamp(angleDegSupplier.getAsDouble(), kMinAngle, kMaxAngle)))
+        .until(this::atGoal);
   }
 
   private Command moveToAngle(double angleDeg) {
diff --git a/src/test/java/org/robolancers321/AimTableTest.java b/src/test/java/org/robolancers321/AimTableTest.java
index 674a70d..90f8f37 100644
--- a/src/test/java/org/robolancers321/AimTableTest.java
+++ b/src/test/java/org/robolancers321/AimTableTest.java
@@ -1,169 +1,158 @@
+/* (C) Robolancers 2024 */
 package org.robolancers321;
 
-import static java.lang.System.in;
 import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.robolancers321.util.MathUtils.epsilonEquals;
 
+import java.util.*;
 import org.junit.jupiter.api.Test;
 import org.robolancers321.subsystems.launcher.AimTable;
 
-import java.util.*;
-
-
 public class AimTableTest {
-    private static final LinkedHashMap<Double, AimTable.AimCharacteristic> table =
-            new LinkedHashMap<>() {
-                {
-                    put(0.0, new AimTable.AimCharacteristic(0, 0.0));
-                    put(1.0, new AimTable.AimCharacteristic(10, 1020));
-                    put(2.0, new AimTable.AimCharacteristic(20, 1500));
-                    put(4.0, new AimTable.AimCharacteristic(40, 2500));
-                    put(5.0, new AimTable.AimCharacteristic(54, 3300));
-                    put(6.0, new AimTable.AimCharacteristic(61, 4227));
-                    put(8.0, new AimTable.AimCharacteristic(83, 5599));
-                }
-            };
-
-        private class DummyAimTable {
-            private AimTable.AimCharacteristic lastAimCharacteristic;
-
-            private final double lastDistance = 0;
-
-            private double kInterpolationCacheThreshold = 0.0;
-
-            private static double interpolate(
-                    double lowKey, double lowValue, double highKey, double highValue, double x) {
-                double percent = (x - lowKey) / (highKey - lowKey);
-
-                return lowValue + percent * (highValue - lowValue);
-            }
-
-            public static AimTable.AimCharacteristic getSpeakerAimCharacteristic(double distance) {
-                List<Double> keys = table.keySet().stream().sorted().toList();
-                double lowerBound = keys.get(0);
-                double upperBound = keys.get(keys.size() - 1);
-
-                if ((distance < lowerBound)) {
-                    AimTable.AimCharacteristic lowerValue = table.get(lowerBound);
-
-                    return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm);
-                }
-                else if (distance > upperBound) {
-                    AimTable.AimCharacteristic upperValue = table.get(upperBound);
-
-                    return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm);
-                }
-
-                System.out.println("Distance " + distance);
-
-                NavigableSet<Double> values = new TreeSet<>(keys);
-                double lowKey = values.floor(distance);
-                double highKey = values.ceiling(distance);
-
-                return new AimTable.AimCharacteristic(
-                        interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance),
-                        interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance));
-            }
-
-            public AimTable.AimCharacteristic getLastAimCharacteristic(double distance) {
-                if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) {
-                    lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance);
-                    kInterpolationCacheThreshold = lastDistance;
-                }
-
-                return lastAimCharacteristic;
-            }
+  private static final LinkedHashMap<Double, AimTable.AimCharacteristic> table =
+      new LinkedHashMap<>() {
+        {
+          put(0.0, new AimTable.AimCharacteristic(0, 0.0));
+          put(1.0, new AimTable.AimCharacteristic(10, 1020));
+          put(2.0, new AimTable.AimCharacteristic(20, 1500));
+          put(4.0, new AimTable.AimCharacteristic(40, 2500));
+          put(5.0, new AimTable.AimCharacteristic(54, 3300));
+          put(6.0, new AimTable.AimCharacteristic(61, 4227));
+          put(8.0, new AimTable.AimCharacteristic(83, 5599));
         }
+      };
 
+  private class DummyAimTable {
+    private AimTable.AimCharacteristic lastAimCharacteristic;
 
-    private List<Double> getKeys() {
-        return table.keySet().stream().sorted().toList();
-    }
-    private static double interpolate(
-            double lowKey, double lowValue, double highKey, double highValue, double x) {
-        double percent = (x - lowKey) / (highKey - lowKey);
+    private final double lastDistance = 0;
+
+    private double kInterpolationCacheThreshold = 0.0;
 
+    private static double interpolate(
+        double lowKey, double lowValue, double highKey, double highValue, double x) {
+      double percent = (x - lowKey) / (highKey - lowKey);
 
-        return lowValue + percent * (highValue - lowValue);
+      return lowValue + percent * (highValue - lowValue);
     }
 
-    @Test
-    void testKeys() {
-        var keys = getKeys();
-
-        var expectedKeys = new Double[] {
-                0.0,
-                1.0,
-                2.0,
-                4.0,
-                5.0,
-                6.0,
-                8.0,
-        };
+    public static AimTable.AimCharacteristic getSpeakerAimCharacteristic(double distance) {
+      List<Double> keys = table.keySet().stream().sorted().toList();
+      double lowerBound = keys.get(0);
+      double upperBound = keys.get(keys.size() - 1);
 
-        assertArrayEquals(keys.toArray(new Double[0]), expectedKeys);
-    }
-    @Test
-    void testUpperBoundKey() {
-        var keys = getKeys();
-        double upperBound = keys.get(keys.size() - 1);
+      if ((distance < lowerBound)) {
+        AimTable.AimCharacteristic lowerValue = table.get(lowerBound);
 
-        assertEquals(14.0, upperBound);
-    }
+        return new AimTable.AimCharacteristic(lowerValue.angle, lowerValue.rpm);
+      } else if (distance > upperBound) {
+        AimTable.AimCharacteristic upperValue = table.get(upperBound);
 
-    @Test
-    void testLowerBoundKey(){
-        var keys = getKeys();
-        double lowerBound = keys.get(0);
+        return new AimTable.AimCharacteristic(upperValue.angle, upperValue.rpm);
+      }
 
-        assertEquals(-1.0, lowerBound);
-    }
+      System.out.println("Distance " + distance);
 
-    @Test
-    void findAdjacentKeysTest() {
-        NavigableSet<Double> values = new TreeSet<>(getKeys());
-        double distance = 3.0;
-        double lowKey = values.floor(distance);
-        double highKey = values.ceiling(distance);
+      NavigableSet<Double> values = new TreeSet<>(keys);
+      double lowKey = values.floor(distance);
+      double highKey = values.ceiling(distance);
 
-        assertEquals(2.0, lowKey);
-        assertEquals(4.0, highKey);
+      return new AimTable.AimCharacteristic(
+          interpolate(lowKey, table.get(lowKey).angle, highKey, table.get(highKey).angle, distance),
+          interpolate(lowKey, table.get(lowKey).rpm, highKey, table.get(highKey).rpm, distance));
     }
 
-    @Test
-    void interpolateTest() {
-
-        double lowKey = 2.0;
-        double lowValue =  1500;
-        double highKey = 4.0;
-        double highValue = 2500;
-        double x = 3.0;
+    public AimTable.AimCharacteristic getLastAimCharacteristic(double distance) {
+      if (!epsilonEquals(lastDistance, distance, kInterpolationCacheThreshold)) {
+        lastAimCharacteristic = getSpeakerAimCharacteristic(lastDistance);
+        kInterpolationCacheThreshold = lastDistance;
+      }
 
-        assertEquals(2000, interpolate(lowKey, lowValue, highKey, highValue, x));
+      return lastAimCharacteristic;
     }
+  }
 
+  private List<Double> getKeys() {
+    return table.keySet().stream().sorted().toList();
+  }
 
-    private Double[] generateNumbers(int count, int range) {
-            Random rand = new Random();
-            List<Double> nums = new ArrayList<>();
-            for (int i = 0;  i < count; i++) {
-                nums.add(rand.nextDouble(range + 1));
-            }
-            return nums.toArray(new Double[0]);
-    }
-    @Test
-    void testAimTable() {
-            DummyAimTable aimTable = new DummyAimTable();
-        Double[] nums = generateNumbers(20, 20);
+  private static double interpolate(
+      double lowKey, double lowValue, double highKey, double highValue, double x) {
+    double percent = (x - lowKey) / (highKey - lowKey);
 
+    return lowValue + percent * (highValue - lowValue);
+  }
 
-        for (Double num : nums) {
-            AimTable.AimCharacteristic characteristic = aimTable.getSpeakerAimCharacteristic(num);
-            System.out.println(num);
-            System.out.println(characteristic.angle + " " + characteristic.rpm);
-        }
-    }
+  @Test
+  void testKeys() {
+    var keys = getKeys();
 
+    var expectedKeys =
+        new Double[] {
+          0.0, 1.0, 2.0, 4.0, 5.0, 6.0, 8.0,
+        };
 
+    assertArrayEquals(keys.toArray(new Double[0]), expectedKeys);
+  }
+
+  @Test
+  void testUpperBoundKey() {
+    var keys = getKeys();
+    double upperBound = keys.get(keys.size() - 1);
+
+    assertEquals(14.0, upperBound);
+  }
+
+  @Test
+  void testLowerBoundKey() {
+    var keys = getKeys();
+    double lowerBound = keys.get(0);
+
+    assertEquals(-1.0, lowerBound);
+  }
+
+  @Test
+  void findAdjacentKeysTest() {
+    NavigableSet<Double> values = new TreeSet<>(getKeys());
+    double distance = 3.0;
+    double lowKey = values.floor(distance);
+    double highKey = values.ceiling(distance);
+
+    assertEquals(2.0, lowKey);
+    assertEquals(4.0, highKey);
+  }
+
+  @Test
+  void interpolateTest() {
+
+    double lowKey = 2.0;
+    double lowValue = 1500;
+    double highKey = 4.0;
+    double highValue = 2500;
+    double x = 3.0;
+
+    assertEquals(2000, interpolate(lowKey, lowValue, highKey, highValue, x));
+  }
+
+  private Double[] generateNumbers(int count, int range) {
+    Random rand = new Random();
+    List<Double> nums = new ArrayList<>();
+    for (int i = 0; i < count; i++) {
+      nums.add(rand.nextDouble(range + 1));
+    }
+    return nums.toArray(new Double[0]);
+  }
+
+  @Test
+  void testAimTable() {
+    DummyAimTable aimTable = new DummyAimTable();
+    Double[] nums = generateNumbers(20, 20);
+
+    for (Double num : nums) {
+      AimTable.AimCharacteristic characteristic = aimTable.getSpeakerAimCharacteristic(num);
+      System.out.println(num);
+      System.out.println(characteristic.angle + " " + characteristic.rpm);
+    }
+  }
 }
diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
new file mode 100644
index 0000000..e978a5f
--- /dev/null
+++ b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+    "fileName": "NavX.json",
+    "name": "NavX",
+    "version": "2024.1.0",
+    "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://dev.studica.com/maven/release/2024/"
+    ],
+    "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-java",
+            "version": "2024.1.0"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.kauailabs.navx.frc",
+            "artifactId": "navx-frc-cpp",
+            "version": "2024.1.0",
+            "headerClassifier": "headers",
+            "sourcesClassifier": "sources",
+            "sharedLibrary": false,
+            "libName": "navx_frc",
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena",
+                "linuxraspbian",
+                "linuxarm32",
+                "linuxarm64",
+                "linuxx86-64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 0000000..3c74146
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.1.2",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.1.2"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.1.2",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json
new file mode 100644
index 0000000..88a68dd
--- /dev/null
+++ b/vendordeps/Phoenix5.json
@@ -0,0 +1,151 @@
+{
+    "fileName": "Phoenix5.json",
+    "name": "CTRE-Phoenix (v5)",
+    "version": "5.33.0",
+    "frcYear": 2024,
+    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
+    "mavenUrls": [
+        "https://maven.ctr-electronics.com/release/"
+    ],
+    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
+    "requires": [
+        {
+            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
+            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
+            "offlineFileName": "Phoenix6.json",
+            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-java",
+            "version": "5.33.0"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-java",
+            "version": "5.33.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.33.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.33.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "wpiapi-cpp",
+            "version": "5.33.0",
+            "libName": "CTRE_Phoenix_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "api-cpp",
+            "version": "5.33.0",
+            "libName": "CTRE_Phoenix",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix",
+            "artifactId": "cci",
+            "version": "5.33.0",
+            "libName": "CTRE_PhoenixCCI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "5.33.0",
+            "libName": "CTRE_Phoenix_WPISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "api-cpp-sim",
+            "version": "5.33.0",
+            "libName": "CTRE_PhoenixSim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix.sim",
+            "artifactId": "cci-sim",
+            "version": "5.33.0",
+            "libName": "CTRE_PhoenixCCISim",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
new file mode 100644
index 0000000..69a4079
--- /dev/null
+++ b/vendordeps/Phoenix6.json
@@ -0,0 +1,339 @@
+{
+    "fileName": "Phoenix6.json",
+    "name": "CTRE-Phoenix (v6)",
+    "version": "24.1.0",
+    "frcYear": 2024,
+    "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-frc2024-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"
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-java",
+            "version": "24.1.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "tools-sim",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonSRX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simPigeonIMU",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simCANCoder",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProTalonFX",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProCANcoder",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simProPigeon2",
+            "version": "24.1.0",
+            "isJar": false,
+            "skipInvalidPlatforms": true,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "wpiapi-cpp",
+            "version": "24.1.0",
+            "libName": "CTRE_Phoenix6_WPI",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6",
+            "artifactId": "tools",
+            "version": "24.1.0",
+            "libName": "CTRE_PhoenixTools",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena"
+            ],
+            "simMode": "hwsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "wpiapi-cpp-sim",
+            "version": "24.1.0",
+            "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": "24.1.0",
+            "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": "24.1.0",
+            "libName": "CTRE_SimTalonSRX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simTalonFX",
+            "version": "24.1.0",
+            "libName": "CTRE_SimTalonFX",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ],
+            "simMode": "swsim"
+        },
+        {
+            "groupId": "com.ctre.phoenix6.sim",
+            "artifactId": "simVictorSPX",
+            "version": "24.1.0",
+            "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": "24.1.0",
+            "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": "24.1.0",
+            "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": "24.1.0",
+            "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": "24.1.0",
+            "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": "24.1.0",
+            "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/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..46211fc
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,57 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.2.4",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "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": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2024.2.4",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.2.4",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2024.2.4"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.2.4"
+        }
+    ]
+}
\ No newline at end of file