diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 38ba8c2..ec8b274 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -26,6 +26,7 @@
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 import com.frcteam3255.components.swerve.SN_SwerveConstants;
 
+import au.grapplerobotics.LaserCan.TimingBudget;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -50,6 +51,16 @@
 public final class Constants {
   public static final Measure<Voltage> MAX_VOLTAGE = Units.Volts.of(12);
 
+  public static final String[] PDH_DEVICES = {
+      "Back Right Steer (0)", "Back Right Drive (1)", "Radio Auxiliary Power (2)", "GrappleCAN (3)", "(4)", "LEDS (5)",
+      "Transfer Roller (6)", "Shooter Pivot(7)",
+      "Back Left Steer (8)", "Back Left Drive (9)",
+      "Front Left Steer (10)", "Front Left Drive (11)", "(12)", "Drainpipe (13)", "Elevator (14)", "Shooter Right (15)",
+      "Shooter Left (16)", "Intake (17)", "Front Right Steer (18)", "Front Right Drive (19)",
+      "Swerve CANCoders & Pigeon (20)", "Radio POE (21)", "RoboRIO Power (22)", "(23)" };
+
+  public static final boolean ENABLE_PDH_LOGGING = true;
+
   public static class constControllers {
     public static final double DRIVER_LEFT_STICK_DEADBAND = 0.05;
     public static final boolean SILENCE_JOYSTICK_WARNINGS = true;
@@ -59,6 +70,8 @@ public static class constControllers {
 
     public static final double DRIVER_GP_COLLECTED_RUMBLE = 0.3;
     public static final double OPERATOR_GP_COLLECTED_RUMBLE = 0.3;
+
+    public static final boolean SOLO_DRIVER = false;
   }
 
   public static class constDrivetrain {
@@ -113,14 +126,18 @@ public static class constDrivetrain {
     public static final Measure<Angle> AT_ROTATION_TOLERANCE = Units.Degrees.of(5);
 
     public static final boolean DRIVE_ENABLE_CURRENT_LIMITING = true;
-    public static final double DRIVE_CURRENT_THRESH = 40;
-    public static final double DRIVE_CURRENT_LIMIT = 30;
-    public static final double DRIVE_CURRENT_TIME_THRESH = 0.1;
+    public static final double DRIVE_CURRENT_LIMIT = 90;
+    public static final double DRIVE_CURRENT_TIME_THRESH = 0.5;
+
+    public static final boolean DRIVE_ENABLE_STATOR_CURRENT_LIMITING = true;
+    public static final double DRIVE_STATOR_CURRENT_LIMIT = 110;
+
+    public static final double DRIVE_PEAK_FORWARD_VOLTAGE = 12.0;
+    public static final double DRIVE_PEAK_REVERSE_VOLTAGE = -12.0;
 
     public static final boolean STEER_ENABLE_CURRENT_LIMITING = true;
-    public static final double STEER_CURRENT_THRESH = 40;
-    public static final double STEER_CURRENT_LIMIT = 30;
-    public static final double STEER_CURRENT_TIME_THRESH = 0.1;
+    public static final double STEER_CURRENT_THRESH = 60;
+    public static final double STEER_CURRENT_TIME_THRESH = 0.2;
 
   }
 
@@ -619,6 +636,9 @@ public static class constIntake {
 
   public static class constTransfer {
     public static final boolean NOTE_SENSOR_INVERT = true;
+    public static final Measure<Distance> PIECE_DETECTED_DIST_THRESH = Units.Millimeters.of(320);
+    public static final TimingBudget TIMING_BUDGET = TimingBudget.TIMING_BUDGET_20MS;
+
     public static final InvertedValue MOTOR_INVERT = InvertedValue.Clockwise_Positive;
     public static final NeutralModeValue FEEDER_NEUTRAL_MODE = NeutralModeValue.Brake;
 
@@ -686,7 +706,7 @@ public static class constLEDs {
         (LED_NUMBER / 2) - 5,
         TwinklePercent.Percent100,
         8);
-    public static final TwinkleAnimation DISABLED_COLOR_2 = new TwinkleAnimation(255, 100, 0, 0, 0.5,
+    public static final TwinkleAnimation DISABLED_COLOR_2 = new TwinkleAnimation(255, 255, 0, 0, 0.5,
         (LED_NUMBER / 2),
         TwinklePercent.Percent100, (LED_NUMBER / 2) + 3);
 
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index ac0ce8f..5d9ef7d 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -50,12 +50,14 @@ public void robotInit() {
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     RobotContainer.AddVisionMeasurement().schedule();
+    RobotContainer.logPDHValues();
   }
 
   @Override
   public void disabledInit() {
     m_robotContainer.subStateMachine.setRobotState(RobotState.NONE);
     m_robotContainer.subStateMachine.setTargetState(TargetState.PREP_NONE);
+    m_robotContainer.setMegaTag2(false);
 
     Shooter.hasZeroed = false;
     Elevator.hasZeroed = false;
@@ -80,6 +82,7 @@ public void disabledExit() {
 
   @Override
   public void autonomousInit() {
+    m_robotContainer.setMegaTag2(true);
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     bothSubsystemsZeroed = Shooter.hasZeroed && Elevator.hasZeroed;
 
@@ -105,6 +108,7 @@ public void autonomousExit() {
   @Override
   public void teleopInit() {
     bothSubsystemsZeroed = Shooter.hasZeroed && Elevator.hasZeroed;
+    m_robotContainer.setMegaTag2(true);
 
     RobotContainer.checkForManualZeroing().cancel();
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4fea79e..75cfc17 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -12,8 +12,11 @@
 
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj.PowerDistribution;
+import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
@@ -24,6 +27,7 @@
 import frc.robot.Constants.constLEDs;
 import frc.robot.Constants.constShooter;
 import frc.robot.RobotMap.mapControllers;
+import frc.robot.RobotPreferences.prefVision;
 import frc.robot.commands.AddVisionMeasurement;
 import frc.robot.commands.Drive;
 import frc.robot.commands.GamePieceRumble;
@@ -56,7 +60,6 @@ public class RobotContainer {
   private final SN_XboxController conOperator = new SN_XboxController(mapControllers.OPERATOR_USB);
   private final SN_XboxController conTestOperator = new SN_XboxController(mapControllers.TEST_OPERATOR_USB);
 
-  public final static StateMachine subStateMachine = new StateMachine();
   private final static Climber subClimber = new Climber();
   private final static Drivetrain subDrivetrain = new Drivetrain();
   private final static Elevator subElevator = new Elevator();
@@ -65,9 +68,12 @@ public class RobotContainer {
   private final static Shooter subShooter = new Shooter();
   private final static Transfer subTransfer = new Transfer();
   private final static Limelight subLimelight = new Limelight();
+  public final static StateMachine subStateMachine = new StateMachine(subClimber, subDrivetrain,
+      subElevator, subIntake, subLEDs, subTransfer, subShooter);
 
+  private final Trigger falseTrigger = new Trigger(() -> false);
   private final Trigger gamePieceStoredTrigger = new Trigger(() -> subTransfer.getGamePieceStored());
-  private final Trigger gamePieceCollectedTrigger = new Trigger(() -> subIntake.getGamePieceCollected());
+  private final Trigger gamePieceCollectedTrigger = falseTrigger;
 
   private final BooleanSupplier readyToShootOperator = (() -> (subDrivetrain.isDrivetrainFacingSpeaker()
       || subDrivetrain.isDrivetrainFacingShuffle())
@@ -93,22 +99,35 @@ public class RobotContainer {
 
   SendableChooser<Command> autoChooser = new SendableChooser<>();
 
+  private static PowerDistribution PDH = new PowerDistribution(1, ModuleType.kRev);
+
   public RobotContainer() {
     conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
 
-    subDrivetrain
-        .setDefaultCommand(
-            new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX, conDriver.axis_RightX,
-                conDriver.btn_LeftBumper, conDriver.btn_RightBumper, conDriver.btn_RightTrigger,
-                conDriver.btn_Y, conDriver.btn_B, conDriver.btn_A,
-                conDriver.btn_X, conDriver.btn_LeftTrigger));
+    if (constControllers.SOLO_DRIVER) {
+      subDrivetrain
+          .setDefaultCommand(
+              new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX,
+                  conDriver.axis_RightX,
+                  conDriver.btn_LeftBumper, falseTrigger, falseTrigger,
+                  falseTrigger, falseTrigger, falseTrigger,
+                  falseTrigger, falseTrigger));
+
+    } else {
+      subDrivetrain
+          .setDefaultCommand(
+              new Drive(subDrivetrain, subStateMachine, conDriver.axis_LeftY, conDriver.axis_LeftX,
+                  conDriver.axis_RightX,
+                  conDriver.btn_LeftBumper, conDriver.btn_RightBumper, conDriver.btn_RightTrigger,
+                  conDriver.btn_Y, conDriver.btn_B, conDriver.btn_A,
+                  conDriver.btn_X, conDriver.btn_LeftTrigger));
+    }
 
     // - Manual Triggers -
     gamePieceStoredTrigger
         .onTrue(Commands
             .deferredProxy(
-                () -> subStateMachine.tryState(RobotState.STORE_FEEDER, subStateMachine, subClimber, subDrivetrain,
-                    subElevator, subIntake, subLEDs, subTransfer, subShooter))
+                () -> subStateMachine.tryState(RobotState.STORE_FEEDER))
             .andThen(Commands.deferredProxy(
                 () -> subStateMachine.tryTargetState(subStateMachine, subIntake, subLEDs, subShooter, subTransfer,
                     subElevator, subDrivetrain))))
@@ -148,13 +167,16 @@ public RobotContainer() {
     conDriver.setTriggerPressThreshold(0.2);
 
     NamedCommands.registerCommand("Intaking", Commands.deferredProxy(
-        () -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator,
-            subIntake, subLEDs, subTransfer, subShooter))
+        () -> subStateMachine.tryState(RobotState.INTAKING))
         .until(gamePieceStoredTrigger));
 
     SmartDashboard.putNumber("Preload Only Delay", 0);
 
-    configureDriverBindings(conDriver);
+    if (constControllers.SOLO_DRIVER) {
+      configureSoloDriverBindings(conDriver);
+    } else {
+      configureDriverBindings(conDriver);
+    }
     configureOperatorBindings(conOperator);
     configureTestBindings(conTestOperator);
 
@@ -167,90 +189,148 @@ private void configureDriverBindings(SN_XboxController controller) {
         Commands.runOnce(() -> subDrivetrain.resetPoseToPose(constField.getFieldPositions().get()[6].toPose2d())));
 
     // Intake from source
-    controller.btn_East.whileTrue(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKE_SOURCE,
-        subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter)))
-        .onFalse(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber,
-            subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
+    controller.btn_East.whileTrue(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKE_SOURCE)))
+        .onFalse(Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.NONE))
             .unless(() -> comIntakeSource.getIntakeSourceGamePiece()));
   }
 
+  private void configureSoloDriverBindings(SN_XboxController controller) {
+    // Reset Pose
+    controller.btn_Start.onTrue(
+        Commands.runOnce(() -> subDrivetrain.resetPoseToPose(constField.getFieldPositions().get()[6].toPose2d())));
+
+    // -- States --
+    // Intake
+    controller.btn_LeftTrigger
+        .whileTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.INTAKING)))
+        .onFalse(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.NONE))
+            .unless(gamePieceStoredTrigger));
+
+    // Shoot
+    controller.btn_RightTrigger.whileTrue(
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING)))
+        .onFalse(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.NONE))
+            .unless(gamePieceStoredTrigger));
+
+    // Prep with vision
+    controller.btn_RightBumper.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)))
+        .onTrue(Commands
+            .deferredProxy(
+                () -> subStateMachine.tryState(RobotState.PREP_VISION)));
+
+    // Ejecting
+    controller.btn_Back.whileTrue(Commands.deferredProxy(
+        () -> subStateMachine.tryState(RobotState.EJECTING)))
+        .onFalse(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.NONE)));
+
+    // Prep spike
+    controller.btn_X.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPIKE)))
+        .onTrue(
+            Commands.deferredProxy(
+                () -> subStateMachine.tryState(RobotState.PREP_SPIKE)));
+
+    controller.btn_B.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_AMP)))
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.PREP_AMP)));
+
+    controller.btn_Y.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SUB_BACKWARDS)))
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS)));
+
+    // "Unalive Shooter"
+    controller.btn_A.onTrue(
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE))
+            .alongWith(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_NONE))))
+        .onFalse(Commands.runOnce(() -> subShooter.setShootingNeutralOutput()));
+
+    // Prep subwoofer
+    controller.btn_South.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPEAKER)))
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.PREP_SPEAKER)));
+
+    // Prep wing
+    controller.btn_North.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_WING)))
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.PREP_WING)));
+
+    // Prep shuffle
+    controller.btn_West.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SHUFFLE)))
+        .onTrue(Commands.deferredProxy(
+            () -> subStateMachine.tryState(RobotState.PREP_SHUFFLE)));
+
+    // Game Piece Override
+    controller.btn_East.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true))
+        .alongWith(Commands.runOnce(() -> subStateMachine.setRobotState(RobotState.STORE_FEEDER))));
+
+  }
+
   private void configureOperatorBindings(SN_XboxController controller) {
     // -- States --
     // Intake
     controller.btn_LeftTrigger
         .whileTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber, subDrivetrain, subElevator,
-                subIntake, subLEDs, subTransfer, subShooter)))
+            () -> subStateMachine.tryState(RobotState.INTAKING)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
-                subIntake, subLEDs, subTransfer, subShooter))
+            () -> subStateMachine.tryState(RobotState.NONE))
             .unless(gamePieceStoredTrigger));
 
     // Shoot
     controller.btn_RightTrigger.whileTrue(
-        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING,
-            subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter)))
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.SHOOTING)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
-                subIntake, subLEDs, subTransfer, subShooter))
+            () -> subStateMachine.tryState(RobotState.NONE))
             .unless(gamePieceStoredTrigger));
 
     // Prep with vision
     controller.btn_RightBumper.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)))
         .onTrue(Commands
             .deferredProxy(
-                () -> subStateMachine.tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain,
-                    subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+                () -> subStateMachine.tryState(RobotState.PREP_VISION)));
 
     // Ejecting
     controller.btn_LeftBumper.whileTrue(Commands.deferredProxy(
-        () -> subStateMachine.tryState(RobotState.EJECTING, subStateMachine, subClimber, subDrivetrain, subElevator,
-            subIntake, subLEDs, subTransfer, subShooter)))
+        () -> subStateMachine.tryState(RobotState.EJECTING)))
         .onFalse(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator,
-                subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.NONE)));
 
     // Prep spike
     controller.btn_X.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPIKE)))
         .onTrue(
             Commands.deferredProxy(
-                () -> subStateMachine.tryState(RobotState.PREP_SPIKE, subStateMachine, subClimber, subDrivetrain,
-                    subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+                () -> subStateMachine.tryState(RobotState.PREP_SPIKE)));
 
     controller.btn_B.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_AMP)))
         .onTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.PREP_AMP, subStateMachine, subClimber, subDrivetrain, subElevator,
