diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644
index 0000000..ce17710
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1,6 @@
+{
+  "download": {
+    "localDir": "C:\\Users\\BeachBots\\Downloads",
+    "serverTeam": "10.76.52.2"
+  }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/FRC_20240119_152551-exported.wpilog b/FRC_20240119_152551-exported.wpilog
new file mode 100644
index 0000000..ce10099
Binary files /dev/null and b/FRC_20240119_152551-exported.wpilog differ
diff --git a/FRC_20240119_152551.wpilog b/FRC_20240119_152551.wpilog
new file mode 100644
index 0000000..0c5f2cb
Binary files /dev/null and b/FRC_20240119_152551.wpilog differ
diff --git a/build.gradle b/build.gradle
index f907561..1ba507c 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id 'com.diffplug.spotless' version '6.20.0'
     id "com.peterabeles.gversion" version "1.10"
 }
diff --git a/src/main/deploy/pathplanner/autos/DriveForward.auto b/src/main/deploy/pathplanner/autos/DriveForward.auto
new file mode 100644
index 0000000..a1f997a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/DriveForward.auto
@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 7.0
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "DriveForward"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BrakeCommand"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/DriveForward.path b/src/main/deploy/pathplanner/paths/DriveForward.path
new file mode 100644
index 0000000..812263e
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/DriveForward.path
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.25,
+        "y": 7.0
+      },
+      "prevControl": {
+        "x": 2.25,
+        "y": 7.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
\ 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
index 3d929c7..a3fe7a4 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -54,7 +54,7 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 0,
     "rotateFast": false
   },
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3047f6a..318f2c5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -15,22 +15,27 @@ public final class Constants {
   public static final class CANConstants {
     // CAN Bus Devices
     /// Drive Train Motors
-    public static final int MOTORFRONTRIGHTID = 11;
-    public static final int MOTORBACKRIGHTID = 12;
-    public static final int MOTORFRONTLEFTID = 13;
-    public static final int MOTORBACKLEFTID = 14;
+    public static final int MOTORFRONTRIGHTID = 14;
+    public static final int MOTORBACKRIGHTID = 13;
+    public static final int MOTORFRONTLEFTID = 12;
+    public static final int MOTORBACKLEFTID = 11;
     /// Arm Motors
     public static final int MOTORARMMAINID = 21;
-    public static final int MOTORARMSECONDARYID = 22;
     /// Shooter Motors
     public static final int MOTORSHOOTERID = 31;
+    /// Lifter Motors
+    public static final int MOTORLIFTERLEFTID = 41;
+    public static final int MOTORLIFTERRIGHTID = 42;
   }
 
   public static final double MAX_SPEED = 0.8;
+  public static final double LIFTERSPEED = 0.5;
 
   // USB Devices
   public static final int CONTROLLERUSBINDEX = 0;
   public static final int FLIGHTSTICKUSBINDEX = 1;
+  // On-Controller joystick deadzone
+  public static final double CONTROLLERDEADZONE = 0.1;
 
   // Game Controller Buttons
   // Now In RobotContainer as Native Triggers.
@@ -44,9 +49,4 @@ public static final class CANConstants {
   public static final int ULTRASONICSHOOTERPORT = 0;
 
   // Digital Ports
-  /// Encoder Ports
-  public static final int DRIVEENCODERLEFTA = 0;
-  public static final int DRIVEENCODERLEFTB = 1;
-  public static final int DRIVEENCODERRIGHTA = 2;
-  public static final int DRIVEENCODERRIGHTB = 3;
 }
diff --git a/src/main/java/frc/robot/DriveConstants.java b/src/main/java/frc/robot/DriveConstants.java
index 7e2b56c..947f9f4 100644
--- a/src/main/java/frc/robot/DriveConstants.java
+++ b/src/main/java/frc/robot/DriveConstants.java
@@ -1,7 +1,6 @@
 package frc.robot;
 
 import com.pathplanner.lib.util.ReplanningConfig;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.util.Units;
 
@@ -13,11 +12,18 @@
  */
 public final class DriveConstants {
   // general drive constants
+  // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2
+  // https://sciencing.com/convert-rpm-linear-speed-8232280.html
   public static final double WHEEL_DIAMETER = Units.inchesToMeters(6); // meters
-  public static final double PULSES_PER_REV = 2048; // resolution of encoder
-  public static final double GEAR_RATIO = 1;
-  public static final double DISTANCE_PER_PULSE =
+  public static final double kTrackwidthMeters = 0.60048;
+  // this is not used and is handled by the rev encoder.
+  public static final double PULSES_PER_REV = 1;
+  public static final double GEAR_RATIO = 8.46; // 8.46:1
+  // basically converted from rotations to to radians to then meters using the wheel diameter.
+  // the diameter is already *2 so we don't need to multiply by 2 again.
+  public static final double POSITION_CONVERSION_RATIO =
       (Math.PI * WHEEL_DIAMETER) / PULSES_PER_REV / GEAR_RATIO;
+  public static final double VELOCITY_CONVERSION_RATIO = POSITION_CONVERSION_RATIO / 60;
   // Kinematic constants
 
   // These characterization values MUST be determined either experimentally or theoretically
@@ -25,17 +31,30 @@ public final class DriveConstants {
   // The Robot Characterization Toolsuite provides a convenient tool for obtaining these
   // values for your robot.
   // Feed Forward Constants
-  public static final double Ks = 0.76856; // volts
-  public static final double Kv = 2.4467; // VoltSecondsPerMeter
-  public static final double Ka = 0.58646; // VoltSecondsSquaredPerMeter
-  public static final SimpleMotorFeedforward FeedForward = new SimpleMotorFeedforward(Ks, Kv, Ka);
+  public static final double ksDriveVolts = 0.16213;
+  public static final double kvDriveVoltSecondsPerMeter = 2.2194;
+  public static final double kaDriveVoltSecondsSquaredPerMeter = 0.33599;
+  // Max speed Constants
+  public static final double kMaxOutputDrive = 1.0;
+  public static final double kMinOutputDrive = -1.0;
   // Feed Back / PID Constants
-  public static final double kPDriveVel = 3.6293;
+  public static final double kPDriveVel = 0.00034388;
+  public static final double kIDriveVel = 0.0;
+  public static final double kDDriveVel = 0.0;
+  public static final double kIzDriveVel = 0.0; // error before integral takes effect
+
+  public static final double kPDrivePos = 5.7745;
+  public static final double kIDrivePos = 0.0;
+  public static final double kDDrivePos = 0.55289;
+  public static final double kIzDrivePos = 0.0; // error before integral takes effect
   // Helper class that converts a chassis velocity (dx and dtheta components) to left and right
   // wheel velocities for a differential drive.
-  public static final double kTrackwidthMeters = 0.60048;
   public static final DifferentialDriveKinematics kDriveKinematics =
       new DifferentialDriveKinematics(kTrackwidthMeters);
   // Default path replanning config. See the API for the options
   public static final ReplanningConfig autoReplanningConfig = new ReplanningConfig();
+
+  // Motor Controller PID Slots
+  public static final int kDrivetrainVelocityPIDSlot = 0;
+  public static final int kDrivetrainPositionPIDSlot = 1;
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7179afb..6fa8e8f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,15 +4,12 @@
 
 package frc.robot;
 
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.cscore.UsbCamera;
-import edu.wpi.first.cscore.VideoSink;
+import edu.wpi.first.wpilibj.DataLogManager;
 // import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import org.littletonrobotics.urcl.URCL;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -25,11 +22,6 @@ public class Robot extends TimedRobot {
 
   private RobotContainer m_robotContainer;
 
-  private UsbCamera camera1;
-  private UsbCamera camera2;
-  private VideoSink mainCameraServer;
-  private int cameraCounter = 2;
-
   /**
    * This function is run when the robot is first started up and should be used for any
    * initialization code.
@@ -40,28 +32,14 @@ public void robotInit() {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
 
-    camera1 = CameraServer.startAutomaticCapture(0);
-    camera2 = CameraServer.startAutomaticCapture(1);
-    mainCameraServer = CameraServer.getServer();
     // Tell both cameras to always stream.
     // camera1.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
     // camera2.setConnectionStrategy(ConnectionStrategy.kKeepOpen);
 
-    // this is to put git info in the dashboard & Logs, uses new 2024 BuildConstants.java
-    String branchName = BuildConstants.GIT_BRANCH;
-    String commitHash = BuildConstants.GIT_SHA;
-    String commitTime = BuildConstants.GIT_DATE;
-    String buildTime = BuildConstants.BUILD_DATE;
-
-    System.out.println("Branch: " + branchName);
-    System.out.println("Commit: " + commitHash);
-    System.out.println("Commit Time: " + commitTime);
-    System.out.println("Build Time: " + buildTime);
-
-    SmartDashboard.putString("Branch", branchName);
-    SmartDashboard.putString("Commit", commitHash);
-    SmartDashboard.putString("CommitTime", commitTime);
-    SmartDashboard.putString("BuildTime", buildTime);
+    if (m_robotContainer.enableAutoProfiling) {
+      DataLogManager.start();
+      URCL.start();
+    }
   }
 
   /**
@@ -78,6 +56,9 @@ public void robotPeriodic() {
     // 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();
+    if (m_robotContainer.enableAutoProfiling) {
+      System.out.println("WARNING, AUTO PROFILE IS ENABLED!");
+    }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -111,20 +92,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer
-        .getCameraButton()
-        .onTrue(
-            new InstantCommand(
-                () -> {
-                  cameraCounter++;
-                  if (cameraCounter % 2 == 0) {
-                    System.out.println("Setting Camera 2");
-                    mainCameraServer.setSource(camera2);
-                  } else {
-                    System.out.println("Setting Camera 1");
-                    mainCameraServer.setSource(camera1);
-                  }
-                }));
   }
 
   /** This function is called periodically during operator control. */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 03db73e..9ad2067 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -10,6 +10,7 @@
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
@@ -20,16 +21,20 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.AimCommand;
 import frc.robot.commands.ArmCommand;
 import frc.robot.commands.BalanceCommand;
 import frc.robot.commands.DefaultDrive;
+import frc.robot.commands.LifterDownCommand;
+import frc.robot.commands.LifterUpCommand;
 import frc.robot.commands.ShooterCommand;
 import frc.robot.commands.StraightCommand;
 import frc.robot.commands.UltrasonicShooterCommand;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.CameraSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.LifterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.UltrasonicSubsystem;
 
@@ -59,6 +64,7 @@ public class RobotContainer {
   private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem);
   private final ArmSubsystem m_armSubsystem = new ArmSubsystem();
   private final ShooterSubsystem m_shooterSubsytem = new ShooterSubsystem();
+  private final LifterSubsystem m_lifterSubsystem = new LifterSubsystem();
   // The robots commands are defined here..
   // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
@@ -73,19 +79,23 @@ public class RobotContainer {
   private final ArmCommand m_armCommand =
       new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY);
   private final ShooterCommand m_shooterCommand = new ShooterCommand(m_shooterSubsytem);
+  private final LifterUpCommand m_lifterUpCommand = new LifterUpCommand(m_lifterSubsystem);
+  private final LifterDownCommand m_lifterDownCommand = new LifterDownCommand(m_lifterSubsystem);
+
   private Command m_driveToSpeaker;
   // Init Buttons
-  private Trigger m_switchCameraButton;
   private Trigger m_balanceButton;
   private Trigger m_straightButton;
-  private Trigger m_brakeButton;
-  private Trigger m_coastButton;
+  private Trigger m_toggleBrakeButton;
+  private Trigger m_lifterUpButton;
+  private Trigger m_lifterDownButton;
   private JoystickButton m_aimButton;
   private JoystickButton m_fireButton;
   private Trigger m_driveToSpeakerButton;
   // Init For Autonomous
   // private RamseteAutoBuilder autoBuilder;
   private SendableChooser<String> autoDashboardChooser = new SendableChooser<String>();
+  public final boolean enableAutoProfiling = false;
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -94,7 +104,15 @@ public RobotContainer() {
     // Setup On the Fly Path Planning
     configureTeleopPaths();
     // Configure the button bindings
-    configureButtonBindings();
+    setupTriggers();
+    // Bind the commands to the triggers
+    if (enableAutoProfiling) {
+      bindDriveSysIDCommands();
+      // bindArmSysIDCommands();
+      // bindShooterSysIDCommands();
+    } else {
+      bindCommands();
+    }
 
     // set default drive command
     m_driveSubsystem.setDefaultCommand(m_defaultDrive);
@@ -110,31 +128,64 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-  private void configureButtonBindings() {
+  private void setupTriggers() {
     // Controller buttons
-    m_switchCameraButton = m_controller1.x();
-    m_brakeButton = m_controller1.a();
-    m_coastButton = m_controller1.b();
+    m_toggleBrakeButton = m_controller1.x();
+    m_lifterUpButton = m_controller1.a();
+    m_lifterDownButton = m_controller1.b();
     m_balanceButton = m_controller1.rightBumper();
     m_straightButton = m_controller1.rightTrigger();
     m_driveToSpeakerButton = m_controller1.y();
+
     // Joystick buttons
     m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON);
     m_fireButton = new JoystickButton(m_flightStick, Constants.FIREBUTTON);
+  }
+
+  private void bindCommands() {
     // commands
     m_balanceButton.whileTrue(m_balanceCommand);
     m_straightButton.whileTrue(m_straightCommand);
     m_aimButton.whileTrue(m_aimCommand);
     m_fireButton.whileTrue(m_shooterCommand);
     m_driveToSpeakerButton.whileTrue(m_driveToSpeaker);
+    m_lifterUpButton.whileTrue(m_lifterUpCommand);
+    m_lifterDownButton.whileTrue(m_lifterDownCommand);
+    m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode()));
+  }
 
-    m_brakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetBrakemode()));
-    m_coastButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SetCoastmode()));
+  private void bindDriveSysIDCommands() {
+    m_controller1.a().whileTrue(m_driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1.b().whileTrue(m_driveSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_driveSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
+  }
+
+  private void bindArmSysIDCommands() {
+    m_controller1.a().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1.b().whileTrue(m_armSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
+  }
+
+  private void bindShooterSysIDCommands() {
+    m_controller1
+        .a()
+        .whileTrue(m_shooterSubsytem.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    m_controller1
+        .b()
+        .whileTrue(m_shooterSubsytem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    m_controller1.x().whileTrue(m_shooterSubsytem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    m_controller1.y().whileTrue(m_shooterSubsytem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop()));
   }
 
   private void initializeAutonomous() {
     // Network Table Routine Options
     autoDashboardChooser.setDefaultOption("Auto With Balancing", "FullAuto");
+    autoDashboardChooser.setDefaultOption("DriveForward", "DriveForward");
     autoDashboardChooser.addOption("End at cones", "EndAtCones");
     autoDashboardChooser.addOption("Do Nothing", "DoNothing");
     SmartDashboard.putData(autoDashboardChooser);
@@ -144,6 +195,8 @@ private void initializeAutonomous() {
     // NamedCommands.registerCommand("A", new PathFollowingCommand(m_driveSubsystem,
     // pathGroup.get(0)));
     NamedCommands.registerCommand("BalanceRobot", m_balanceCommand);
+    NamedCommands.registerCommand(
+        "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode()));
 
     // autoBuilder =
     //     new RamseteAutoBuilder(
@@ -196,11 +249,6 @@ public DefaultDrive getDefaultDrive() {
     return m_defaultDrive;
   }
 
-  // to swap camera type.
-  public Trigger getCameraButton() {
-    return m_switchCameraButton;
-  }
-
   // for future SmartDashboard uses.
   public CommandXboxController getController1() {
     return this.m_controller1;
diff --git a/src/main/java/frc/robot/commands/AimCommand.java b/src/main/java/frc/robot/commands/AimCommand.java
index fe6a50d..8ee9406 100644
--- a/src/main/java/frc/robot/commands/AimCommand.java
+++ b/src/main/java/frc/robot/commands/AimCommand.java
@@ -53,10 +53,9 @@ public void execute() {
                   Units.degreesToRadians(CamResult.getBestTarget().getPitch()))
               - m_cameraSubsystem.frontCameraGoalRangeMeters;
       // turn and move towards target.
-      m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
+      // m_driveSubsystem.driveAndTurn(m_driveSubsystem.getYaw(), angleGoal, distanceFromTarget);
       // we reset the angle everytime as the target could change / move.
       m_driveSubsystem.turnSetGoal(angleGoal);
-      m_driveSubsystem.distanceSetGoal(distanceFromTarget);
     } else {
       SmartDashboard.putBoolean("CameraTargetDetected", false);
       SmartDashboard.putNumber("CameraTargetPitch", 0.0);
@@ -68,7 +67,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     m_driveSubsystem.turnResetPID(); // we clear the PID turn controller.
-    m_driveSubsystem.distanceResetPID(); // we clear the distance PID contoller too.
+    m_driveSubsystem.stop(); // end execution of on board PID.
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java
index e40e835..5e2e4db 100644
--- a/src/main/java/frc/robot/commands/ArmCommand.java
+++ b/src/main/java/frc/robot/commands/ArmCommand.java
@@ -4,9 +4,12 @@
 
 package frc.robot.commands;
 
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.ShooterState;
 import frc.robot.subsystems.ArmSubsystem;
+import frc.robot.utils.HelperFunctions;
 import java.util.function.DoubleSupplier;
 
 /** An example command that uses an example subsystem. */
@@ -16,7 +19,7 @@ public class ArmCommand extends Command {
 
   private final ShooterState m_shooterState;
   private final DoubleSupplier m_yAxis;
-  private final double kMaxRotationsPerInput = 1000;
+  private final double kMaxRadiansPerInput = Units.degreesToRadians(5);
 
   /**
    * Creates a new ExampleCommand.
@@ -38,12 +41,14 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (m_yAxis.getAsDouble() != 0.0) {
-      m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRotationsPerInput);
+    if (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) {
+      m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput);
 
     } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) {
       m_ArmSubsystem.lowerArm();
       m_shooterState.setLowered();
+    } else {
+      m_ArmSubsystem.stop();
     }
   }
 
diff --git a/src/main/java/frc/robot/commands/DefaultDrive.java b/src/main/java/frc/robot/commands/DefaultDrive.java
index 7b075f8..8b03842 100644
--- a/src/main/java/frc/robot/commands/DefaultDrive.java
+++ b/src/main/java/frc/robot/commands/DefaultDrive.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.utils.HelperFunctions;
 import java.util.function.DoubleSupplier;
 
 /** The default drive command that uses the drive subsystem. */
@@ -38,9 +39,12 @@ public void initialize() {}
   public void execute() {
     // we include a limit on the drivers speed for safety.
     // Additonally the axis's on the
-    this.m_driveSubsystem.tankDrive(
-        Constants.MAX_SPEED * m_left_y.getAsDouble(),
-        Constants.MAX_SPEED * m_right_y.getAsDouble());
+    if (!HelperFunctions.inDeadzone(m_left_y.getAsDouble(), Constants.CONTROLLERDEADZONE)
+        || !HelperFunctions.inDeadzone(m_right_y.getAsDouble(), Constants.CONTROLLERDEADZONE)) {
+      this.m_driveSubsystem.tankDrive(
+          Constants.MAX_SPEED * m_left_y.getAsDouble(),
+          Constants.MAX_SPEED * m_right_y.getAsDouble());
+    }
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/LifterDownCommand.java b/src/main/java/frc/robot/commands/LifterDownCommand.java
new file mode 100644
index 0000000..0ccaf25
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LifterDownCommand.java
@@ -0,0 +1,51 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.LifterSubsystem;
+
+/** Lifter Command using the Lifter Subsystem. */
+public class LifterDownCommand extends Command {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final LifterSubsystem m_lifterSubsystem;
+
+  private final double speed = Constants.LIFTERSPEED;
+
+  /**
+   * Creates a new LifterDownCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public LifterDownCommand(LifterSubsystem l_subsystem) {
+    m_lifterSubsystem = l_subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(l_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_lifterSubsystem.leftDown(speed);
+    m_lifterSubsystem.rightDown(speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_lifterSubsystem.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/LifterUpCommand.java b/src/main/java/frc/robot/commands/LifterUpCommand.java
new file mode 100644
index 0000000..b7c35a0
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LifterUpCommand.java
@@ -0,0 +1,51 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.subsystems.LifterSubsystem;
+
+/** Lifter Command using the Lifter Subsystem. */
+public class LifterUpCommand extends Command {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final LifterSubsystem m_lifterSubsystem;
+
+  private final double speed = Constants.LIFTERSPEED;
+
+  /**
+   * Creates a new LifterUpCommand.
+   *
+   * @param l_subsystem The subsystem used by this command.
+   */
+  public LifterUpCommand(LifterSubsystem l_subsystem) {
+    m_lifterSubsystem = l_subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(l_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_lifterSubsystem.leftUp(speed);
+    m_lifterSubsystem.rightUp(speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_lifterSubsystem.stop();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java
index 1366a38..2d14be6 100644
--- a/src/main/java/frc/robot/commands/ShooterCommand.java
+++ b/src/main/java/frc/robot/commands/ShooterCommand.java
@@ -30,8 +30,9 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // TODO: Implement ShooterCommand
+    // TODO: Finish Implementing ShooterCommand
     System.out.println("ShooterCommand Activated");
+    m_shooterSubsystem.SpinShooterFull();
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
index 145d361..34ef757 100644
--- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java
@@ -4,93 +4,168 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.CANConstants;
 
 public class ArmSubsystem extends SubsystemBase {
-  private final CANSparkMax m_armMotorMain, m_armMotorSecondary;
+  private final CANSparkMax m_armMotorMain;
   private final SparkPIDController m_armMainPIDController;
-  private RelativeEncoder m_MainEncoder;
-  private final double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, kLoweredArmPosition;
+  private final RelativeEncoder m_MainEncoder;
+  private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput;
+  private final double kminArmAngle =
+      Units.degreesToRadians(
+          0); // this is needed for the feedforward control, basically min angle relative to flat on
+  // floor.
+  private static double kDt = 0.02; // 20ms (update rate for wpilib)
+  private final double ksArmVolts = 0.0;
+  private final double kgArmGravityGain = 0.0;
+  private final double kvArmVoltSecondsPerMeter = 0.0;
+  private final double kaArmVoltSecondsSquaredPerMeter = 0.0;
+  private final double kLoweredArmPositionRadians = Units.degreesToRadians(45);
+  private final double karmMaxVelocity = 2; // m/s
+  private final double karmMaxAcceleration = 1; // m/s^2
+  // general drive constants
+  // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2
+  // https://sciencing.com/convert-rpm-linear-speed-8232280.html
+  private final double kGearRatio = 1; // TBD
+  // basically converted from rotations to radians by multiplying by 2 pi, then adjusting for the
+  // gear ratio by dividing by the gear ratio.
+  // remember that 2pi radians in 360 degrees.
+  private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio;
+  private final ArmFeedforward m_armFeedforward =
+      new ArmFeedforward(
+          ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter);
+  private final TrapezoidProfile m_armTrapezoidProfile =
+      new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration));
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+  private boolean m_stopped = true;
+
+  // setup SysID for auto profiling
+  private final SysIdRoutine m_sysIdRoutine;
 
   /** Creates a new ArmSubsystem. */
   public ArmSubsystem() {
     // create the arm motors
     m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless);
-    m_armMotorSecondary =
-        new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless);
 
     // set the idle mode to brake
     m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake);
-    m_armMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake);
-
-    // set the secondary motor to follow the main motor
-    m_armMotorSecondary.follow(m_armMotorMain);
 
     // connect to built in PID controller
     m_armMainPIDController = m_armMotorMain.getPIDController();
     // allow us to read the encoder
     m_MainEncoder = m_armMotorMain.getEncoder();
+    // setup the encoders
+    m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio);
     // PID coefficients
     kP = 0.1;
     kI = 1e-4;
     kD = 1;
     kIz = 0;
-    kFF = 0;
     kMaxOutput = 1;
     kMinOutput = -1;
-    kLoweredArmPosition = 0;
 
     // set PID coefficients
     m_armMainPIDController.setP(kP);
     m_armMainPIDController.setI(kI);
     m_armMainPIDController.setD(kD);
     m_armMainPIDController.setIZone(kIz);
-    m_armMainPIDController.setFF(kFF);
     m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput);
+
+    // setup SysID for auto profiling
+    m_sysIdRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(
+                (voltage) -> this.setVoltage(voltage),
+                null, // No log consumer, since data is recorded by URCL
+                this));
+  }
+
+  public void setVoltage(Measure<Voltage> voltage) {
+    m_armMotorMain.setVoltage(voltage.in(Volts));
+  }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
+
+  public double getAverageEncoderPosition() {
+    // get the average encoder position
+    return (m_MainEncoder.getPosition());
   }
 
   /*
-   * Move arm relative to current position
+   * Move arm a certain number of radians relative to current position
    */
-  public void MoveArmRelative(double rotations) {
-    rotations = rotations + m_MainEncoder.getPosition();
+  public void MoveArmRelative(double radians) {
+    radians = radians + getAverageEncoderPosition();
     // update the PID controller with current encoder position
-    MoveArmToPosition(rotations);
+    MoveArmToPosition(radians);
   }
 
   public void lowerArm() {
     // move to set lowered arm position
-    MoveArmToPosition(kLoweredArmPosition);
+    MoveArmToPosition(kLoweredArmPositionRadians);
   }
 
   /*
-   * Move arm to global position
+   * Move arm to global position (by updating goal)
    */
-  public void MoveArmToPosition(double rotations) {
-    // update the PID controller with current encoder position
-    m_armMainPIDController.setReference(rotations, CANSparkBase.ControlType.kPosition);
+  public void MoveArmToPosition(double radians) {
+    m_stopped = false;
+    // update the motion profile with new goal
+    // add minimum starting angle to the target angle to get the real angle
+    double total_radians = radians + kminArmAngle;
+    m_goal = new TrapezoidProfile.State(total_radians, 0); // set the goal
   }
 
   /*
    * attempt to hold arm at current location
    */
   public void stop() {
-    // update the PID controller with current encoder position
-    MoveArmToPosition(m_MainEncoder.getPosition());
+    if (!m_stopped) {
+      // update the PID controller with current encoder position
+      MoveArmToPosition(getAverageEncoderPosition());
+      m_stopped = true;
+    }
   }
 
-  public void zero() {
+  public void zeroEncoders() {
     m_MainEncoder.setPosition(0);
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+
+    // update the setpoint
+    m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal);
+
+    // Call the controller and feedforward with the target position and velocity
+    m_armMainPIDController.setReference(
+        m_setpoint.position,
+        CANSparkBase.ControlType.kPosition,
+        0,
+        m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity));
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index e757ad9..69506c9 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -2,27 +2,34 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 // import motor & frc dependencies
-import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import static edu.wpi.first.units.Units.Volts;
+
 import com.kauailabs.navx.frc.AHRS;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
 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.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 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 frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.CANConstants;
 import frc.robot.DriveConstants;
 
@@ -32,16 +39,35 @@ public class DriveSubsystem extends SubsystemBase {
   private final AHRS m_Gyro;
 
   // motors
-  private final WPI_VictorSPX m_backLeft; // Main / Master Motor for Left
-  private final WPI_VictorSPX m_frontLeft; // Slave Motor for Left (Follow Master)
-  private final WPI_VictorSPX m_backRight; // Main / Master Motor for Right
-  private final WPI_VictorSPX m_frontRight; // Slave Motor for Right (Follow Master)
+  private final CANSparkMax m_backLeft; // Main / Master Motor for Left
+  private final CANSparkMax m_frontLeft; // Slave Motor for Left (Follow Master)
+  private final CANSparkMax m_backRight; // Main / Master Motor for Right
+  private final CANSparkMax m_frontRight; // Slave Motor for Right (Follow Master)
   // Main drive function
   private final DifferentialDrive m_ddrive;
 
   // Encoders
-  private final Encoder m_encoderLeft;
-  private final Encoder m_encoderRight;
+  private final RelativeEncoder m_encoderBackLeft;
+  private final RelativeEncoder m_encoderFrontLeft;
+  private final RelativeEncoder m_encoderBackRight;
+  private final RelativeEncoder m_encoderFrontRight;
+
+  // Motor PID Controllers
+  private final SparkPIDController m_backLeftPIDController;
+  private final SparkPIDController m_backRightPIDController;
+
+  // Current Idle mode
+  private boolean isBrakeMode;
+
+  // motor feedforward
+  SimpleMotorFeedforward m_driveFeedForward =
+      new SimpleMotorFeedforward(
+          DriveConstants.ksDriveVolts,
+          DriveConstants.kvDriveVoltSecondsPerMeter,
+          DriveConstants.kaDriveVoltSecondsSquaredPerMeter);
+
+  // ex:
+  // https://github.com/REVrobotics/SPARK-MAX-Examples/blob/master/Java/Read%20Encoder%20Values/src/main/java/frc/robot/Robot.java
 
   // Odometry class for tracking robot pose (position on field)
   private final DifferentialDrivePoseEstimator m_driveOdometry;
@@ -50,8 +76,8 @@ public class DriveSubsystem extends SubsystemBase {
   static final double turn_P = 0.1;
   static final double turn_I = 0.00;
   static final double turn_D = 0.00;
-  static final double MaxTurnRateDegPerS = 100;
-  static final double MaxTurnAccelerationDegPerSSquared = 300;
+  static final double MaxTurnRateDegPerS = 20;
+  static final double MaxTurnAccelerationDegPerSSquared = 50;
   static final double TurnToleranceDeg = 3; // max diff in degrees
   static final double TurnRateToleranceDegPerS = 10; // degrees per second
   // false when inactive, true when active / a target is set.
@@ -65,25 +91,6 @@ public class DriveSubsystem extends SubsystemBase {
           turn_D,
           new TrapezoidProfile.Constraints(MaxTurnRateDegPerS, MaxTurnAccelerationDegPerSSquared));
 
-  // Distance PID / MoveDistance
-  static final double distance_P = 0.1;
-  static final double distance_I = 0.00;
-  static final double distance_D = 0.00;
-  static final double distanceMaxSpeed = 1; // m/s
-  static final double distanceMaxAcceleration = 2; // m/s^2
-  static final double DistanceTolerance = 0.01; // max diff in meters
-  static final double DistanceSpeedTolerance = 0.1; // ignore if velocity is below. (m)
-  // false when inactive, true when active / a target is set.
-  private boolean distanceControllerEnabled = false;
-  private double distanceThrottleRate; // This value will be updated by the PID Controller
-  // pid controller for "MoveDistance"
-  private final ProfiledPIDController m_distanceController =
-      new ProfiledPIDController(
-          distance_P,
-          distance_I,
-          distance_D,
-          new TrapezoidProfile.Constraints(distanceMaxSpeed, distanceMaxAcceleration));
-
   // Balance PID / AutoBalance
   static final double balance_P = 0.0625; // 1/16
   static final double balance_I = 0.00;
@@ -106,60 +113,79 @@ public class DriveSubsystem extends SubsystemBase {
   // track robot field location for dashboard
   private Field2d field = new Field2d();
 
+  // setup SysID for auto profiling
+  private final SysIdRoutine m_sysIdRoutine;
+
+  private boolean gyroZeroPending = true;
+
   /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     // Init gyro
     m_Gyro = new AHRS(SPI.Port.kMXP);
     // init motors
     // rio means built into the roboRIO
-    m_backLeft = new WPI_VictorSPX(CANConstants.MOTORBACKLEFTID);
-    m_frontLeft = new WPI_VictorSPX(CANConstants.MOTORFRONTLEFTID);
-    m_frontRight = new WPI_VictorSPX(CANConstants.MOTORFRONTRIGHTID);
-    m_backRight = new WPI_VictorSPX(CANConstants.MOTORBACKRIGHTID);
+    m_backLeft = new CANSparkMax(CANConstants.MOTORBACKLEFTID, CANSparkMax.MotorType.kBrushless);
+    m_frontLeft = new CANSparkMax(CANConstants.MOTORFRONTLEFTID, CANSparkMax.MotorType.kBrushless);
+    m_frontRight =
+        new CANSparkMax(CANConstants.MOTORFRONTRIGHTID, CANSparkMax.MotorType.kBrushless);
+    m_backRight = new CANSparkMax(CANConstants.MOTORBACKRIGHTID, CANSparkMax.MotorType.kBrushless);
+
+    // invert right side
+    m_backRight.setInverted(true);
+    m_frontRight.setInverted(true);
 
     // setup main and secondary motors
     m_frontLeft.follow(m_backLeft); // set front left to follow back left
     m_frontRight.follow(m_backRight); // set front right to follow back right
 
-    m_backRight.setInverted(true); // invert right side
-
     // init drive function
     m_ddrive = new DifferentialDrive(m_backLeft, m_backRight);
 
-    // init Encoders
-    m_encoderLeft = new Encoder(Constants.DRIVEENCODERLEFTA, Constants.DRIVEENCODERLEFTB);
-    m_encoderRight = new Encoder(Constants.DRIVEENCODERRIGHTA, Constants.DRIVEENCODERRIGHTB);
-    m_encoderRight.setReverseDirection(true); // invert left to match drive
+    // init Encoders, we use all 4 encoders even though only 2 are used in feedback to decrease
+    // error
+    m_encoderBackLeft = m_backLeft.getEncoder();
+    m_encoderFrontLeft = m_frontLeft.getEncoder();
+    m_encoderBackRight = m_backRight.getEncoder();
+    m_encoderFrontRight = m_frontRight.getEncoder();
+    // Encoders inverted with motors
+
+    // init PID Controllers
+    m_backLeftPIDController = m_backLeft.getPIDController();
+    m_backRightPIDController = m_backRight.getPIDController();
 
     // configure encoders
-    m_encoderLeft.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE); // distance in meters
-    m_encoderRight.setDistancePerPulse(DriveConstants.DISTANCE_PER_PULSE); // distance in meters
-    m_encoderLeft.setSamplesToAverage(5);
-    m_encoderRight.setSamplesToAverage(5);
-    m_encoderLeft.setMinRate(0.1); // min rate to be determined moving
-    m_encoderRight.setMinRate(0.1); // min rate to be determined moving
+    // RPM TO m/s
+    m_encoderBackLeft.setVelocityConversionFactor(DriveConstants.VELOCITY_CONVERSION_RATIO);
+    m_encoderBackRight.setVelocityConversionFactor(DriveConstants.VELOCITY_CONVERSION_RATIO);
+    m_encoderFrontLeft.setVelocityConversionFactor(DriveConstants.VELOCITY_CONVERSION_RATIO);
+    m_encoderFrontRight.setVelocityConversionFactor(DriveConstants.VELOCITY_CONVERSION_RATIO);
+    // rotations to meters
+    m_encoderBackLeft.setPositionConversionFactor(DriveConstants.POSITION_CONVERSION_RATIO);
+    m_encoderBackRight.setPositionConversionFactor(DriveConstants.POSITION_CONVERSION_RATIO);
+    m_encoderFrontLeft.setPositionConversionFactor(DriveConstants.POSITION_CONVERSION_RATIO);
+    m_encoderFrontRight.setPositionConversionFactor(DriveConstants.POSITION_CONVERSION_RATIO);
     resetEncoders();
-    // Every time this function is called, A dollar is taken out of Jack's savings. Aka do it more.
-    resetGyro();
 
-    // configure Odemetry
-    m_driveOdometry =
-        new DifferentialDrivePoseEstimator(
-            DriveConstants.kDriveKinematics,
-            getRotation2d(),
-            m_encoderLeft.getDistance(),
-            m_encoderRight.getDistance(),
-            new Pose2d());
+    // setup PID controllers
+    configureMotorPIDControllers();
 
+    // Configure RIO PID Controllers
     // config turn pid controller.
     m_turnController.enableContinuousInput(-180.0f, 180.0f);
     m_turnController.setTolerance(TurnToleranceDeg, TurnRateToleranceDegPerS);
-    // config distance pid controller
-    m_distanceController.setTolerance(DistanceTolerance, DistanceSpeedTolerance);
     // this is the target pitch/ tilt error.
     m_balanceController.setGoal(0);
     m_balanceController.setTolerance(BalanceToleranceDeg); // max error in degrees
 
+    // configure Odemetry
+    m_driveOdometry =
+        new DifferentialDrivePoseEstimator(
+            DriveConstants.kDriveKinematics,
+            getRotation2d(),
+            getPositionLeft(),
+            getPositionRight(),
+            new Pose2d());
+
     // Setup Base AutoBuilder (Autonomous)
     AutoBuilder.configureRamsete(
         this::getPose, // Pose2d supplier
@@ -182,6 +208,83 @@ public DriveSubsystem() {
         );
 
     SmartDashboard.putData("Field", field); // add field to dashboard
+
+    // setup SysID for auto profiling
+    m_sysIdRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(
+                (voltage) -> this.setVoltage(voltage, voltage),
+                null, // No log consumer, since data is recorded by URCL
+                this));
+  }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
+  }
+
+  private void configureMotorPIDControllers() {
+    // setup velocity PID controllers (used by auto)
+    m_backLeftPIDController.setP(
+        DriveConstants.kPDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setP(
+        DriveConstants.kPDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backLeftPIDController.setI(
+        DriveConstants.kIDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setI(
+        DriveConstants.kIDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backLeftPIDController.setD(
+        DriveConstants.kDDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setD(
+        DriveConstants.kDDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backLeftPIDController.setIZone(
+        DriveConstants.kIzDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setIZone(
+        DriveConstants.kIzDriveVel, DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backLeftPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainVelocityPIDSlot);
+    m_backRightPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainVelocityPIDSlot);
+
+    // setup position PID controllers (used when we manually path find)
+    m_backLeftPIDController.setP(
+        DriveConstants.kPDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setP(
+        DriveConstants.kPDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backLeftPIDController.setI(
+        DriveConstants.kIDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setI(
+        DriveConstants.kIDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backLeftPIDController.setD(
+        DriveConstants.kDDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setD(
+        DriveConstants.kDDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backLeftPIDController.setIZone(
+        DriveConstants.kIzDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setIZone(
+        DriveConstants.kIzDrivePos, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backLeftPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setOutputRange(
+        DriveConstants.kMinOutputDrive,
+        DriveConstants.kMaxOutputDrive,
+        DriveConstants.kDrivetrainPositionPIDSlot);
+  }
+
+  public void setVoltage(Measure<Voltage> rightVoltage, Measure<Voltage> leftVoltage) {
+    m_backLeft.setVoltage(leftVoltage.in(Volts));
+    m_backRight.setVoltage(rightVoltage.in(Volts));
+    m_ddrive.feed();
   }
 
   // default tank drive function
@@ -209,45 +312,10 @@ public void balanceCorrection(double gyroPitchAngle) {
     }
   }
 
-  public void distanceResetPID() {
-    /** This should be run when stopping a pid command. */
-    distanceControllerEnabled = false;
-  }
-
-  public void distanceSetGoal(double targetDistance) {
-    m_distanceController.setGoal(AverageDistance() + targetDistance);
-  }
-
-  private void calculateDistanceRate(double targetDistance) {
-    if (!distanceControllerEnabled) {
-      m_distanceController.reset(AverageDistance());
-      m_distanceController.setGoal(AverageDistance() + targetDistance);
-      distanceControllerEnabled = true;
-    }
-    distanceThrottleRate =
-        MathUtil.clamp(m_distanceController.calculate(AverageDistance()), -1.0, 1.0);
-  }
-
-  public void driveToDistance(double targetDistance) {
-    this.calculateDistanceRate(targetDistance);
-    double leftStickValue = turnRotateToAngleRate;
-    double rightStickValue = turnRotateToAngleRate;
-    if (!m_distanceController.atGoal()) {
-      this.tankDrive(leftStickValue, rightStickValue);
-    }
-  }
-
-  public void driveAndTurn(double gyroYawAngle, double TargetAngleDegrees, double targetDistance) {
-    /*
-    This lets you set a gyro angle and a distance you need to travel.
-    this should not be used in auto mode.
-     */
-    this.calcuateAngleRate(gyroYawAngle, TargetAngleDegrees);
-    this.calculateDistanceRate(targetDistance);
-    double leftStickValue = distanceThrottleRate + turnRotateToAngleRate;
-    double rightStickValue = distanceThrottleRate - turnRotateToAngleRate;
-    if (!m_distanceController.atGoal() || !m_turnController.atGoal()) {
-      this.tankDrive(leftStickValue, rightStickValue);
+  public void driveToRelativePosition(double targetPosition) {
+    if (targetPosition < 0.1) { // less then 0.1 meters, do nothing, pervents feedback loop
+      double totalPosition = this.AverageDistance() + targetPosition;
+      this.driveToPosition(totalPosition);
     }
   }
 
@@ -309,7 +377,7 @@ public void driveStraight(
    * This function can return our robots DiffernentialDriveWheelSpeeds, which is the speed of each side of the robot.
    */
   public DifferentialDriveWheelSpeeds getWheelSpeeds() {
-    return new DifferentialDriveWheelSpeeds(m_encoderLeft.getRate(), m_encoderRight.getRate());
+    return new DifferentialDriveWheelSpeeds(getVelocityLeft(), getVelocityRight());
   }
 
   /*
@@ -331,9 +399,30 @@ public void setSpeeds(ChassisSpeeds speeds) {
    * This function can set our robots DifferentialDriveWheelSpeeds, which is the speed of each side of the robot.
    */
   public void setWheelVelocities(DifferentialDriveWheelSpeeds speeds) {
-    // TODO: Implement, this makes everything more accurate and allows auto to work.
-    // https://docs.wpilib.org/en/stable/docs/software/advanced-controls/trajectories/ramsete.html#ramsete-in-the-command-based-framework
-    // we should use in controller velocity PID
+    // get left and right speeds in m/s, and run through feedforward to get feedforward voltage
+    // offset
+    double leftSpeed = speeds.leftMetersPerSecond;
+    double rightSpeed = speeds.rightMetersPerSecond;
+    // set to position of motors
+    m_backLeftPIDController.setReference(
+        leftSpeed,
+        CANSparkBase.ControlType.kVelocity,
+        DriveConstants.kDrivetrainVelocityPIDSlot,
+        m_driveFeedForward.calculate(leftSpeed));
+    m_backRightPIDController.setReference(
+        rightSpeed,
+        CANSparkBase.ControlType.kVelocity,
+        DriveConstants.kDrivetrainVelocityPIDSlot,
+        m_driveFeedForward.calculate(rightSpeed));
+  }
+
+  // in meters, use averageDistance() to get average distance traveled, as an offset to set this
+  // function.
+  public void driveToPosition(final double NewPosition) {
+    m_backLeftPIDController.setReference(
+        NewPosition, CANSparkBase.ControlType.kPosition, DriveConstants.kDrivetrainPositionPIDSlot);
+    m_backRightPIDController.setReference(
+        NewPosition, CANSparkBase.ControlType.kPosition, DriveConstants.kDrivetrainPositionPIDSlot);
   }
 
   public Pose2d getPose() {
@@ -345,26 +434,38 @@ public void updateVisionPose(Pose2d visionRobotPose, double timestamp) {
   }
 
   public void resetEncoders() {
-    m_encoderLeft.reset();
-    m_encoderRight.reset();
+    m_encoderBackLeft.setPosition(0);
+    m_encoderFrontLeft.setPosition(0);
+    m_encoderBackRight.setPosition(0);
+    m_encoderFrontRight.setPosition(0);
   }
 
   public double AverageDistance() {
-    return (m_encoderLeft.getDistance() + m_encoderRight.getDistance()) / 2;
+    return (getPositionLeft() + getPositionRight()) / 2;
   }
 
   public void SetBrakemode() {
-    m_backLeft.setNeutralMode(NeutralMode.Brake);
-    m_backRight.setNeutralMode(NeutralMode.Brake);
-    m_frontLeft.setNeutralMode(NeutralMode.Brake);
-    m_frontRight.setNeutralMode(NeutralMode.Brake);
+    m_backLeft.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    m_backRight.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    m_frontLeft.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    m_frontRight.setIdleMode(CANSparkMax.IdleMode.kBrake);
+    isBrakeMode = true;
   }
 
   public void SetCoastmode() {
-    m_backLeft.setNeutralMode(NeutralMode.Coast);
-    m_backRight.setNeutralMode(NeutralMode.Coast);
-    m_frontLeft.setNeutralMode(NeutralMode.Coast);
-    m_frontRight.setNeutralMode(NeutralMode.Coast);
+    m_backLeft.setIdleMode(CANSparkMax.IdleMode.kCoast);
+    m_backRight.setIdleMode(CANSparkMax.IdleMode.kCoast);
+    m_frontLeft.setIdleMode(CANSparkMax.IdleMode.kCoast);
+    m_frontRight.setIdleMode(CANSparkMax.IdleMode.kCoast);
+    isBrakeMode = false;
+  }
+
+  public void SwitchBrakemode() {
+    if (this.isBrakeMode) {
+      this.SetCoastmode();
+    } else {
+      this.SetBrakemode();
+    }
   }
 
   /**
@@ -374,8 +475,7 @@ public void SetCoastmode() {
    */
   public void resetPose(Pose2d pose) {
     resetEncoders();
-    m_driveOdometry.resetPosition(
-        getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance(), pose);
+    m_driveOdometry.resetPosition(getRotation2d(), getPositionLeft(), getPositionRight(), pose);
   }
 
   public void stop() {
@@ -404,23 +504,40 @@ public void resetGyro() {
     m_Gyro.reset();
   }
 
+  public double getVelocityLeft() {
+    return (m_encoderBackLeft.getVelocity() + m_encoderFrontLeft.getVelocity()) / 2;
+  }
+
+  public double getVelocityRight() {
+    return (m_encoderBackRight.getVelocity() + m_encoderFrontRight.getVelocity()) / 2;
+  }
+
+  public double getPositionLeft() {
+    return (m_encoderBackLeft.getPosition() + m_encoderFrontLeft.getPosition()) / 2;
+  }
+
+  public double getPositionRight() {
+    return (m_encoderBackRight.getPosition() + m_encoderFrontRight.getPosition()) / 2;
+  }
+
   @Override
   public void periodic() {
+    if (gyroZeroPending && !m_Gyro.isCalibrating()) {
+      resetGyro();
+      gyroZeroPending = false;
+    }
     // This method will be called once per scheduler run
-    SmartDashboard.putNumber("Left Encoder Speed (M/s)", this.m_encoderLeft.getRate());
-    SmartDashboard.putNumber("Right Encoder Speed (M/s)", this.m_encoderRight.getRate());
-    SmartDashboard.putNumber("Distance L", this.m_encoderLeft.getDistance());
-    SmartDashboard.putNumber("Distance R", this.m_encoderRight.getDistance());
-    SmartDashboard.putNumber("Current Robot Location X axis", getPose().getX());
-    SmartDashboard.putNumber("Current Robot Location Y axis", getPose().getY());
-    SmartDashboard.putNumber("Current Robot Rotation", getPose().getRotation().getDegrees());
+    DifferentialDriveWheelSpeeds wheelSpeeds = this.getWheelSpeeds();
+    SmartDashboard.putNumber("Left Encoder Speed (M/s)", wheelSpeeds.leftMetersPerSecond);
+    SmartDashboard.putNumber("Right Encoder Speed (M/s)", wheelSpeeds.rightMetersPerSecond);
+    SmartDashboard.putNumber("Distance L", this.getPositionLeft());
+    SmartDashboard.putNumber("Distance R", this.getPositionRight());
     SmartDashboard.putNumber("Average Distance Traveled", AverageDistance());
     SmartDashboard.putNumber("Current Gyro Pitch", getPitch());
     SmartDashboard.putNumber("Current Gyro Yaw", getYaw());
     SmartDashboard.putBoolean("Gyro Calibrating", m_Gyro.isCalibrating());
     // Update the odometry in the periodic block
-    m_driveOdometry.update(
-        getRotation2d(), m_encoderLeft.getDistance(), m_encoderRight.getDistance());
+    m_driveOdometry.update(getRotation2d(), getPositionLeft(), getPositionRight());
     field.setRobotPose(getPose());
   }
 
diff --git a/src/main/java/frc/robot/subsystems/LifterSubsystem.java b/src/main/java/frc/robot/subsystems/LifterSubsystem.java
new file mode 100644
index 0000000..938d386
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LifterSubsystem.java
@@ -0,0 +1,51 @@
+// 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;
+
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.CANConstants;
+
+public class LifterSubsystem extends SubsystemBase {
+  private final CANSparkMax m_left; // Motor for Left
+  private final CANSparkMax m_right; // Motor for right
+
+  /** Creates a new LifterSubsystem. */
+  public LifterSubsystem() {
+    m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless);
+    m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless);
+  }
+
+  public void leftUp(double speed) {
+    m_left.set(speed);
+  }
+
+  public void leftDown(double speed) {
+    m_left.set(-speed);
+  }
+
+  public void rightUp(double speed) {
+    m_right.set(speed);
+  }
+
+  public void rightDown(double speed) {
+    m_right.set(-speed);
+  }
+
+  public void stop() {
+    m_left.set(0);
+    m_right.set(0);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 9e4e8a4..5cdff47 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -4,18 +4,46 @@
 
 package frc.robot.subsystems;
 
+import static edu.wpi.first.units.Units.Volts;
+
 import com.revrobotics.CANSparkBase;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.CANConstants;
 
 public class ShooterSubsystem extends SubsystemBase {
   private final CANSparkMax m_ShooterMotorMain;
   private final SparkPIDController m_ShooterMainPIDController;
   private RelativeEncoder m_ShooterMainEncoder;
-  private final double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM;
+  private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput, kMaxSpeed;
+  // general drive constants
+  // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2
+  // https://sciencing.com/convert-rpm-linear-speed-8232280.html
+  private final double kWheelDiameter = Units.inchesToMeters(6); // meters
+  private final double kGearRatio = 1; // TBD
+  // basically converted from rotations to to radians to then meters using the wheel diameter.
+  // the diameter is already *2 so we don't need to multiply by 2 again.
+  private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60;
+
+  // setup feedforward
+  private final double ksShooterVolts = 0.0;
+  private final double kvDriveVoltSecondsPerMeter = 0.0;
+  private final double kaDriveVoltSecondsSquaredPerMeter = 0.0;
+
+  SimpleMotorFeedforward m_shooterFeedForward =
+      new SimpleMotorFeedforward(
+          ksShooterVolts, kvDriveVoltSecondsPerMeter, kaDriveVoltSecondsSquaredPerMeter);
+
+  // setup SysID for auto profiling
+  private final SysIdRoutine m_sysIdRoutine;
 
   /** Creates a new ShooterSubsystem. */
   public ShooterSubsystem() {
@@ -30,30 +58,50 @@ public ShooterSubsystem() {
 
     // allow us to read the encoder
     m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder();
+    m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio);
     // PID coefficients
     kP = 6e-5;
     kI = 0;
     kD = 0;
     kIz = 0;
-    kFF = 0.000015;
     kMaxOutput = 1;
     kMinOutput = -1;
-    maxRPM = 5700;
+    kMaxSpeed = 5;
 
     // set PID coefficients
     m_ShooterMainPIDController.setP(kP);
     m_ShooterMainPIDController.setI(kI);
     m_ShooterMainPIDController.setD(kD);
     m_ShooterMainPIDController.setIZone(kIz);
-    m_ShooterMainPIDController.setFF(kFF);
     m_ShooterMainPIDController.setOutputRange(kMinOutput, kMaxOutput);
+    // setup SysID for auto profiling
+    m_sysIdRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(
+                (voltage) -> this.setVoltage(voltage),
+                null, // No log consumer, since data is recorded by URCL
+                this));
+  }
+
+  public void setVoltage(Measure<Voltage> voltage) {
+    m_ShooterMotorMain.setVoltage(voltage.in(Volts));
+  }
+
+  public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.quasistatic(direction);
+  }
+
+  public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+    return m_sysIdRoutine.dynamic(direction);
   }
 
   /*
-   * Spin shooter at a given RPM
+   * Spin shooter at a given Speed (M/S)
    */
-  public void SpinShooter(double RPM) {
-    m_ShooterMainPIDController.setReference(RPM, CANSparkBase.ControlType.kVelocity);
+  public void SpinShooter(double speed) {
+    m_ShooterMainPIDController.setReference(
+        speed, CANSparkBase.ControlType.kVelocity, 0, m_shooterFeedForward.calculate(speed));
   }
 
   /*
@@ -64,25 +112,25 @@ public void StopShooter() {
   }
 
   /*
-   * Spin Shooter at max RPM
+   * Spin Shooter at max Speed
    */
   public void SpinShooterFull() {
-    SpinShooter(maxRPM);
+    SpinShooter(kMaxSpeed);
   }
 
   /*
-   * Check if shooter is at a given RPM
+   * Check if shooter is at a given Speed
    */
-  public Boolean isAtRPMTolerance(double RPM) {
-    return (m_ShooterMainEncoder.getVelocity() > RPM - 100
-        && m_ShooterMainEncoder.getVelocity() < RPM + 100);
+  public Boolean isAtSpeedTolerance(double speed) {
+    return (m_ShooterMainEncoder.getVelocity() > speed - 0.1
+        && m_ShooterMainEncoder.getVelocity() < speed + 0.1);
   }
 
   /*
-   * Check if shooter is at max RPM
+   * Check if shooter is at max Speed
    */
-  public Boolean isAtMaxRPM() {
-    return isAtRPMTolerance(maxRPM);
+  public Boolean isAtMaxSpeed() {
+    return isAtSpeedTolerance(kMaxSpeed);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/utils/HelperFunctions.java b/src/main/java/frc/robot/utils/HelperFunctions.java
new file mode 100644
index 0000000..c003f67
--- /dev/null
+++ b/src/main/java/frc/robot/utils/HelperFunctions.java
@@ -0,0 +1,8 @@
+package frc.robot.utils;
+
+public final class HelperFunctions {
+
+  public static boolean inDeadzone(double value, double deadzone) {
+    return Math.abs(value) < deadzone;
+  }
+}
diff --git a/sysid.json b/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
index 3c74146..cae1363 100644
--- a/vendordeps/PathplannerLib.json
+++ b/vendordeps/PathplannerLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.2",
+    "version": "2024.1.6",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.2"
+            "version": "2024.1.6"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.2",
+            "version": "2024.1.6",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json
deleted file mode 100644
index 88a68dd..0000000
--- a/vendordeps/Phoenix5.json
+++ /dev/null
@@ -1,151 +0,0 @@
-{
-    "fileName": "Phoenix5.json",
-    "name": "CTRE-Phoenix (v5)",
-    "version": "5.33.0",
-    "frcYear": 2024,
-    "uuid": "ab676553-b602-441f-a38d-f1296eff6537",
-    "mavenUrls": [
-        "https://maven.ctr-electronics.com/release/"
-    ],
-    "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json",
-    "requires": [
-        {
-            "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
-            "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6.  Please add the Phoenix 6 vendordep before adding Phoenix 5.",
-            "offlineFileName": "Phoenix6.json",
-            "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json"
-        }
-    ],
-    "javaDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-java",
-            "version": "5.33.0"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-java",
-            "version": "5.33.0"
-        }
-    ],
-    "jniDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.33.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.33.0",
-            "isJar": false,
-            "skipInvalidPlatforms": true,
-            "validPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ],
-    "cppDependencies": [
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "wpiapi-cpp",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix_WPI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "api-cpp",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix",
-            "artifactId": "cci",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixCCI",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "linuxathena"
-            ],
-            "simMode": "hwsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "wpiapi-cpp-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_Phoenix_WPISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "api-cpp-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixSim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        },
-        {
-            "groupId": "com.ctre.phoenix.sim",
-            "artifactId": "cci-sim",
-            "version": "5.33.0",
-            "libName": "CTRE_PhoenixCCISim",
-            "headerClassifier": "headers",
-            "sharedLibrary": true,
-            "skipInvalidPlatforms": true,
-            "binaryPlatforms": [
-                "windowsx86-64",
-                "linuxx86-64",
-                "osxuniversal"
-            ],
-            "simMode": "swsim"
-        }
-    ]
-}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json
deleted file mode 100644
index 69a4079..0000000
--- a/vendordeps/Phoenix6.json
+++ /dev/null
@@ -1,339 +0,0 @@
-{
-    "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/URCL.json b/vendordeps/URCL.json
new file mode 100644
index 0000000..d2c0e48
--- /dev/null
+++ b/vendordeps/URCL.json
@@ -0,0 +1,65 @@
+{
+    "fileName": "URCL.json",
+    "name": "URCL",
+    "version": "2024.0.1",
+    "frcYear": "2024",
+    "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c",
+    "mavenUrls": [
+        "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.0.1"
+    ],
+    "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-java",
+            "version": "2024.0.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-driver",
+            "version": "2024.0.1",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-cpp",
+            "version": "2024.0.1",
+            "libName": "URCL",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-driver",
+            "version": "2024.0.1",
+            "libName": "URCLDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 8a0e424..d4b0b67 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -1,57 +1,57 @@
 {
-  "fileName": "photonlib.json",
-  "name": "photonlib",
-  "version": "v2024.1.2",
-  "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.1.2",
-      "libName": "photonlib",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "linuxathena",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    },
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photontargeting-cpp",
-      "version": "v2024.1.2",
-      "libName": "photontargeting",
-      "headerClassifier": "headers",
-      "sharedLibrary": true,
-      "skipInvalidPlatforms": true,
-      "binaryPlatforms": [
-        "windowsx86-64",
-        "linuxathena",
-        "linuxx86-64",
-        "osxuniversal"
-      ]
-    }
-  ],
-  "javaDependencies": [
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photonlib-java",
-      "version": "v2024.1.2"
-    },
-    {
-      "groupId": "org.photonvision",
-      "artifactId": "photontargeting-java",
-      "version": "v2024.1.2"
-    }
-  ]
-}
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.2.3",
+    "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.3",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.2.3",
+            "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.3"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.2.3"
+        }
+    ]
+}
\ No newline at end of file