-                subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.PREP_AMP)));
 
     controller.btn_Y.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SUB_BACKWARDS)))
         .onTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS, subStateMachine, subClimber, subDrivetrain,
-                subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.PREP_SUB_BACKWARDS)));
 
     // "Unalive Shooter"
     controller.btn_A.onTrue(
-        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE,
-            subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.PREP_NONE))
             .alongWith(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_NONE))))
         .onFalse(Commands.runOnce(() -> subShooter.setShootingNeutralOutput()));
 
     // Prep subwoofer
     controller.btn_South.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SPEAKER)))
         .onTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.PREP_SPEAKER, subStateMachine, subClimber, subDrivetrain,
-                subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.PREP_SPEAKER)));
 
     // Prep wing
     controller.btn_North.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_WING)))
         .onTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.PREP_WING, subStateMachine, subClimber, subDrivetrain,
-                subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.PREP_WING)));
 
     // Prep shuffle
     controller.btn_West.onTrue(Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_SHUFFLE)))
         .onTrue(Commands.deferredProxy(
-            () -> subStateMachine.tryState(RobotState.PREP_SHUFFLE, subStateMachine, subClimber, subDrivetrain,
-                subElevator, subIntake, subLEDs, subTransfer, subShooter)));
+            () -> subStateMachine.tryState(RobotState.PREP_SHUFFLE)));
 
     // Game Piece Override
     controller.btn_East.onTrue(Commands.runOnce(() -> subTransfer.setGamePieceCollected(true))
@@ -333,6 +413,43 @@ public static Command AddVisionMeasurement() {
         .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming).ignoringDisable(true);
   }
 
+  public void setMegaTag2(boolean setMegaTag2) {
+
+    if (setMegaTag2) {
+      subDrivetrain.swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(
+          prefVision.megaTag2StdDevsPosition.getValue(),
+          prefVision.megaTag2StdDevsPosition.getValue(),
+          prefVision.megaTag2StdDevsHeading.getValue()));
+    } else {
+      // Use MegaTag 1
+      subDrivetrain.swervePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(
+          prefVision.megaTag1StdDevsPosition.getValue(),
+          prefVision.megaTag1StdDevsPosition.getValue(),
+          prefVision.megaTag1StdDevsHeading.getValue()));
+    }
+    subLimelight.setMegaTag2(setMegaTag2);
+  }
+
+  // -- PDH --
+  /**
+   * Updates the values supplied to the PDH to SmartDashboard. Should be called
+   * periodically.
+   */
+  public static void logPDHValues() {
+    SmartDashboard.putNumber("PDH/Input Voltage", PDH.getVoltage());
+    SmartDashboard.putBoolean("PDH/Is Switchable Channel Powered", PDH.getSwitchableChannel());
+    SmartDashboard.putNumber("PDH/Total Current", PDH.getTotalCurrent());
+    SmartDashboard.putNumber("PDH/Total Power", PDH.getTotalPower());
+    SmartDashboard.putNumber("PDH/Total Energy", PDH.getTotalEnergy());
+
+    if (Constants.ENABLE_PDH_LOGGING) {
+      for (int i = 0; i < Constants.PDH_DEVICES.length; i++) {
+        SmartDashboard.putNumber("PDH/" + Constants.PDH_DEVICES[i] + " Current", PDH.getCurrent(i));
+      }
+    }
+  }
+
+  // -- LEDS --
   public void setDisabledLEDs() {
     subLEDs.setLEDAnimation(constLEDs.DISABLED_COLOR_1, 0);
     subLEDs.setLEDAnimation(constLEDs.DISABLED_COLOR_2, 1);
diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java
index 39ae974..760d544 100644
--- a/src/main/java/frc/robot/RobotMap.java
+++ b/src/main/java/frc/robot/RobotMap.java
@@ -62,7 +62,7 @@ public static class mapClimber {
   // MOTORS: 50 -> 59
   public static class mapTransfer {
     public static final int TRANSFER_MOTOR_CAN = 50;
-    public static final int NOTE_SENSOR_DIO = 0;
+    public static final int NOTE_SENSOR_CAN = 51;
   }
 
   public static class mapLEDs {
diff --git a/src/main/java/frc/robot/RobotPreferences.java b/src/main/java/frc/robot/RobotPreferences.java
index a5cf46b..8f7a3d4 100644
--- a/src/main/java/frc/robot/RobotPreferences.java
+++ b/src/main/java/frc/robot/RobotPreferences.java
@@ -85,23 +85,18 @@ public static final class prefElevator {
   }
 
   public static final class prefVision {
-    /**
-     * <p>
-     * Pose estimator standard deviation for vision data using Multi-tag
-     * <p>
-     * <b>Units:</b> Meters
-     */
-    public static final SN_DoublePreference multiTagStdDevsPosition = new SN_DoublePreference(
-        "multiTagStdDevsPosition", 0.7);
+    public static final SN_DoublePreference megaTag2StdDevsPosition = new SN_DoublePreference(
+        "megaTag2StdDevsPosition", 0.7);
+
+    public static final SN_DoublePreference megaTag2StdDevsHeading = new SN_DoublePreference(
+        "megaTag2StdDevsHeading", 9999999);
+
+    public static final SN_DoublePreference megaTag1StdDevsPosition = new SN_DoublePreference(
+        "megaTag1StdDevsPosition", 0.3);
+
+    public static final SN_DoublePreference megaTag1StdDevsHeading = new SN_DoublePreference(
+        "megaTag1StdDevsHeading", 0.1);
 
-    /**
-     * <p>
-     * Pose estimator standard deviation for vision data using Multi-Tag
-     * </p>
-     * <b>Units:</b> Radians
-     */
-    public static final SN_DoublePreference multiTagStdDevsHeading = new SN_DoublePreference(
-        "multiTagStdDevsHeading", 9999999);
   }
 
   public static final class prefIntake {
diff --git a/src/main/java/frc/robot/commands/Autos/Centerline.java b/src/main/java/frc/robot/commands/Autos/Centerline.java
index e6a1937..745078d 100644
--- a/src/main/java/frc/robot/commands/Autos/Centerline.java
+++ b/src/main/java/frc/robot/commands/Autos/Centerline.java
@@ -62,8 +62,7 @@ public Centerline(StateMachine subStateMachine, Climber subClimber, Drivetrain s
             () -> subDrivetrain.resetPoseToPose(getInitialPose().get())),
 
         // -- PRELOAD --
-        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
-            subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
             .until(() -> subTransfer.getGamePieceStored()).withTimeout(3),
 
         Commands.deferredProxy(shootSequence),
diff --git a/src/main/java/frc/robot/commands/Autos/PreloadOnly.java b/src/main/java/frc/robot/commands/Autos/PreloadOnly.java
index 82f4a8d..0032eec 100644
--- a/src/main/java/frc/robot/commands/Autos/PreloadOnly.java
+++ b/src/main/java/frc/robot/commands/Autos/PreloadOnly.java
@@ -78,22 +78,19 @@ public PreloadOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain
 
         Commands.waitSeconds(secondSupplier.getAsDouble()),
 
-        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
-            subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
             .until(() -> subTransfer.getGamePieceStored()),
 
         Commands.waitUntil(() -> subShooter.readyToShoot()),
 
         // Shoot! (Ends when we don't have a game piece anymore)
         Commands.deferredProxy(() -> subStateMachine
-            .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
-                subTransfer, subShooter)
+            .tryState(RobotState.SHOOTING)
             .until(() -> !subTransfer.getGamePieceStored())),
 
         // Reset subsystems to chill
         Commands.deferredProxy(() -> subStateMachine
-            .tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
-                subTransfer, subShooter)));
+            .tryState(RobotState.NONE)));
   }
 
   public Supplier<Pose2d> getInitialPose() {
diff --git a/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java b/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java
index 90af5eb..d38be64 100644
--- a/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java
+++ b/src/main/java/frc/robot/commands/Autos/PreloadTaxi.java
@@ -51,8 +51,7 @@ public class PreloadTaxi extends SequentialCommandGroup {
       Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)),
 
       Commands.deferredProxy(() -> subStateMachine
-          .tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
-              subTransfer, subShooter)
+          .tryState(RobotState.PREP_VISION)
           .repeatedly().until(() -> subShooter.readyToShoot())),
 
       Commands.runOnce(() -> subDrivetrain.drive(
@@ -62,14 +61,12 @@ public class PreloadTaxi extends SequentialCommandGroup {
 
       // Shoot! (Ends when we don't have a game piece anymore)
       Commands.deferredProxy(() -> subStateMachine
-          .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
-              subLEDs, subTransfer, subShooter)
+          .tryState(RobotState.SHOOTING)
           .until(() -> !subTransfer.getGamePieceStored())),
 
       // Reset subsystems to chill
       Commands.deferredProxy(() -> subStateMachine
-          .tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake, subLEDs,
-              subTransfer, subShooter)),
+          .tryState(RobotState.NONE)),
 
       Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)));
 
@@ -91,8 +88,7 @@ public PreloadTaxi(StateMachine subStateMachine, Climber subClimber, Drivetrain
             getInitialPose().get())),
 
         // -- PRELOAD --
-        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING, subStateMachine, subClimber,
-            subDrivetrain, subElevator, subIntake, subLEDs, subTransfer, subShooter))
+        Commands.deferredProxy(() -> subStateMachine.tryState(RobotState.INTAKING))
             .until(() -> subTransfer.getGamePieceStored()),
 
         Commands.deferredProxy(() -> shootSequence),
diff --git a/src/main/java/frc/robot/commands/Autos/ShootSequence.java b/src/main/java/frc/robot/commands/Autos/ShootSequence.java
index 7b626cc..bc70f6a 100644
--- a/src/main/java/frc/robot/commands/Autos/ShootSequence.java
+++ b/src/main/java/frc/robot/commands/Autos/ShootSequence.java
@@ -52,8 +52,7 @@ public ShootSequence(StateMachine subStateMachine, Climber subClimber, Drivetrai
 
             Commands.parallel(
                 Commands.deferredProxy(() -> subStateMachine
-                    .tryState(RobotState.PREP_VISION, subStateMachine, subClimber, subDrivetrain, subElevator,
-                        subIntake, subLEDs, subTransfer, subShooter)
+                    .tryState(RobotState.PREP_VISION)
                     .repeatedly()),
 
                 Commands.runOnce(() -> subDrivetrain.drive(
@@ -65,17 +64,14 @@ public ShootSequence(StateMachine subStateMachine, Climber subClimber, Drivetrai
 
             // Shoot! (Ends when we don't have a game piece anymore)
             Commands.deferredProxy(() -> subStateMachine
-                .tryState(RobotState.SHOOTING, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
-                    subLEDs, subTransfer, subShooter)
+                .tryState(RobotState.SHOOTING)
                 .until(() -> !subTransfer.getGamePieceStored())),
 
             Commands.waitSeconds(constShooter.AUTO_PREP_NONE_DELAY.in(Units.Seconds)),
 
             // Reset subsystems to chill
             Commands.deferredProxy(() -> subStateMachine
-                .tryState(RobotState.NONE, subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
-                    subLEDs, subTransfer,
-                    subShooter)))
+                .tryState(RobotState.NONE)))
             .unless(() -> !subTransfer.getGamePieceStored()));
 
   }
diff --git a/src/main/java/frc/robot/commands/Autos/WingOnly.java b/src/main/java/frc/robot/commands/Autos/WingOnly.java
index 55c2353..8374a79 100644
--- a/src/main/java/frc/robot/commands/Autos/WingOnly.java
+++ b/src/main/java/frc/robot/commands/Autos/WingOnly.java
@@ -63,10 +63,6 @@ public WingOnly(StateMachine subStateMachine, Climber subClimber, Drivetrain sub
     shootSequence = () -> new ShootSequence(subStateMachine, subClimber, subDrivetrain, subElevator, subIntake,
         subLEDs, subTransfer, subShooter, readyToShoot);
     addCommands(
-        // Resetting pose
-        Commands.runOnce(() -> subDrivetrain.resetPoseToPose(
-            getInitialPose().get())),
-
         // -- PRELOAD --
         Commands.runOnce(() -> subStateMachine.setTargetState(TargetState.PREP_VISION)),
 
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index e6fef60..46f8579 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -8,6 +8,7 @@
 
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.frcteam3255.components.swerve.SN_SuperSwerve;
 import com.frcteam3255.components.swerve.SN_SwerveModule;
 import com.pathplanner.lib.util.PIDConstants;
@@ -26,6 +27,7 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Robot;
@@ -41,6 +43,7 @@ public class Drivetrain extends SN_SuperSwerve {
   private static TalonFXConfiguration steerConfiguration = new TalonFXConfiguration();
   private static PIDController yawSnappingController;
   private static String[] moduleNames = { "Front Left", "Front Right", "Back Left", "Back Right" };
+  VoltageOut voltageRequest;
 
   StructPublisher<Pose2d> robotPosePublisher = NetworkTableInstance.getDefault()
       .getStructTopic("/SmartDashboard/Drivetrain/Robot Pose", Pose2d.struct).publish();
@@ -79,9 +82,9 @@ public Drivetrain() {
             prefDrivetrain.measurementStdDevsPosition.getValue(),
             prefDrivetrain.measurementStdDevsHeading.getValue()),
         VecBuilder.fill(
-            prefVision.multiTagStdDevsPosition.getValue(),
-            prefVision.multiTagStdDevsPosition.getValue(),
-            prefVision.multiTagStdDevsHeading.getValue()),
+            prefVision.megaTag2StdDevsPosition.getValue(),
+            prefVision.megaTag2StdDevsPosition.getValue(),
+            prefVision.megaTag2StdDevsHeading.getValue()),
         new PIDConstants(prefDrivetrain.autoDriveP,
             prefDrivetrain.autoDriveI,
             prefDrivetrain.autoDriveD),
@@ -92,6 +95,8 @@ public Drivetrain() {
         () -> constField.isRedAlliance(),
         Robot.isSimulation());
 
+    voltageRequest = new VoltageOut(0);
+
   }
 
   @Override
@@ -100,11 +105,16 @@ public void configure() {
     driveConfiguration.Slot0.kI = prefDrivetrain.driveI.getValue();
     driveConfiguration.Slot0.kD = prefDrivetrain.driveD.getValue();
 
+    driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = constDrivetrain.DRIVE_ENABLE_STATOR_CURRENT_LIMITING;
+    driveConfiguration.CurrentLimits.StatorCurrentLimit = constDrivetrain.DRIVE_STATOR_CURRENT_LIMIT;
+
     driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = constDrivetrain.DRIVE_ENABLE_CURRENT_LIMITING;
-    driveConfiguration.CurrentLimits.SupplyCurrentThreshold = constDrivetrain.DRIVE_CURRENT_THRESH;
     driveConfiguration.CurrentLimits.SupplyCurrentLimit = constDrivetrain.DRIVE_CURRENT_LIMIT;
     driveConfiguration.CurrentLimits.SupplyTimeThreshold = constDrivetrain.DRIVE_CURRENT_TIME_THRESH;
 
+    driveConfiguration.Voltage.PeakForwardVoltage = constDrivetrain.DRIVE_PEAK_FORWARD_VOLTAGE;
+    driveConfiguration.Voltage.PeakReverseVoltage = constDrivetrain.DRIVE_PEAK_REVERSE_VOLTAGE;
+
     driveConfiguration.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.1;
     driveConfiguration.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1;
     driveConfiguration.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.1;
@@ -113,11 +123,16 @@ public void configure() {
     steerConfiguration.Slot0.kI = prefDrivetrain.steerI.getValue();
     steerConfiguration.Slot0.kD = prefDrivetrain.steerD.getValue();
 
+    steerConfiguration.CurrentLimits.StatorCurrentLimitEnable = true;
+    steerConfiguration.CurrentLimits.StatorCurrentLimit = 80;
+
     steerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = constDrivetrain.STEER_ENABLE_CURRENT_LIMITING;
     steerConfiguration.CurrentLimits.SupplyCurrentThreshold = constDrivetrain.STEER_CURRENT_THRESH;
-    steerConfiguration.CurrentLimits.SupplyCurrentLimit = constDrivetrain.STEER_CURRENT_LIMIT;
     steerConfiguration.CurrentLimits.SupplyTimeThreshold = constDrivetrain.STEER_CURRENT_TIME_THRESH;
 
+    steerConfiguration.Voltage.PeakForwardVoltage = 12.0;
+    steerConfiguration.Voltage.PeakReverseVoltage = -12.0;
+
     steerConfiguration.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = 0.1;
     steerConfiguration.ClosedLoopRamps.TorqueClosedLoopRampPeriod = 0.1;
     steerConfiguration.ClosedLoopRamps.VoltageClosedLoopRampPeriod = 0.1;
@@ -275,6 +290,11 @@ public void periodic() {
       SmartDashboard.putNumber(
           "Drivetrain/Module " + moduleNames[mod.moduleNumber] + "/Absolute Encoder Raw Value (Rotations)",
           mod.getRawAbsoluteEncoder());
+
+      SmartDashboard.putNumber("Drivetrain/Module " + moduleNames[mod.moduleNumber] + "/Stator Current",
+          mod.driveMotor.getStatorCurrent().getValueAsDouble());
+      SmartDashboard.putNumber("Drivetrain/Module " + moduleNames[mod.moduleNumber] + "/Supply Current",
+          mod.driveMotor.getSupplyCurrent().getValueAsDouble());
     }
 
     robotPosePublisher.set(getPose());
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index cb77e6e..17f5966 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -65,18 +65,19 @@ public void configure() {
     elevatorConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
     elevatorConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = constElevator.BACKWARD_LIMIT.in(Units.Meters);
 
-    elevatorConfig.CurrentLimits.SupplyCurrentLimitEnable = constElevator.ELEVATOR_ENABLE_CURRENT_LIMITING;
-    elevatorConfig.CurrentLimits.SupplyCurrentLimit = constElevator.ELEVATOR_CURRENT_LIMIT;
-    elevatorConfig.CurrentLimits.SupplyCurrentThreshold = constElevator.ELEVATOR_CURRENT_THRESH;
-    elevatorConfig.CurrentLimits.SupplyTimeThreshold = constElevator.ELEVATOR_CURRENT_TIME_THRESH;
+    elevatorConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    elevatorConfig.CurrentLimits.SupplyCurrentLimit = 40;
+    elevatorConfig.CurrentLimits.SupplyCurrentThreshold = 40;
+    elevatorConfig.CurrentLimits.SupplyTimeThreshold = 0.01;
 
     elevatorMotor.getConfigurator().apply(elevatorConfig);
 
     // -- Drainpipe Motor --
-    drainpipeConfig.CurrentLimits.SupplyCurrentLimitEnable = constElevator.DRAINPIPE_ENABLE_CURRENT_LIMITING;
-    drainpipeConfig.CurrentLimits.SupplyCurrentLimit = constElevator.DRAINPIPE_CURRENT_LIMIT;
-    drainpipeConfig.CurrentLimits.SupplyCurrentThreshold = constElevator.DRAINPIPE_CURRENT_THRESH;
-    drainpipeConfig.CurrentLimits.SupplyTimeThreshold = constElevator.DRAINPIPE_CURRENT_TIME_THRESH;
+    drainpipeConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    drainpipeConfig.CurrentLimits.StatorCurrentLimit = 80;
+
+    drainpipeConfig.Voltage.PeakForwardVoltage = 12.0;
+    drainpipeConfig.Voltage.PeakReverseVoltage = -12.0;
 
     drainpipeMotor.getConfigurator().apply(drainpipeConfig);
   }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 9c2c722..d93a319 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -34,10 +34,14 @@ public Intake() {
   public void configure() {
     rollerConfig.MotorOutput.Inverted = constIntake.MOTOR_INVERT;
 
-    rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = constIntake.ENABLE_CURRENT_LIMITING;
-    rollerConfig.CurrentLimits.SupplyCurrentLimit = constIntake.CURRENT_LIMIT;
-    rollerConfig.CurrentLimits.SupplyCurrentThreshold = constIntake.CURRENT_THRESH;
-    rollerConfig.CurrentLimits.SupplyTimeThreshold = constIntake.CURRENT_TIME_THRESH;
+    rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    rollerConfig.CurrentLimits.SupplyCurrentLimit = 40;
+
+    rollerConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    rollerConfig.CurrentLimits.StatorCurrentLimit = 80;
+
+    rollerConfig.Voltage.PeakForwardVoltage = 12.0;
+    rollerConfig.Voltage.PeakReverseVoltage = -12.0;
 
     rollerMotor.getConfigurator().apply(rollerConfig);
   }
@@ -61,6 +65,7 @@ public boolean getGamePieceCollected() {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putBoolean("Game Piece Detected", getGamePieceCollected());
+    SmartDashboard.putNumber("Intake/Supply Current", rollerMotor.getSupplyCurrent().getValueAsDouble());
+    SmartDashboard.putNumber("Intake/Stator Current", rollerMotor.getStatorCurrent().getValueAsDouble());
   }
-
 }
diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index c66ded6..adcefd4 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -17,6 +17,7 @@
 
 public class Limelight extends SubsystemBase {
   PoseEstimate lastEstimate = new PoseEstimate();
+  private boolean useMegaTag2 = false;
 
   public Limelight() {
   }
@@ -25,6 +26,10 @@ public PoseEstimate getPoseEstimate() {
     return lastEstimate;
   }
 
+  public void setMegaTag2(boolean useMegaTag2) {
+    this.useMegaTag2 = useMegaTag2;
+  }
+
   /**
    * Determines if a given pose estimate should be rejected.
    * 
@@ -34,6 +39,11 @@ public PoseEstimate getPoseEstimate() {
    * @return True if the estimate should be rejected
    */
   public boolean rejectUpdate(PoseEstimate poseEstimate, Measure<Velocity<Angle>> gyroRate) {
+    // We only use MegaTag 1 in disabled, so we have full faith in our position
+    if (!useMegaTag2 && (poseEstimate.tagCount != 0)) {
+      return false;
+    }
+
     // Angular velocity is too high to have accurate vision
     if (gyroRate.compareTo(constLimelight.MAX_ANGULAR_VELOCITY) > 0) {
       return true;
@@ -56,7 +66,13 @@ public boolean rejectUpdate(PoseEstimate poseEstimate, Measure<Velocity<Angle>>
 
   @Override
   public void periodic() {
-    PoseEstimate currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    PoseEstimate currentEstimate;
+    if (useMegaTag2) {
+      currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    } else {
+      currentEstimate = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
+    }
+
     if (currentEstimate != null) {
       lastEstimate = currentEstimate;
     }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 2a2cdf4..7a830d7 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -114,11 +114,14 @@ public void configure() {
     pivotConfig.MotionMagic.MotionMagicJerk = 1600;
 
     // - Current Limits -
-    pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = constShooter.PIVOT_ENABLE_CURRENT_LIMITING;
-    pivotConfig.CurrentLimits.SupplyCurrentThreshold = constShooter.PIVOT_CURRENT_THRESH;
-    pivotConfig.CurrentLimits.SupplyCurrentLimit = constShooter.PIVOT_CURRENT_LIMIT;
+    pivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    pivotConfig.CurrentLimits.SupplyCurrentThreshold = 40;
+    pivotConfig.CurrentLimits.SupplyCurrentLimit = 40;
     pivotConfig.CurrentLimits.SupplyTimeThreshold = constShooter.PIVOT_CURRENT_TIME_THRESH;
 
+    pivotConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    pivotConfig.CurrentLimits.StatorCurrentLimit = 50;
+
     pivotMotor.getConfigurator().apply(pivotConfig);
   }
 
@@ -371,16 +374,21 @@ public void periodic() {
     SmartDashboard.putNumber("Shooter/Left/Desired Velocity RPS", desiredLeftVelocity.in(Units.RotationsPerSecond));
     SmartDashboard.putBoolean("Shooter/Left/Up to Speed", isLeftShooterUpToSpeed());
     SmartDashboard.putNumber("Shooter/Left/PID Slot", currentLeftSlot);
+    SmartDashboard.putNumber("Shooter/Left/Stator Current", leftMotor.getStatorCurrent().getValueAsDouble());
+    SmartDashboard.putNumber("Shooter/Left/Supply Current", leftMotor.getSupplyCurrent().getValueAsDouble());
 
     SmartDashboard.putNumber("Shooter/Right/Velocity RPS", getRightShooterVelocity().in(Units.RotationsPerSecond));
     SmartDashboard.putNumber("Shooter/Right/Desired Velocity RPS", desiredRightVelocity.in(Units.RotationsPerSecond));
     SmartDashboard.putBoolean("Shooter/Right/Up to Speed", isRightShooterUpToSpeed());
     SmartDashboard.putNumber("Shooter/Right/PID Slot", currentRightSlot);
+    SmartDashboard.putNumber("Shooter/Right/Stator Current", rightMotor.getStatorCurrent().getValueAsDouble());
+    SmartDashboard.putNumber("Shooter/Right/Supply Current", rightMotor.getSupplyCurrent().getValueAsDouble());
 
     SmartDashboard.putNumber("Shooter/Pivot/Position", getShooterPosition().in(Units.Degrees));
     SmartDashboard.putNumber("Shooter/Pivot/Last Desired Angle", lastDesiredPivotAngle.in(Units.Degrees));
     SmartDashboard.putBoolean("Shooter/Pivot/At Desired Position", isShooterAtPosition(lastDesiredPivotAngle));
     SmartDashboard.putNumber("Shooter/Pivot/Stator Current", pivotMotor.getStatorCurrent().getValueAsDouble());
+    SmartDashboard.putNumber("Shooter/Pivot/Supply Current", pivotMotor.getSupplyCurrent().getValueAsDouble());
     SmartDashboard.putNumber("Shooter/Pivot/Rotor Velocity", pivotMotor.getRotorVelocity().getValueAsDouble());
 
     SmartDashboard.putBoolean("Shooter/Safe to Move Elevator", isSafeToMoveElevator());
diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java
index 204b133..d5e791d 100644
--- a/src/main/java/frc/robot/subsystems/StateMachine.java
+++ b/src/main/java/frc/robot/subsystems/StateMachine.java
@@ -24,11 +24,29 @@
 public class StateMachine extends SubsystemBase {
   public static RobotState currentState;
   public static TargetState currentTargetState;
+  Climber subClimber;
+  Drivetrain subDrivetrain;
+  Elevator subElevator;
+  Intake subIntake;
+  LEDs subLEDs;
+  Transfer subTransfer;
+  Shooter subShooter;
+  StateMachine subStateMachine = this;
 
   /** Creates a new StateMachine. */
-  public StateMachine() {
+  public StateMachine(Climber subClimber,
+      Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, LEDs subLEDs, Transfer subTransfer,
+      Shooter subShooter) {
     currentState = RobotState.NONE;
     currentTargetState = TargetState.PREP_NONE;
+
+    this.subClimber = subClimber;
+    this.subDrivetrain = subDrivetrain;
+    this.subElevator = subElevator;
+    this.subIntake = subIntake;
+    this.subLEDs = subLEDs;
+    this.subTransfer = subTransfer;
+    this.subShooter = subShooter;
   }
 
   public void setRobotState(RobotState robotState) {
@@ -59,9 +77,7 @@ public TargetState getTargetState() {
    *                     possible from your current state
    * @return The Command to run for that desired state
    */
-  public Command tryState(RobotState desiredState, StateMachine subStateMachine, Climber subClimber,
-      Drivetrain subDrivetrain, Elevator subElevator, Intake subIntake, LEDs subLEDs, Transfer subTransfer,
-      Shooter subShooter) {
+  public Command tryState(RobotState desiredState) {
 
     // TODO: Write this functionality in a later pr
     if (isGivenStateTargetState(desiredState)) {
diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java
index 88dfdb7..a75c136 100644
--- a/src/main/java/frc/robot/subsystems/Transfer.java
+++ b/src/main/java/frc/robot/subsystems/Transfer.java
@@ -8,7 +8,11 @@
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.wpilibj.DigitalInput;
+import au.grapplerobotics.ConfigurationFailedException;
+import au.grapplerobotics.LaserCan;
+import au.grapplerobotics.LaserCan.Measurement;
+import au.grapplerobotics.LaserCan.TimingBudget;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.constTransfer;
@@ -17,14 +21,14 @@
 public class Transfer extends SubsystemBase {
   TalonFX feederMotor;
   TalonFXConfiguration feederConfig = new TalonFXConfiguration();
-  DigitalInput noteSensor;
+  LaserCan noteSensor;
   boolean hasGamePiece;
 
   /** Creates a new Transfer. */
   public Transfer() {
     feederMotor = new TalonFX(mapTransfer.TRANSFER_MOTOR_CAN, "rio");
 
-    noteSensor = new DigitalInput(mapTransfer.NOTE_SENSOR_DIO);
+    noteSensor = new LaserCan(mapTransfer.NOTE_SENSOR_CAN);
 
     configure();
   }
@@ -34,12 +38,20 @@ public void configure() {
     feederConfig.MotorOutput.Inverted = constTransfer.MOTOR_INVERT;
     feederConfig.MotorOutput.NeutralMode = constTransfer.FEEDER_NEUTRAL_MODE;
 
-    feederConfig.CurrentLimits.SupplyCurrentLimitEnable = constTransfer.ENABLE_CURRENT_LIMITING;
-    feederConfig.CurrentLimits.SupplyCurrentLimit = constTransfer.CURRENT_LIMIT;
-    feederConfig.CurrentLimits.SupplyCurrentThreshold = constTransfer.CURRENT_THRESH;
-    feederConfig.CurrentLimits.SupplyTimeThreshold = constTransfer.CURRENT_TIME_THRESH;
+    feederConfig.CurrentLimits.StatorCurrentLimitEnable = false;
+    feederConfig.CurrentLimits.StatorCurrentLimit = 80;
+
+    feederConfig.Voltage.PeakForwardVoltage = 12.0;
+    feederConfig.Voltage.PeakReverseVoltage = -12.0;
 
     feederMotor.getConfigurator().apply(feederConfig);
+
+    try {
+      noteSensor.setTimingBudget(constTransfer.TIMING_BUDGET);
+    } catch (ConfigurationFailedException e) {
+      System.out.println("LaserCAN could not be initialized :<");
+
+    }
   }
 
   public void setFeederSpeed(double speed) {
@@ -51,8 +63,16 @@ public void setFeederNeutralOutput() {
   }
 
   public boolean getGamePieceStored() {
-    boolean noteSensorValue = (constTransfer.NOTE_SENSOR_INVERT) ? !noteSensor.get() : noteSensor.get();
-    return (noteSensorValue || hasGamePiece);
+    Measurement measurement = noteSensor.getMeasurement();
+    if (hasGamePiece) {
+      return hasGamePiece;
+    }
+
+    if (measurement != null) {
+      return measurement.distance_mm <= constTransfer.PIECE_DETECTED_DIST_THRESH.in(Units.Millimeters);
+    }
+
+    return false;
   }
 
   public void setGamePieceCollected(boolean isCollected) {
@@ -63,5 +83,12 @@ public void setGamePieceCollected(boolean isCollected) {
   public void periodic() {
     // This method will be called once per scheduler run
     SmartDashboard.putBoolean("Game Piece Stored", getGamePieceStored());
+    SmartDashboard.putNumber("Transfer/Supply Current", feederMotor.getSupplyCurrent().getValueAsDouble());
+    SmartDashboard.putNumber("Transfer/Stator Current", feederMotor.getStatorCurrent().getValueAsDouble());
+    if (noteSensor.getMeasurement() != null) {
+      SmartDashboard.putNumber("Transfer/Laser Can/Distance", noteSensor.getMeasurement().distance_mm);
+      SmartDashboard.putNumber("Transfer/Laser Can/Ambient Light", noteSensor.getMeasurement().ambient);
+      SmartDashboard.putNumber("Transfer/Laser Can/Timing Budget", noteSensor.getMeasurement().budget_ms);
+    }
   }
 }
diff --git a/vendordeps/libgrapplefrc2024.json b/vendordeps/libgrapplefrc2024.json
new file mode 100644
index 0000000..cef1bde
--- /dev/null
+++ b/vendordeps/libgrapplefrc2024.json
@@ -0,0 +1,74 @@
+{
+    "fileName": "libgrapplefrc2024.json",
+    "name": "libgrapplefrc",
+    "version": "2024.3.1",
+    "frcYear": "2024",
+    "uuid": "8ef3423d-9532-4665-8339-206dae1d7168",
+    "mavenUrls": [
+        "https://storage.googleapis.com/grapple-frc-maven"
+    ],
+    "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2024.json",
+    "javaDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcjava",
+            "version": "2024.3.1"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2024.3.1",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrccpp",
+            "version": "2024.3.1",
+            "libName": "grapplefrc",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "au.grapplerobotics",
+            "artifactId": "libgrapplefrcdriver",
+            "version": "2024.3.1",
+            "libName": "grapplefrcdriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "windowsx86",
+                "linuxarm64",
+                "linuxx86-64",
+                "linuxathena",
+                "linuxarm32",
+                "osxuniversal"
+            ]
+        }
+    ]
+}
\ No newline at end of file