From 60f1dc55ff2ea46b889b104d25c07e15dd7724e2 Mon Sep 17 00:00:00 2001
From: Noland <nolan.bjorn.schuler@gmail.com>
Date: Tue, 3 Sep 2024 19:41:39 -0700
Subject: [PATCH 01/29] Intake stops at serializer, and the shooter shoots from
 the serializer

---
 gradlew                                         |  0
 src/main/java/frc/robot/Constants.java          |  3 ++-
 src/main/java/frc/robot/RobotContainer.java     |  6 +++---
 .../robot/commands/AdvancedIntakeCommand.java   |  4 +---
 .../java/frc/robot/commands/IntakeCommand.java  |  8 ++++----
 .../java/frc/robot/commands/OuttakeCommand.java |  4 ++--
 .../java/frc/robot/commands/ShootCommand.java   |  8 +++++++-
 .../frc/robot/commands/StopIntakeCommand.java   |  4 ++--
 .../frc/robot/subsystems/IntakeSubsystem.java   | 17 +++++++++--------
 .../frc/robot/subsystems/WristSubsystem.java    |  0
 10 files changed, 30 insertions(+), 24 deletions(-)
 mode change 100644 => 100755 gradlew
 delete mode 100644 src/main/java/frc/robot/subsystems/WristSubsystem.java

diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7c3f2ca..a2d4dc0 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -244,6 +244,7 @@ public static final class Modes {
       public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
       public static final IntakePowers HOLD = new IntakePowers(0, 0d);
       public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
+      public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
     }
 
     public static final boolean IS_BEAMBREAK = true;
@@ -259,7 +260,7 @@ public static final class Ports {
 
     public static final class Modes {
       public static final ShooterSubsystem.ShooterPowers INTAKE =
-          new ShooterSubsystem.ShooterPowers(76, 1, .15);
+          new ShooterSubsystem.ShooterPowers(76, 1, 0);
       public static final ShooterSubsystem.ShooterPowers IDLE =
           new ShooterSubsystem.ShooterPowers(0, 0, 0);
       public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER =
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 11fb263..127a3c9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -127,7 +127,7 @@ public RobotContainer() {
     // reigster commands for pathplanner
     NamedCommands.registerCommand(
         "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-    NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
+    NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem, intakeSubsystem));
     NamedCommands.registerCommand(
         "ShooterRampUpCommand",
         new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
@@ -351,14 +351,14 @@ private void configureButtonBindings() {
         .rightBumper()
         .onTrue(
             new AccelNoteCommand(shooterSubsystem)
-                .andThen(new ShootCommand(shooterSubsystem))
+                .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem))
                 .andThen(
                     new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
 
     // SHOOT OVERRIDE
     jacob
         .rightTrigger()
-        .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem)));
+        .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem, intakeSubsystem)));
 
     anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
     anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
index 22b1606..9e2a86a 100644
--- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
@@ -17,9 +17,7 @@ public AdvancedIntakeCommand(
     addCommands(
         new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem),
         new ParallelCommandGroup(
-            new OuttakeCommand(intakeSubsystem)
-                .withTimeout(3)
-                .andThen(new StopIntakeCommand(intakeSubsystem)),
+                (new StopIntakeCommand(intakeSubsystem)),
             new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
   }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index 1453543..41187ed 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.Modes;
+import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
@@ -34,7 +34,7 @@ public IntakeCommand(
   @Override
   public void initialize() {
     pivotSubsystem.setTargetDegrees(20);
-    intakeSubsystem.setIntakeMode(Modes.INTAKE);
+    intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
     shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
   }
 
@@ -45,7 +45,7 @@ public void execute() {}
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    intakeSubsystem.setIntakeMode(IntakeSubsystem.Modes.HOLD);
+    intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD);
     shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER);
     shooterSubsystem.haltAccelerator();
   }
@@ -53,6 +53,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return shooterSubsystem.isBeamBreakSensorTriggered();
+    return intakeSubsystem.isBeamBreakSensorTriggered();
   }
 }
diff --git a/src/main/java/frc/robot/commands/OuttakeCommand.java b/src/main/java/frc/robot/commands/OuttakeCommand.java
index f0386e7..0883a30 100644
--- a/src/main/java/frc/robot/commands/OuttakeCommand.java
+++ b/src/main/java/frc/robot/commands/OuttakeCommand.java
@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.Modes;
+import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 
 public class OuttakeCommand extends Command {
 
@@ -23,7 +23,7 @@ public OuttakeCommand(IntakeSubsystem intakeSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeSubsystem.setIntakeMode(Modes.REVERSE);
+    intakeSubsystem.setIntakeMode(IntakeMode.REVERSE);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index 6ceb2fa..1dfb64d 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -5,15 +5,20 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
+import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
+
 
 public class ShootCommand extends Command {
   private ShooterSubsystem shooterSubsystem;
+  private IntakeSubsystem intakeSubsystem;
 
   /** Creates a new ShootCommand. */
-  public ShootCommand(ShooterSubsystem shooterSubsystem) {
+  public ShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
+    this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     addRequirements(shooterSubsystem);
   }
@@ -22,6 +27,7 @@ public ShootCommand(ShooterSubsystem shooterSubsystem) {
   @Override
   public void initialize() {
     shooterSubsystem.advanceToShootMode();
+    intakeSubsystem.setIntakeMode(IntakeMode.SHOOT_SPEAKER);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/commands/StopIntakeCommand.java b/src/main/java/frc/robot/commands/StopIntakeCommand.java
index 696f4c3..1f5a4fa 100644
--- a/src/main/java/frc/robot/commands/StopIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/StopIntakeCommand.java
@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.IntakeSubsystem.Modes;
+import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 
 public class StopIntakeCommand extends Command {
 
@@ -23,7 +23,7 @@ public StopIntakeCommand(IntakeSubsystem intakeSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    intakeSubsystem.setIntakeMode(Modes.HOLD);
+    intakeSubsystem.setIntakeMode(IntakeMode.HOLD);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 37d9314..3f3be1b 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -19,19 +19,20 @@ public class IntakeSubsystem extends SubsystemBase {
   private final TalonFX serializerMotor;
   private final ShuffleboardTab tab = Shuffleboard.getTab("Intake");
   private final DigitalInput noteSensor;
-  private Modes intakeMode;
-  private Modes pastMode;
+  private IntakeMode intakeMode;
+  private IntakeMode pastMode;
   private double timeSincePenaltyHazard;
   private boolean pastPenalty;
 
-  public enum Modes {
+  public enum IntakeMode {
     INTAKE(Intake.Modes.INTAKE),
     HOLD(Intake.Modes.HOLD),
-    REVERSE(Intake.Modes.REVERSE);
+    REVERSE(Intake.Modes.REVERSE),
+    SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER);
 
     public final IntakePowers modePowers;
 
-    private Modes(IntakePowers modePowers) {
+    private IntakeMode(IntakePowers modePowers) {
       this.modePowers = modePowers;
     }
   }
@@ -57,7 +58,7 @@ public IntakeSubsystem() {
     intakeMotor.setInverted(true);
     serializerMotor.setInverted(true);
 
-    intakeMode = Modes.HOLD;
+    intakeMode = IntakeMode.HOLD;
 
     timeSincePenaltyHazard = 7;
 
@@ -70,7 +71,7 @@ public IntakeSubsystem() {
     }
   }
 
-  public void setIntakeMode(Modes intakeMode) {
+  public void setIntakeMode(IntakeMode intakeMode) {
     this.intakeMode = intakeMode;
   }
 
@@ -79,7 +80,7 @@ public boolean isBeamBreakSensorTriggered() {
     return !noteSensor.get();
   }
 
-  private Modes getIntakeMode() {
+  private IntakeMode getIntakeMode() {
     return intakeMode;
   }
 
diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java
deleted file mode 100644
index e69de29..0000000

From 482a6bbcbb00ed9ab1127a59e32e226c8594c10f Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Mon, 9 Sep 2024 17:26:27 -0700
Subject: [PATCH 02/29] Added arm code to elevatorsubsystem

same as normal elevator code but with degrees
---
 .../robot/subsystems/ElevatorSubsystem.java   | 104 +++++++++++-------
 1 file changed, 64 insertions(+), 40 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 786c04d..92846f2 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -1,8 +1,8 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.MathUtil;
+// import com.ctre.phoenix6.signals.NeutralModeValue;
+// import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -13,55 +13,79 @@
 
 public class ElevatorSubsystem extends SubsystemBase {
 
-  private TalonFX motor;
-  private PIDController controller;
-  private double targetHeight;
-  private double motorPower;
-  private double currentHeight = 0;
-  private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
+    private TalonFX motor;
+    private PIDController controller;
+    private double targetHeight;
+    private double targetAngle;
+    private double motorPower;
+    private double currentHeight;
+    private double currentAngle;
+    private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
 
-  /** Creates a new ElevatorSubsystem. */
-  public ElevatorSubsystem() {
+    /** Creates a new ElevatorSubsystem. */
+    public ElevatorSubsystem() {
 
-    motor = new TalonFX(15);
+        motor = new TalonFX(15);
 
-    motor.clearStickyFaults();
+        motor.clearStickyFaults();
 
-    controller = new PIDController(0.4, 0, 0.0125);
+        controller = new PIDController(0.4, 0, 0.0125);
 
-    ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
-    ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
-    // ElevatorTab.addNumber("PID output", () -> this.controller);
-    ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
-    ElevatorTab.add(controller);
+        ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
+        ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+        ElevatorTab.addNumber("Target Angle", () -> this.targetAngle);
+        // ElevatorTab.addNumber("PID output", () -> this.controller);
+        ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
+        ElevatorTab.add(controller);
 
-    targetHeight = 0;
-    currentHeight = 0;
-  }
+        targetHeight = 0;
+        currentHeight = 0;
+        targetAngle = 0;
+        currentAngle = 0;
+    }
+
+    public static double inchesToRotations(double height) {
+        return height / Elevator.GEAR_RATIO;
+    }
+
+    public static double rotationsToInches(double rotations) {
+        return rotations * Elevator.GEAR_RATIO;
+    }
+
+    public static double degreesToRotations(double degrees) {
+        return degrees / Elevator.GEAR_RATIO;
+    }
 
-  public static double inchesToRotations(double height) {
-    return height / Elevator.GEAR_RATIO;
-  }
+    public static double rotationsToDegrees(double rotations) {
+        return rotations * Elevator.GEAR_RATIO;
+    }
 
-  public static double rotationsToInches(double rotations) {
-    return rotations * Elevator.GEAR_RATIO;
-  }
+    public void setTargetHeight(double targetHeight) {
+        this.targetHeight = targetHeight;
+    }
 
-  public void setTargetHeight(double targetHeight) {
-    this.targetHeight = targetHeight;
-  }
+    public void setTargetAngle(double targetAngle) {
+        this.targetAngle = targetAngle;
+    }
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-    motorPower = controller.calculate(rotationsToInches(0), targetHeight); //Placeholder for rotations
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        motorPower = controller.calculate(inchesToRotations(targetHeight), targetHeight);
+        motorPower = controller.calculate(degreesToRotations(targetAngle), targetAngle);
+    }
 
-  }
+    public boolean nearTargetHeight() {
+        if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) {
+            return true;
+        }
+        return false;
+    }
 
-  public boolean nearTargetHeight() {
-    if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) {
-      return true;
+    public boolean nearTargetAngle() {
+        if (targetAngle - 1 <= currentAngle && currentAngle <= targetAngle + 1) {
+            return true;
+        }
+        return false;
     }
-    return false;
-  }
 }

From b0983e65c5fc164226ac795b8acffc3b6402a9aa Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Mon, 9 Sep 2024 17:40:52 -0700
Subject: [PATCH 03/29] Angle and Height commands added

gear ratios still missing with placeholder
---
 .../java/frc/robot/commands/AngleCommand.java | 53 +++++++++++++++++++
 .../frc/robot/commands/HeightCommand.java     | 53 +++++++++++++++++++
 2 files changed, 106 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/AngleCommand.java
 create mode 100644 src/main/java/frc/robot/commands/HeightCommand.java

diff --git a/src/main/java/frc/robot/commands/AngleCommand.java b/src/main/java/frc/robot/commands/AngleCommand.java
new file mode 100644
index 0000000..509d2b7
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AngleCommand.java
@@ -0,0 +1,53 @@
+// 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 frc.robot.subsystems.ElevatorSubsystem;
+
+import frc.robot.subsystems.ElevatorSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class AngleCommand extends Command {
+
+    ElevatorSubsystem elevatorSubsystem;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    double angle;
+
+    public AngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        this.angle = angle;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(elevatorSubsystem);
+    }
+
+    // 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() {
+
+        elevatorSubsystem.setTargetAngle(angle);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.nearTargetAngle();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/HeightCommand.java b/src/main/java/frc/robot/commands/HeightCommand.java
new file mode 100644
index 0000000..f2beb09
--- /dev/null
+++ b/src/main/java/frc/robot/commands/HeightCommand.java
@@ -0,0 +1,53 @@
+// 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 frc.robot.subsystems.ElevatorSubsystem;
+
+import frc.robot.subsystems.ElevatorSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** An example command that uses an example subsystem. */
+public class HeightCommand extends Command {
+
+    ElevatorSubsystem elevatorSubsystem;
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    double height;
+
+    public HeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
+        this.elevatorSubsystem = elevatorSubsystem;
+        this.height = height;
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(elevatorSubsystem);
+    }
+
+    // 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() {
+
+        elevatorSubsystem.setTargetHeight(height);
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return elevatorSubsystem.nearTargetHeight();
+    }
+}

From 648b240024d35374efe0ffea575619d8f529091a Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Mon, 9 Sep 2024 18:16:37 -0700
Subject: [PATCH 04/29] SHOOT_AMP command added

---
 src/main/java/frc/robot/commands/ShootCommand.java      | 5 +++--
 src/main/java/frc/robot/subsystems/IntakeSubsystem.java | 3 ++-
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index 1dfb64d..4778a69 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -10,7 +10,6 @@
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 
-
 public class ShootCommand extends Command {
   private ShooterSubsystem shooterSubsystem;
   private IntakeSubsystem intakeSubsystem;
@@ -28,11 +27,13 @@ public ShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSub
   public void initialize() {
     shooterSubsystem.advanceToShootMode();
     intakeSubsystem.setIntakeMode(IntakeMode.SHOOT_SPEAKER);
+    intakeSubsystem.setIntakeMode(IntakeMode.SHOOT_AMP);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+  }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 3f3be1b..b728c69 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -28,7 +28,8 @@ public enum IntakeMode {
     INTAKE(Intake.Modes.INTAKE),
     HOLD(Intake.Modes.HOLD),
     REVERSE(Intake.Modes.REVERSE),
-    SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER);
+    SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER),
+    SHOOT_AMP(Intake.Modes.SHOOT_AMP);
 
     public final IntakePowers modePowers;
 

From 06ee3c56fd6e29d1c17ff597019c2b0dc4ab3c5b Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Mon, 9 Sep 2024 18:21:52 -0700
Subject: [PATCH 05/29] Moved serializer code from intake to shooter

Added serializerSpeed to ShooterPowers and Constants
---
 src/main/java/frc/robot/Constants.java        | 981 +++++++++---------
 .../frc/robot/subsystems/IntakeSubsystem.java |   8 -
 .../robot/subsystems/ShooterSubsystem.java    |  54 +-
 3 files changed, 521 insertions(+), 522 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 40442c3..0e2621d 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -51,523 +51,522 @@
 
 @SuppressWarnings("java:S1118")
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants. This class should not be used for any other purpose. All constants should be declared
+ * The Constants class provides a convenient place for teams to hold robot-wide
+ * numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants
+ * should be declared
  * globally (i.e. public static). Do not put anything functional in this class.
  *
- * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * <p>
+ * It is advised to statically import this class (or one of its inner classes)
+ * wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
 
-  public static final class Config {
-    // maybe tune PID values?
-    public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG =
-        new HolonomicPathFollowerConfig(
-            new PIDConstants(20, 0, 0),
-            new PIDConstants(10, 0, 0),
-            Drive.MAX_VELOCITY_METERS_PER_SECOND,
-            Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
-            new ReplanningConfig());
-
-    /** turn this off before comp. */
-    public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
-
-    /** turn this off! only use on practice eboard testing. */
-    public static final boolean DISABLE_SWERVE_INIT = false;
-
-    /** keep this on for pigeon, disable if absolutely necessary */
-    public static final boolean FLIP_GYROSCOPE = true;
-
-    /** def turn this off unless you are using it, generates in excess of 100k rows for a match. */
-    public static final boolean WRITE_APRILTAG_DATA = false;
-
-    public static final Path APRILTAG_DATA_PATH =
-        Filesystem.getDeployDirectory().toPath().resolve("poseEstimationsAtDistances.csv");
-    public static final double REAL_X = 0.0;
-    public static final double REAL_Y = 0.0;
-  }
-
-  public static final class Drive {
-    public static final int PIGEON_PORT = 0; // placeholder
-    public static final String SWERVE_CANBUS = "rio"; // placeholder
-
-    // max voltage delivered to drivebase
-    // supposedly useful to limit speed for testing
-    public static final double MAX_VOLTAGE = 12.0;
-    // maximum velocity
-    // FIXME measure this value experimentally
-    public static final double MAX_VELOCITY_METERS_PER_SECOND =
-        6380.0 // falcon 500 free speed rpm
-            / 60.0
-            * 0.10033
-            * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
-            * Math.PI;
-    // theoretical value
-    // FIXME measure and validate experimentally
-    public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND =
-        MAX_VELOCITY_METERS_PER_SECOND
-            / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
-            * .5;
-
-    /** the maximum amount of angular error pid loops will tolerate for rotation */
-    public static final double ANGULAR_ERROR = 1.0;
-    /** the minimum magnitude of the right stick for it to be used as a new rotation angle */
-    public static final double ROTATE_VECTOR_MAGNITUDE = .7;
-
-    public static final class Dims {
-      // FIXME validate with hardware
-      public static final double TRACKWIDTH_METERS =
-          .5207; // 20.5 inches (source: cad) converted to meters
-      public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
-
-      public static final double BUMPER_WIDTH_METERS_X = .9779;
-      public static final double BUMPER_WIDTH_METERS_Y = .8382;
+    public static final class Config {
+        // maybe tune PID values?
+        public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig(
+                new PIDConstants(20, 0, 0),
+                new PIDConstants(10, 0, 0),
+                Drive.MAX_VELOCITY_METERS_PER_SECOND,
+                Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
+                new ReplanningConfig());
+
+        /** turn this off before comp. */
+        public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
+
+        /** turn this off! only use on practice eboard testing. */
+        public static final boolean DISABLE_SWERVE_INIT = false;
+
+        /** keep this on for pigeon, disable if absolutely necessary */
+        public static final boolean FLIP_GYROSCOPE = true;
+
+        /**
+         * def turn this off unless you are using it, generates in excess of 100k rows
+         * for a match.
+         */
+        public static final boolean WRITE_APRILTAG_DATA = false;
+
+        public static final Path APRILTAG_DATA_PATH = Filesystem.getDeployDirectory().toPath()
+                .resolve("poseEstimationsAtDistances.csv");
+        public static final double REAL_X = 0.0;
+        public static final double REAL_Y = 0.0;
     }
 
-    /*
-     module layout:
-        |──────
-     |->│#   ##steer motor
-     │  │  ##cancoder
-     │  │##drive motor
-     module number
-
-     steer is always left
-     from corner perspective
-
-     robot visualization:
-    |──────────────────────|
-    │2   10          04   1│
-    │  25              24  │
-    │11     S      D     03│
-    │     D          S     │
-    │                      │
-    │                      │
-    │     S          D     │
-    │       D      S       │
-    │12    |────────|    02│
-    │  26  │        │  27  │
-    │3   13│  batt  │01   4│
-    |──────┴───┬┬───┴──────|
-               ││
-               ││
-               ▼▼
-         software front
-     */
-
-    public static final class Modules {
-      public static final class Params {
-        public static final double WHEEL_RADIUS = 2; // also in INCHES
-        public static final double COUPLING_GEAR_RATIO = 3.125;
-        public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
-        public static final double STEER_GEAR_RATIO = 21.428571428571427;
-        public static final Slot0Configs DRIVE_MOTOR_GAINS =
-            new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0.32).withKV(0.11).withKA(0);
-        public static final Slot0Configs STEER_MOTOR_GAINS =
-            new Slot0Configs().withKP(11).withKI(0).withKD(0).withKS(0.4).withKV(0.6).withKA(0);
-        public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT =
-            ClosedLoopOutputType.Voltage;
-        public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT =
-            ClosedLoopOutputType.Voltage;
-        public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
-        public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
-        public static final double SLIP_CURRENT = 0; // optional, unused rn
-        public static final boolean STEER_MOTOR_INVERTED = true;
-
-        public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
-        public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
-      }
-
-      public static final class Module1 { // back left
-        public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
-        public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
-        public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
-
-        public static final double STEER_OFFSET =
-            IS_COMP_BOT
-                ? 0.41943359375 // comp bot offset
-                : 0.0595703125; // practice bot offset
-      }
-
-      public static final class Module2 { // back right
-        public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
-        public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
-        public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
-
-        public static final double STEER_OFFSET =
-            IS_COMP_BOT
-                ? -0.39990234375 // comp bot offset
-                : 0.262451171875; // practice bot offset
-      }
-
-      public static final class Module3 { // front right
-        public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
-        public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
-        public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
-
-        public static final double STEER_OFFSET =
-            IS_COMP_BOT
-                ? 0.225341796875 // comp bot offset
-                : -0.20825195312; // practice bot offset
-      }
-
-      public static final class Module4 { // front left
-        public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
-        public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
-        public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
-
-        public static final double STEER_OFFSET =
-            IS_COMP_BOT
-                ? 0.316650390625 // comp bot offset
-                : -0.3564453125 + 180; // practice bot offset
-      }
+    public static final class Drive {
+        public static final int PIGEON_PORT = 0; // placeholder
+        public static final String SWERVE_CANBUS = "rio"; // placeholder
+
+        // max voltage delivered to drivebase
+        // supposedly useful to limit speed for testing
+        public static final double MAX_VOLTAGE = 12.0;
+        // maximum velocity
+        // FIXME measure this value experimentally
+        public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 // falcon 500 free speed rpm
+                / 60.0
+                * 0.10033
+                * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
+                * Math.PI;
+        // theoretical value
+        // FIXME measure and validate experimentally
+        public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND
+                / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
+                * .5;
+
+        /** the maximum amount of angular error pid loops will tolerate for rotation */
+        public static final double ANGULAR_ERROR = 1.0;
+        /**
+         * the minimum magnitude of the right stick for it to be used as a new rotation
+         * angle
+         */
+        public static final double ROTATE_VECTOR_MAGNITUDE = .7;
+
+        public static final class Dims {
+            // FIXME validate with hardware
+            public static final double TRACKWIDTH_METERS = .5207; // 20.5 inches (source: cad) converted to meters
+            public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
+
+            public static final double BUMPER_WIDTH_METERS_X = .9779;
+            public static final double BUMPER_WIDTH_METERS_Y = .8382;
+        }
+
+        /*
+         * module layout:
+         * |──────
+         * |->│# ##steer motor
+         * │ │ ##cancoder
+         * │ │##drive motor
+         * module number
+         * 
+         * steer is always left
+         * from corner perspective
+         * 
+         * robot visualization:
+         * |──────────────────────|
+         * │2 10 04 1│
+         * │ 25 24 │
+         * │11 S D 03│
+         * │ D S │
+         * │ │
+         * │ │
+         * │ S D │
+         * │ D S │
+         * │12 |────────| 02│
+         * │ 26 │ │ 27 │
+         * │3 13│ batt │01 4│
+         * |──────┴───┬┬───┴──────|
+         * ││
+         * ││
+         * ▼▼
+         * software front
+         */
+
+        public static final class Modules {
+            public static final class Params {
+                public static final double WHEEL_RADIUS = 2; // also in INCHES
+                public static final double COUPLING_GEAR_RATIO = 3.125;
+                public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
+                public static final double STEER_GEAR_RATIO = 21.428571428571427;
+                public static final Slot0Configs DRIVE_MOTOR_GAINS = new Slot0Configs().withKP(3).withKI(0).withKD(0)
+                        .withKS(0.32).withKV(0.11).withKA(0);
+                public static final Slot0Configs STEER_MOTOR_GAINS = new Slot0Configs().withKP(11).withKI(0).withKD(0)
+                        .withKS(0.4).withKV(0.6).withKA(0);
+                public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
+                public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
+                public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
+                public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
+                public static final double SLIP_CURRENT = 0; // optional, unused rn
+                public static final boolean STEER_MOTOR_INVERTED = true;
+
+                public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
+                public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
+            }
+
+            public static final class Module1 { // back left
+                public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
+                public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
+                public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
+
+                public static final double STEER_OFFSET = IS_COMP_BOT
+                        ? 0.41943359375 // comp bot offset
+                        : 0.0595703125; // practice bot offset
+            }
+
+            public static final class Module2 { // back right
+                public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
+                public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
+                public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
+
+                public static final double STEER_OFFSET = IS_COMP_BOT
+                        ? -0.39990234375 // comp bot offset
+                        : 0.262451171875; // practice bot offset
+            }
+
+            public static final class Module3 { // front right
+                public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
+                public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
+                public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
+
+                public static final double STEER_OFFSET = IS_COMP_BOT
+                        ? 0.225341796875 // comp bot offset
+                        : -0.20825195312; // practice bot offset
+            }
+
+            public static final class Module4 { // front left
+                public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
+                public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
+                public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
+
+                public static final double STEER_OFFSET = IS_COMP_BOT
+                        ? 0.316650390625 // comp bot offset
+                        : -0.3564453125 + 180; // practice bot offset
+            }
+        }
+
+        public static final class Setpoints {
+            public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
+
+            public static final int SOURCE_DEGREES = 39;
+            public static final int SPEAKER_DEGREES = 11;
+            public static final int EPSILON = 3;
+        }
     }
 
-    public static final class Setpoints {
-      public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
-
-      public static final int SOURCE_DEGREES = 39;
-      public static final int SPEAKER_DEGREES = 11;
-      public static final int EPSILON = 3;
+    public static final class Intake {
+        public static final class Ports {
+            public static final int INTAKE_MOTOR_PORT = 15;
+            public static final int SERIALIZER_MOTOR_PORT = 16;
+            public static final int INTAKE_SENSOR_PORT = 9;
+        }
+
+        public static final class Modes {
+            public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
+            public static final IntakePowers HOLD = new IntakePowers(0, 0d);
+            public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
+            public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
+            public static final IntakePowers SHOOT_AMP = new IntakePowers(0, 1);
+        }
+
+        public static final boolean IS_BEAMBREAK = true;
     }
-  }
 
-  public static final class Intake {
-    public static final class Ports {
-      public static final int INTAKE_MOTOR_PORT = 15;
-      public static final int SERIALIZER_MOTOR_PORT = 16;
-      public static final int INTAKE_SENSOR_PORT = 9;
+    public static final class Shooter {
+        public static final class Ports {
+            public static final int TOP_SHOOTER_MOTOR_PORT = 20;
+            public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19;
+            public static final int ACCELERATOR_MOTOR_PORT = 17;
+            public static final int BEAM_BREAK_SENSOR_PORT = 8;
+        }
+
+        public static final class Modes {
+            public static final ShooterSubsystem.ShooterPowers INTAKE = new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
+            public static final ShooterSubsystem.ShooterPowers IDLE = new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
+            public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(76, 1,
+                    0, 0);
+            public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK = new ShooterSubsystem.ShooterPowers(25,
+                    0.4, 0, 0);
+            public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT = new ShooterSubsystem.ShooterPowers(0, 0,
+                    0, 0);
+            public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = new ShooterSubsystem.ShooterPowers(76, 1,
+                    .5, 0);
+            public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = new ShooterSubsystem.ShooterPowers(0, 1, 0,
+                    0);
+            public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK = new ShooterSubsystem.ShooterPowers(25,
+                    0.4, .5, 0);
+            public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD = new ShooterSubsystem.ShooterPowers(8,
+                    2.5, .5, 0);
+            public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
+                    40, 1, 0, 0);
+            public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(30, 1, 0,
+                    0);
+            public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = new ShooterSubsystem.ShooterPowers(30, 1,
+                    0.5, 0);
+            public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = new ShooterSubsystem.ShooterPowers(76, 1,
+                    0.5, 0);
+            public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = new ShooterSubsystem.ShooterPowers(
+                    30, 1, 0, 0);
+            public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = new ShooterSubsystem.ShooterPowers(30, 1,
+                    0.5, 0);
+        }
+
+        public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23).withKV(0.24);
+
+        public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
     }
 
-    public static final class Modes {
-      public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
-      public static final IntakePowers HOLD = new IntakePowers(0, 0d);
-      public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-      public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
+    public static final class Pivot {
+        public static final class Ports {
+            public static final int PIVOT_MOTOR_PORT = 18;
+            public static final int CANCODER_PORT = 28;
+            public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
+        }
+
+        public static final class MotorConfigs {
+            public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR = new MagnetSensorConfigs()
+                    .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+                    .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
+                    .withMagnetOffset(PIVOT_CANCODER_OFFSET);
+            public static final CANcoderConfiguration CANCODER_CONFIG = new CANcoderConfiguration()
+                    .withMagnetSensor(CANCODER_MAGNET_SENSOR);
+
+            public static final FeedbackConfigs PIVOT_FEEDBACK = new FeedbackConfigs()
+                    .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
+                    .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+                    .withSensorToMechanismRatio(1.0)
+                    .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
+            public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT = new SoftwareLimitSwitchConfigs()
+                    .withForwardSoftLimitThreshold(0.25)
+                    .withReverseSoftLimitThreshold(-0.015)
+                    .withForwardSoftLimitEnable(false)
+                    .withReverseSoftLimitEnable(false);
+            public static final VoltageConfigs PIVOT_VOLTAGE = new VoltageConfigs().withPeakForwardVoltage(5)
+                    .withPeakReverseVoltage(-5);
+            public static final TalonFXConfiguration PIVOT_CONFIG = new TalonFXConfiguration()
+                    .withFeedback(PIVOT_FEEDBACK)
+                    .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
+                    .withVoltage(PIVOT_VOLTAGE);
+        }
+
+        public static final class Setpoints {
+            public static final int MINIMUM_ANGLE = 15;
+            public static final int MAXIMUM_ANGLE = 125;
+
+            public static final int MINIMUM_SAFE_THRESHOLD = 15;
+            public static final int MAXIMUM_SAFE_THRESHOLD = 80;
+
+            public static final int SPEAKER_ANGLE = 30;
+        }
+
+        public static final int EPSILON = 2;
+
+        public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
+        public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder values
+        public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
+
+        public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
+        public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
+
+        public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
+        public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
+
+        public static final double GRAVITY = 9.80665; // meters per second
+
+        public static final double GRAVITY_VOLTAGE = 0.4;
+        public static final double PIVOT_MAX_VOLTAGE = 3.5;
     }
 
-    public static final boolean IS_BEAMBREAK = true;
-  }
-
-  public static final class Shooter {
-    public static final class Ports {
-      public static final int TOP_SHOOTER_MOTOR_PORT = 20;
-      public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19;
-      public static final int ACCELERATOR_MOTOR_PORT = 17;
-      public static final int BEAM_BREAK_SENSOR_PORT = 8;
+    public static final class Vision {
+        public static record VisionSource(String name, Transform3d robotToCamera) {
+        }
+
+        public static final List<VisionSource> VISION_SOURCES = List.of(
+                /*
+                 * new VisionSource(
+                 * "frontCam",
+                 * new Transform3d(
+                 * new Translation3d(
+                 * -0.305, // front/back
+                 * -0.2286, // left/right
+                 * -0.2159 // up/down
+                 * ),
+                 * new Rotation3d(
+                 * 0,
+                 * Math.toRadians(30), // angle up/down
+                 * Math.toRadians(180)))),
+                 * new VisionSource(
+                 * "backCam",
+                 * new Transform3d(
+                 * new Translation3d(
+                 * -0.2796, // front/back
+                 * -0.2286, // left/right
+                 * -0.2159 // up/down
+                 * ),
+                 * new Rotation3d(
+                 * 0,
+                 * Math.toRadians(30), // angle up/down
+                 * Math.toRadians(180)))),
+                 */
+                new VisionSource(
+                        "backUpCam",
+                        new Transform3d(
+                                new Translation3d(
+                                        -0.2796, // front/back
+                                        0.2286, // left/right
+                                        -0.2159 // up/down
+                                ),
+                                new Rotation3d(
+                                        0,
+                                        Math.toRadians(30), // angle up/down
+                                        Math.toRadians(180)))));
+
+        public static final int THREAD_SLEEP_DURATION_MS = 5;
     }
 
-    public static final class Modes {
-      public static final ShooterSubsystem.ShooterPowers INTAKE =
-          new ShooterSubsystem.ShooterPowers(76, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers IDLE =
-          new ShooterSubsystem.ShooterPowers(0, 0, 0);
-      public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER =
-          new ShooterSubsystem.ShooterPowers(76, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK =
-          new ShooterSubsystem.ShooterPowers(25, 0.4, 0);
-      public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT =
-          new ShooterSubsystem.ShooterPowers(10, 2.5, 0);
-      public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER =
-          new ShooterSubsystem.ShooterPowers(76, 1, .5);
-      public static final ShooterSubsystem.ShooterPowers TARGET_LOCK =
-          new ShooterSubsystem.ShooterPowers(0, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK =
-          new ShooterSubsystem.ShooterPowers(25, 0.4, .5);
-      public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD =
-          new ShooterSubsystem.ShooterPowers(8, 2.5, .5);
-      public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY =
-          new ShooterSubsystem.ShooterPowers(40, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers SHUTTLE =
-          new ShooterSubsystem.ShooterPowers(30, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE =
-          new ShooterSubsystem.ShooterPowers(30, 1, 0.5);
-      public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE =
-          new ShooterSubsystem.ShooterPowers(76, 1, 0.5);
-      public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY =
-          new ShooterSubsystem.ShooterPowers(30, 1, 0);
-      public static final ShooterSubsystem.ShooterPowers SHOOT_VAR =
-          new ShooterSubsystem.ShooterPowers(30, 1, 0.5);
+    public static final class PoseEstimator {
+        /**
+         * Standard deviations of model states. Increase these numbers to trust your
+         * model's state
+         * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+         * meters and radians.
+         */
+        public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS = MatBuilder.fill(
+                Nat.N3(),
+                Nat.N1(),
+                0.1, // x
+                0.1, // y
+                0.1 // theta
+        );
+
+        /**
+         * Standard deviations of the vision measurements. Increase these numbers to
+         * trust global
+         * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ,
+         * with units in
+         * meters and radians.
+         *
+         * <p>
+         * These are not actually used anymore, but the constructor for the pose
+         * estimator wants
+         * them. This value is calculated dynamically using the below list.
+         */
+        public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS = MatBuilder.fill(
+                Nat.N3(),
+                Nat.N1(),
+                1, // x
+                1, // y
+                1 * Math.PI // theta
+        );
+
+        public static final double POSE_AMBIGUITY_CUTOFF = .05;
+
+        public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS = List.of(
+                // 1 tag
+                new TagCountDeviation(
+                        new UnitDeviationParams(.25, .4, .9),
+                        new UnitDeviationParams(.35, .5, 1.2),
+                        new UnitDeviationParams(.5, .7, 1.5)),
+
+                // 2 tags
+                new TagCountDeviation(
+                        new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)),
+
+                // 3+ tags
+                new TagCountDeviation(
+                        new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5)));
+
+        /** about one inch */
+        public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
+
+        public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
+
+        public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS = List.of(Set.of(1, 2, 3, 4, 5),
+                Set.of(6, 7, 8, 9, 10));
+
+        public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
+
+        public static final int MAX_FRAME_FIDS = 4;
     }
 
-    public static final Slot0Configs ROLLER_PID_CONFIG =
-        new Slot0Configs().withKP(0.2).withKS(0.23).withKV(0.24);
-
-    public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
-  }
-
-  public static final class Pivot {
-    public static final class Ports {
-      public static final int PIVOT_MOTOR_PORT = 18;
-      public static final int CANCODER_PORT = 28;
-      public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
+    public static final class NetworkWatchdog {
+        /** The IP addresses to ping for testing bridging, on the second vlan. */
+        public static final List<IPv4> TEST_IP_ADDRESSES = List.of(IPv4.internal(17), IPv4.internal(18),
+                IPv4.internal(19));
+
+        /**
+         * The number of ms (sleep delta using oshi system uptime) to wait before
+         * beginning to ping the
+         * test IP.
+         */
+        public static final int BOOT_SCAN_DELAY_MS = 50_000;
+
+        /**
+         * The number of seconds for ping to wait before giving up on reaching a device.
+         */
+        public static final int PING_TIMEOUT_SECONDS = 2;
+
+        /** The number of ms to wait before retrying successful health checks. */
+        public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
+
+        /**
+         * The number of ms to leave the switching pdh port off before turning it back
+         * on as part of
+         * rebooting the network switch.
+         */
+        public static final int REBOOT_DURATION_MS = 1_000;
+
+        /**
+         * The number of ms to wait before rerunning health checks after a failed check
+         * which triggered
+         * switch reboot.
+         */
+        public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
     }
 
-    public static final class MotorConfigs {
-      public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR =
-          new MagnetSensorConfigs()
-              .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
-              .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
-              .withMagnetOffset(PIVOT_CANCODER_OFFSET);
-      public static final CANcoderConfiguration CANCODER_CONFIG =
-          new CANcoderConfiguration().withMagnetSensor(CANCODER_MAGNET_SENSOR);
-
-      public static final FeedbackConfigs PIVOT_FEEDBACK =
-          new FeedbackConfigs()
-              .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
-              .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
-              .withSensorToMechanismRatio(1.0)
-              .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
-      public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT =
-          new SoftwareLimitSwitchConfigs()
-              .withForwardSoftLimitThreshold(0.25)
-              .withReverseSoftLimitThreshold(-0.015)
-              .withForwardSoftLimitEnable(false)
-              .withReverseSoftLimitEnable(false);
-      public static final VoltageConfigs PIVOT_VOLTAGE =
-          new VoltageConfigs().withPeakForwardVoltage(5).withPeakReverseVoltage(-5);
-      public static final TalonFXConfiguration PIVOT_CONFIG =
-          new TalonFXConfiguration()
-              .withFeedback(PIVOT_FEEDBACK)
-              .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
-              .withVoltage(PIVOT_VOLTAGE);
+    public static final class CANWatchdog {
+        public static final int SCAN_DELAY_MS = 100;
     }
 
-    public static final class Setpoints {
-      public static final int MINIMUM_ANGLE = 15;
-      public static final int MAXIMUM_ANGLE = 125;
-
-      public static final int MINIMUM_SAFE_THRESHOLD = 15;
-      public static final int MAXIMUM_SAFE_THRESHOLD = 80;
-
-      public static final int SPEAKER_ANGLE = 30;
+    public static final class Lights {
+        public static final int CANDLE_ID = 34;
+        public static final int NUM_LEDS = 89
+                // 8 inside the candle, 8 more for balance
+                + 8 * 2;
+
+        public static final class Colors {
+            public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
+            public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
+            public static final RGBColor RED = new RGBColor(255, 0, 0);
+            public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
+            public static final RGBColor BLUE = new RGBColor(0, 0, 255);
+            public static final RGBColor PINK = new RGBColor(250, 35, 100);
+            public static final RGBColor MINT = new RGBColor(55, 255, 50);
+            public static final RGBColor TEAL = new RGBColor(0, 255, 255);
+            public static final RGBColor WHITE = new RGBColor(255, 255, 255);
+        }
     }
 
-    public static final int EPSILON = 2;
-
-    public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
-    public static final double PIVOT_GEAR_RATIO =
-        (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder values
-    public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
-
-    public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
-    public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
-
-    public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
-    public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
-
-    public static final double GRAVITY = 9.80665; // meters per second
-
-    public static final double GRAVITY_VOLTAGE = 0.4;
-    public static final double PIVOT_MAX_VOLTAGE = 3.5;
-  }
-
-  public static final class Vision {
-    public static record VisionSource(String name, Transform3d robotToCamera) {}
-
-    public static final List<VisionSource> VISION_SOURCES =
-        List.of(
-            /*new VisionSource(
-                "frontCam",
-                new Transform3d(
-                    new Translation3d(
-                        -0.305, // front/back
-                        -0.2286, // left/right
-                        -0.2159 // up/down
-                        ),
-                    new Rotation3d(
-                        0,
-                        Math.toRadians(30), // angle up/down
-                        Math.toRadians(180)))),
-            new VisionSource(
-                "backCam",
-                new Transform3d(
-                    new Translation3d(
-                        -0.2796, // front/back
-                        -0.2286, // left/right
-                        -0.2159 // up/down
-                        ),
-                    new Rotation3d(
-                        0,
-                        Math.toRadians(30), // angle up/down
-                        Math.toRadians(180)))),*/
-            new VisionSource(
-                "backUpCam",
-                new Transform3d(
-                    new Translation3d(
-                        -0.2796, // front/back
-                        0.2286, // left/right
-                        -0.2159 // up/down
-                        ),
-                    new Rotation3d(
-                        0,
-                        Math.toRadians(30), // angle up/down
-                        Math.toRadians(180)))));
-
-    public static final int THREAD_SLEEP_DURATION_MS = 5;
-  }
-
-  public static final class PoseEstimator {
-    /**
-     * Standard deviations of model states. Increase these numbers to trust your model's state
-     * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
-     */
-    public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS =
-        MatBuilder.fill(
-            Nat.N3(),
-            Nat.N1(),
-            0.1, // x
-            0.1, // y
-            0.1 // theta
-            );
-
-    /**
-     * Standard deviations of the vision measurements. Increase these numbers to trust global
-     * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in
-     * meters and radians.
-     *
-     * <p>These are not actually used anymore, but the constructor for the pose estimator wants
-     * them. This value is calculated dynamically using the below list.
-     */
-    public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS =
-        MatBuilder.fill(
-            Nat.N3(),
-            Nat.N1(),
-            1, // x
-            1, // y
-            1 * Math.PI // theta
-            );
-
-    public static final double POSE_AMBIGUITY_CUTOFF = .05;
-
-    public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS =
-        List.of(
-            // 1 tag
-            new TagCountDeviation(
-                new UnitDeviationParams(.25, .4, .9),
-                new UnitDeviationParams(.35, .5, 1.2),
-                new UnitDeviationParams(.5, .7, 1.5)),
-
-            // 2 tags
-            new TagCountDeviation(
-                new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)),
-
-            // 3+ tags
-            new TagCountDeviation(
-                new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5)));
-
-    /** about one inch */
-    public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
-
-    public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
-
-    public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS =
-        List.of(Set.of(1, 2, 3, 4, 5), Set.of(6, 7, 8, 9, 10));
-
-    public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
-
-    public static final int MAX_FRAME_FIDS = 4;
-  }
-
-  public static final class NetworkWatchdog {
-    /** The IP addresses to ping for testing bridging, on the second vlan. */
-    public static final List<IPv4> TEST_IP_ADDRESSES =
-        List.of(IPv4.internal(17), IPv4.internal(18), IPv4.internal(19));
-
-    /**
-     * The number of ms (sleep delta using oshi system uptime) to wait before beginning to ping the
-     * test IP.
-     */
-    public static final int BOOT_SCAN_DELAY_MS = 50_000;
-
-    /** The number of seconds for ping to wait before giving up on reaching a device. */
-    public static final int PING_TIMEOUT_SECONDS = 2;
-
-    /** The number of ms to wait before retrying successful health checks. */
-    public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
-
-    /**
-     * The number of ms to leave the switching pdh port off before turning it back on as part of
-     * rebooting the network switch.
-     */
-    public static final int REBOOT_DURATION_MS = 1_000;
-
-    /**
-     * The number of ms to wait before rerunning health checks after a failed check which triggered
-     * switch reboot.
-     */
-    public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
-  }
-
-  public static final class CANWatchdog {
-    public static final int SCAN_DELAY_MS = 100;
-  }
-
-  public static final class Lights {
-    public static final int CANDLE_ID = 34;
-    public static final int NUM_LEDS =
-        89
-            // 8 inside the candle, 8 more for balance
-            + 8 * 2;
-
-    public static final class Colors {
-      public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
-      public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
-      public static final RGBColor RED = new RGBColor(255, 0, 0);
-      public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
-      public static final RGBColor BLUE = new RGBColor(0, 0, 255);
-      public static final RGBColor PINK = new RGBColor(250, 35, 100);
-      public static final RGBColor MINT = new RGBColor(55, 255, 50);
-      public static final RGBColor TEAL = new RGBColor(0, 255, 255);
-      public static final RGBColor WHITE = new RGBColor(255, 255, 255);
+    public static final class AutoAlign {
+
+        public static final double FIELD_WIDTH = 16.54;
+
+        // Blue team:
+        // pose angles
+        public static final int BOTTOM_MID_ANGLE = 148;
+        public static final int STAGE_ANGLE = 0;
+        public static final int TOP_MID_ANGLE = 0;
+        public static final int AMP_ANGLE = 0;
+
+        // These poses have not been verified
+        public static final Pose2d BLUE_AMP = new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
+        public static final Pose2d BLUE_TOP_MID = new Pose2d(5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
+
+        // These poses have been verified
+        public static final Pose2d BLUE_STAGE = new Pose2d(4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
+        public static final Pose2d BLUE_BOTTOM_MID = new Pose2d(5.84, 1.18,
+                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
+
+        // Red team:
+        // These poses have not been verified
+        public static final Pose2d RED_AMP = new Pose2d(FIELD_WIDTH - 2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE));
+        public static final Pose2d RED_TOP_MID = new Pose2d(FIELD_WIDTH - 5.8, 7.0,
+                Rotation2d.fromDegrees(-TOP_MID_ANGLE));
+
+        // These poses have been verified
+        public static final Pose2d RED_STAGE = new Pose2d(FIELD_WIDTH - 4.17, 4.74,
+                Rotation2d.fromDegrees(-STAGE_ANGLE));
+        public static final Pose2d RED_BOTTOM_MID = new Pose2d(FIELD_WIDTH - 5.84, 1.18,
+                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
+
+        public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
+        public static final double TOP_MID_TARGET_ANGLE = 0;
+        public static final double STAGE_TARGET_ANGLE = 0;
+        public static final double AMP_TARGET_ANGLE = 0;
+        public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540),
+                Units.degreesToRadians(720));
     }
-  }
-
-  public static final class AutoAlign {
-
-    public static final double FIELD_WIDTH = 16.54;
-
-    // Blue team:
-    // pose angles
-    public static final int BOTTOM_MID_ANGLE = 148;
-    public static final int STAGE_ANGLE = 0;
-    public static final int TOP_MID_ANGLE = 0;
-    public static final int AMP_ANGLE = 0;
-
-    // These poses have not been verified
-    public static final Pose2d BLUE_AMP =
-        new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
-    public static final Pose2d BLUE_TOP_MID =
-        new Pose2d(5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
-
-    // These poses have been verified
-    public static final Pose2d BLUE_STAGE =
-        new Pose2d(4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
-    public static final Pose2d BLUE_BOTTOM_MID =
-        new Pose2d(5.84, 1.18, Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
-
-    // Red team:
-    // These poses have not been verified
-    public static final Pose2d RED_AMP =
-        new Pose2d(FIELD_WIDTH - 2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE));
-    public static final Pose2d RED_TOP_MID =
-        new Pose2d(FIELD_WIDTH - 5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE));
-
-    // These poses have been verified
-    public static final Pose2d RED_STAGE =
-        new Pose2d(FIELD_WIDTH - 4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE));
-    public static final Pose2d RED_BOTTOM_MID =
-        new Pose2d(FIELD_WIDTH - 5.84, 1.18, Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
-
-    public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
-    public static final double TOP_MID_TARGET_ANGLE = 0;
-    public static final double STAGE_TARGET_ANGLE = 0;
-    public static final double AMP_TARGET_ANGLE = 0;
-    public static final PathConstraints CONSTRAINTS =
-        new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720));
-    }
-          public static class Elevator{ //Placeholder elevator constant (update as needed)
-            public static final double GEAR_RATIO = 0;
+
+    public static class Elevator { // Placeholder elevator constant (update as needed)
+        public static final double GEAR_RATIO = 0;
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index b728c69..6144ee1 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -16,7 +16,6 @@
 public class IntakeSubsystem extends SubsystemBase {
 
   private final TalonFX intakeMotor;
-  private final TalonFX serializerMotor;
   private final ShuffleboardTab tab = Shuffleboard.getTab("Intake");
   private final DigitalInput noteSensor;
   private IntakeMode intakeMode;
@@ -49,15 +48,11 @@ public IntakePowers(double intakeSpeed, double serializerSpeed) {
   public IntakeSubsystem() {
 
     intakeMotor = new TalonFX(Intake.Ports.INTAKE_MOTOR_PORT);
-    serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT);
     noteSensor = new DigitalInput(Intake.Ports.INTAKE_SENSOR_PORT);
     intakeMotor.clearStickyFaults();
-    serializerMotor.clearStickyFaults();
 
     intakeMotor.setNeutralMode(NeutralModeValue.Brake);
-    serializerMotor.setNeutralMode(NeutralModeValue.Brake);
     intakeMotor.setInverted(true);
-    serializerMotor.setInverted(true);
 
     intakeMode = IntakeMode.HOLD;
 
@@ -65,8 +60,6 @@ public IntakeSubsystem() {
 
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble());
-      tab.addDouble(
-          "Serializer motor voltage", () -> serializerMotor.getMotorVoltage().getValueAsDouble());
       tab.addString("Current Mode", () -> intakeMode.toString());
       tab.addBoolean("Intake Sensor", this::isBeamBreakSensorTriggered);
     }
@@ -88,6 +81,5 @@ private IntakeMode getIntakeMode() {
   @Override
   public void periodic() {
     intakeMotor.set(intakeMode.modePowers.intakeSpeed);
-    serializerMotor.set(intakeMode.modePowers.serializerSpeed);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index b250a62..3be61fd 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.Config;
+import frc.robot.Constants.Intake;
 import frc.robot.Constants.Shooter;
 import java.util.Map;
 
@@ -22,6 +23,7 @@ public class ShooterSubsystem extends SubsystemBase {
   private final TalonFX rollerMotorBottom;
   private final TalonFX rollerMotorTop;
   private final TalonFX acceleratorMotor;
+  private final TalonFX serializerMotor;
 
   private DigitalInput noteSensor;
 
@@ -64,11 +66,12 @@ private ShooterMode(ShooterPowers shooterPowers) {
     }
   }
 
-  public record ShooterPowers(double roller, double topToBottomRatio, double accelerator) {
-    public ShooterPowers(double roller, double topToBottomRatio, double accelerator) {
+  public record ShooterPowers(double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
+    public ShooterPowers(double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
       this.roller = roller;
       this.topToBottomRatio = topToBottomRatio;
       this.accelerator = accelerator;
+      this.serializerSpeed = serializerSpeed;
     }
   }
 
@@ -76,6 +79,7 @@ public ShooterSubsystem() {
     rollerMotorTop = new TalonFX(Shooter.Ports.TOP_SHOOTER_MOTOR_PORT);
     rollerMotorBottom = new TalonFX(Shooter.Ports.BOTTOM_SHOOTER_MOTOR_PORT);
     acceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT);
+    serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT);
 
     noteSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
 
@@ -86,13 +90,16 @@ public ShooterSubsystem() {
     rollerMotorTop.clearStickyFaults();
     acceleratorMotor.clearStickyFaults();
     rollerMotorBottom.clearStickyFaults();
+    serializerMotor.clearStickyFaults();
 
     rollerMotorBottom.setControl(new Follower(rollerMotorTop.getDeviceID(), false));
 
     acceleratorMotor.setInverted(true);
     rollerMotorBottom.setInverted(true);
     rollerMotorTop.setInverted(true);
+    serializerMotor.setInverted(true);
 
+    serializerMotor.setNeutralMode(NeutralModeValue.Brake);
     acceleratorMotor.setNeutralMode(NeutralModeValue.Brake);
     rollerMotorTop.setNeutralMode(NeutralModeValue.Coast);
     rollerMotorBottom.setNeutralMode(NeutralModeValue.Coast);
@@ -106,6 +113,8 @@ public ShooterSubsystem() {
               "Top Roller Velocity (RPS)", () -> rollerMotorTop.getVelocity().getValueAsDouble())
           .withWidget(BuiltInWidgets.kGraph)
           .withSize(2, 1);
+      shooterTab.addDouble(
+          "Serializer motor voltage", () -> serializerMotor.getMotorVoltage().getValueAsDouble());
       shooterTab
           .addDouble(
               "Bottom Roller Velocity (RPS)",
@@ -118,26 +127,23 @@ public ShooterSubsystem() {
           "Bottom roller amps", () -> rollerMotorBottom.getSupplyCurrent().getValueAsDouble());
       shooterTab.addString("mode", () -> shooterMode.toString());
 
-      ampRollerRatioEntry =
-          shooterTab
-              .add("DEBUG Amp Top to Bottom Roller Ratio", 1)
-              .withWidget(BuiltInWidgets.kNumberSlider)
-              .withProperties(Map.of("min", 0, "max", 1))
-              .withSize(3, 1)
-              .getEntry();
-      shooterSpeedEntry =
-          shooterTab
-              .add("DEBUG Shooter Velocity", .5)
-              .withWidget(BuiltInWidgets.kNumberSlider)
-              .withProperties(Map.of("min", 0, "max", 90))
-              .withSize(3, 1)
-              .getEntry();
-      useDebugControls =
-          shooterTab
-              .add("Use Debug Controls", false)
-              .withWidget(BuiltInWidgets.kToggleSwitch)
-              .withSize(2, 1)
-              .getEntry();
+      ampRollerRatioEntry = shooterTab
+          .add("DEBUG Amp Top to Bottom Roller Ratio", 1)
+          .withWidget(BuiltInWidgets.kNumberSlider)
+          .withProperties(Map.of("min", 0, "max", 1))
+          .withSize(3, 1)
+          .getEntry();
+      shooterSpeedEntry = shooterTab
+          .add("DEBUG Shooter Velocity", .5)
+          .withWidget(BuiltInWidgets.kNumberSlider)
+          .withProperties(Map.of("min", 0, "max", 90))
+          .withSize(3, 1)
+          .getEntry();
+      useDebugControls = shooterTab
+          .add("Use Debug Controls", false)
+          .withWidget(BuiltInWidgets.kToggleSwitch)
+          .withSize(2, 1)
+          .getEntry();
     }
   }
 
@@ -167,7 +173,8 @@ public void setShooterMode(ShooterMode shooterMode) {
   }
 
   public void setVariableVelocity(double velocity) {
-    if (shooterMode != ShooterMode.VARIABLE_VELOCITY) shooterMode = ShooterMode.VARIABLE_VELOCITY;
+    if (shooterMode != ShooterMode.VARIABLE_VELOCITY)
+      shooterMode = ShooterMode.VARIABLE_VELOCITY;
     this.variableVelocity = velocity;
   }
 
@@ -214,5 +221,6 @@ public void periodic() {
     }
 
     acceleratorMotor.set(shooterMode.shooterPowers.accelerator());
+    serializerMotor.set(shooterMode.shooterPowers.serializerSpeed());
   }
 }

From 0644c0d05b0e2a7e92811e03b264e5c0d6a3a8e0 Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Mon, 9 Sep 2024 18:52:43 -0700
Subject: [PATCH 06/29] Button bound amp height

---
 src/main/java/frc/robot/Constants.java      | 973 ++++++++++----------
 src/main/java/frc/robot/RobotContainer.java | 956 +++++++++----------
 2 files changed, 982 insertions(+), 947 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 0e2621d..033bec7 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -64,509 +64,534 @@
  */
 public final class Constants {
 
-    public static final class Config {
-        // maybe tune PID values?
-        public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig(
-                new PIDConstants(20, 0, 0),
-                new PIDConstants(10, 0, 0),
-                Drive.MAX_VELOCITY_METERS_PER_SECOND,
-                Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
-                new ReplanningConfig());
-
-        /** turn this off before comp. */
-        public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
-
-        /** turn this off! only use on practice eboard testing. */
-        public static final boolean DISABLE_SWERVE_INIT = false;
-
-        /** keep this on for pigeon, disable if absolutely necessary */
-        public static final boolean FLIP_GYROSCOPE = true;
-
-        /**
-         * def turn this off unless you are using it, generates in excess of 100k rows
-         * for a match.
-         */
-        public static final boolean WRITE_APRILTAG_DATA = false;
-
-        public static final Path APRILTAG_DATA_PATH = Filesystem.getDeployDirectory().toPath()
-                .resolve("poseEstimationsAtDistances.csv");
-        public static final double REAL_X = 0.0;
-        public static final double REAL_Y = 0.0;
-    }
-
-    public static final class Drive {
-        public static final int PIGEON_PORT = 0; // placeholder
-        public static final String SWERVE_CANBUS = "rio"; // placeholder
-
-        // max voltage delivered to drivebase
-        // supposedly useful to limit speed for testing
-        public static final double MAX_VOLTAGE = 12.0;
-        // maximum velocity
-        // FIXME measure this value experimentally
-        public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 // falcon 500 free speed rpm
-                / 60.0
-                * 0.10033
-                * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
-                * Math.PI;
-        // theoretical value
-        // FIXME measure and validate experimentally
-        public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND
-                / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
-                * .5;
-
-        /** the maximum amount of angular error pid loops will tolerate for rotation */
-        public static final double ANGULAR_ERROR = 1.0;
-        /**
-         * the minimum magnitude of the right stick for it to be used as a new rotation
-         * angle
-         */
-        public static final double ROTATE_VECTOR_MAGNITUDE = .7;
-
-        public static final class Dims {
-            // FIXME validate with hardware
-            public static final double TRACKWIDTH_METERS = .5207; // 20.5 inches (source: cad) converted to meters
-            public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
-
-            public static final double BUMPER_WIDTH_METERS_X = .9779;
-            public static final double BUMPER_WIDTH_METERS_Y = .8382;
-        }
+        public static final class Config {
+                // maybe tune PID values?
+                public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig(
+                                new PIDConstants(20, 0, 0),
+                                new PIDConstants(10, 0, 0),
+                                Drive.MAX_VELOCITY_METERS_PER_SECOND,
+                                Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
+                                new ReplanningConfig());
+
+                /** turn this off before comp. */
+                public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
+
+                /** turn this off! only use on practice eboard testing. */
+                public static final boolean DISABLE_SWERVE_INIT = false;
+
+                /** keep this on for pigeon, disable if absolutely necessary */
+                public static final boolean FLIP_GYROSCOPE = true;
+
+                /**
+                 * def turn this off unless you are using it, generates in excess of 100k rows
+                 * for a match.
+                 */
+                public static final boolean WRITE_APRILTAG_DATA = false;
 
-        /*
-         * module layout:
-         * |──────
-         * |->│# ##steer motor
-         * │ │ ##cancoder
-         * │ │##drive motor
-         * module number
-         * 
-         * steer is always left
-         * from corner perspective
-         * 
-         * robot visualization:
-         * |──────────────────────|
-         * │2 10 04 1│
-         * │ 25 24 │
-         * │11 S D 03│
-         * │ D S │
-         * │ │
-         * │ │
-         * │ S D │
-         * │ D S │
-         * │12 |────────| 02│
-         * │ 26 │ │ 27 │
-         * │3 13│ batt │01 4│
-         * |──────┴───┬┬───┴──────|
-         * ││
-         * ││
-         * ▼▼
-         * software front
-         */
-
-        public static final class Modules {
-            public static final class Params {
-                public static final double WHEEL_RADIUS = 2; // also in INCHES
-                public static final double COUPLING_GEAR_RATIO = 3.125;
-                public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
-                public static final double STEER_GEAR_RATIO = 21.428571428571427;
-                public static final Slot0Configs DRIVE_MOTOR_GAINS = new Slot0Configs().withKP(3).withKI(0).withKD(0)
-                        .withKS(0.32).withKV(0.11).withKA(0);
-                public static final Slot0Configs STEER_MOTOR_GAINS = new Slot0Configs().withKP(11).withKI(0).withKD(0)
-                        .withKS(0.4).withKV(0.6).withKA(0);
-                public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
-                public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
-                public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
-                public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
-                public static final double SLIP_CURRENT = 0; // optional, unused rn
-                public static final boolean STEER_MOTOR_INVERTED = true;
-
-                public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
-                public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
-            }
-
-            public static final class Module1 { // back left
-                public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
-                public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
-                public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
-
-                public static final double STEER_OFFSET = IS_COMP_BOT
-                        ? 0.41943359375 // comp bot offset
-                        : 0.0595703125; // practice bot offset
-            }
-
-            public static final class Module2 { // back right
-                public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
-                public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
-                public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
-
-                public static final double STEER_OFFSET = IS_COMP_BOT
-                        ? -0.39990234375 // comp bot offset
-                        : 0.262451171875; // practice bot offset
-            }
-
-            public static final class Module3 { // front right
-                public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
-                public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
-                public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
-
-                public static final double STEER_OFFSET = IS_COMP_BOT
-                        ? 0.225341796875 // comp bot offset
-                        : -0.20825195312; // practice bot offset
-            }
-
-            public static final class Module4 { // front left
-                public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
-                public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
-                public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
-
-                public static final double STEER_OFFSET = IS_COMP_BOT
-                        ? 0.316650390625 // comp bot offset
-                        : -0.3564453125 + 180; // practice bot offset
-            }
+                public static final Path APRILTAG_DATA_PATH = Filesystem.getDeployDirectory().toPath()
+                                .resolve("poseEstimationsAtDistances.csv");
+                public static final double REAL_X = 0.0;
+                public static final double REAL_Y = 0.0;
         }
 
-        public static final class Setpoints {
-            public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
+        public static final class Drive {
+                public static final int PIGEON_PORT = 0; // placeholder
+                public static final String SWERVE_CANBUS = "rio"; // placeholder
+
+                // max voltage delivered to drivebase
+                // supposedly useful to limit speed for testing
+                public static final double MAX_VOLTAGE = 12.0;
+                // maximum velocity
+                // FIXME measure this value experimentally
+                public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 // falcon 500 free speed rpm
+                                / 60.0
+                                * 0.10033
+                                * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
+                                * Math.PI;
+                // theoretical value
+                // FIXME measure and validate experimentally
+                public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND
+                                / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
+                                * .5;
+
+                /** the maximum amount of angular error pid loops will tolerate for rotation */
+                public static final double ANGULAR_ERROR = 1.0;
+                /**
+                 * the minimum magnitude of the right stick for it to be used as a new rotation
+                 * angle
+                 */
+                public static final double ROTATE_VECTOR_MAGNITUDE = .7;
 
-            public static final int SOURCE_DEGREES = 39;
-            public static final int SPEAKER_DEGREES = 11;
-            public static final int EPSILON = 3;
-        }
-    }
+                public static final class Dims {
+                        // FIXME validate with hardware
+                        public static final double TRACKWIDTH_METERS = .5207; // 20.5 inches (source: cad) converted to
+                                                                              // meters
+                        public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
+
+                        public static final double BUMPER_WIDTH_METERS_X = .9779;
+                        public static final double BUMPER_WIDTH_METERS_Y = .8382;
+                }
+
+                /*
+                 * module layout:
+                 * |──────
+                 * |->│# ##steer motor
+                 * │ │ ##cancoder
+                 * │ │##drive motor
+                 * module number
+                 * 
+                 * steer is always left
+                 * from corner perspective
+                 * 
+                 * robot visualization:
+                 * |──────────────────────|
+                 * │2 10 04 1│
+                 * │ 25 24 │
+                 * │11 S D 03│
+                 * │ D S │
+                 * │ │
+                 * │ │
+                 * │ S D │
+                 * │ D S │
+                 * │12 |────────| 02│
+                 * │ 26 │ │ 27 │
+                 * │3 13│ batt │01 4│
+                 * |──────┴───┬┬───┴──────|
+                 * ││
+                 * ││
+                 * ▼▼
+                 * software front
+                 */
 
-    public static final class Intake {
-        public static final class Ports {
-            public static final int INTAKE_MOTOR_PORT = 15;
-            public static final int SERIALIZER_MOTOR_PORT = 16;
-            public static final int INTAKE_SENSOR_PORT = 9;
+                public static final class Modules {
+                        public static final class Params {
+                                public static final double WHEEL_RADIUS = 2; // also in INCHES
+                                public static final double COUPLING_GEAR_RATIO = 3.125;
+                                public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
+                                public static final double STEER_GEAR_RATIO = 21.428571428571427;
+                                public static final Slot0Configs DRIVE_MOTOR_GAINS = new Slot0Configs().withKP(3)
+                                                .withKI(0).withKD(0)
+                                                .withKS(0.32).withKV(0.11).withKA(0);
+                                public static final Slot0Configs STEER_MOTOR_GAINS = new Slot0Configs().withKP(11)
+                                                .withKI(0).withKD(0)
+                                                .withKS(0.4).withKV(0.6).withKA(0);
+                                public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
+                                public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
+                                public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
+                                public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
+                                public static final double SLIP_CURRENT = 0; // optional, unused rn
+                                public static final boolean STEER_MOTOR_INVERTED = true;
+
+                                public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
+                                public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
+                        }
+
+                        public static final class Module1 { // back left
+                                public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
+                                public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
+                                public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
+
+                                public static final double STEER_OFFSET = IS_COMP_BOT
+                                                ? 0.41943359375 // comp bot offset
+                                                : 0.0595703125; // practice bot offset
+                        }
+
+                        public static final class Module2 { // back right
+                                public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
+                                public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
+                                public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
+
+                                public static final double STEER_OFFSET = IS_COMP_BOT
+                                                ? -0.39990234375 // comp bot offset
+                                                : 0.262451171875; // practice bot offset
+                        }
+
+                        public static final class Module3 { // front right
+                                public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
+                                public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
+                                public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
+
+                                public static final double STEER_OFFSET = IS_COMP_BOT
+                                                ? 0.225341796875 // comp bot offset
+                                                : -0.20825195312; // practice bot offset
+                        }
+
+                        public static final class Module4 { // front left
+                                public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
+                                public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
+                                public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
+
+                                public static final double STEER_OFFSET = IS_COMP_BOT
+                                                ? 0.316650390625 // comp bot offset
+                                                : -0.3564453125 + 180; // practice bot offset
+                        }
+                }
+
+                public static final class Setpoints {
+                        public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
+
+                        public static final int SOURCE_DEGREES = 39;
+                        public static final int SPEAKER_DEGREES = 11;
+                        public static final int EPSILON = 3;
+                }
         }
 
-        public static final class Modes {
-            public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
-            public static final IntakePowers HOLD = new IntakePowers(0, 0d);
-            public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-            public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
-            public static final IntakePowers SHOOT_AMP = new IntakePowers(0, 1);
+        public static final class Intake {
+                public static final class Ports {
+                        public static final int INTAKE_MOTOR_PORT = 15;
+                        public static final int SERIALIZER_MOTOR_PORT = 16;
+                        public static final int INTAKE_SENSOR_PORT = 9;
+                }
+
+                public static final class Modes {
+                        public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
+                        public static final IntakePowers HOLD = new IntakePowers(0, 0d);
+                        public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
+                        public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
+                        public static final IntakePowers SHOOT_AMP = new IntakePowers(0, 1);
+                }
+
+                public static final boolean IS_BEAMBREAK = true;
         }
 
-        public static final boolean IS_BEAMBREAK = true;
-    }
+        public static final class Shooter {
+                public static final class Ports {
+                        public static final int TOP_SHOOTER_MOTOR_PORT = 20;
+                        public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19;
+                        public static final int ACCELERATOR_MOTOR_PORT = 17;
+                        public static final int BEAM_BREAK_SENSOR_PORT = 8;
+                }
+
+                public static final class Modes {
+                        public static final ShooterSubsystem.ShooterPowers INTAKE = new ShooterSubsystem.ShooterPowers(
+                                        76, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers IDLE = new ShooterSubsystem.ShooterPowers(0,
+                                        0, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(
+                                        76, 1,
+                                        0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK = new ShooterSubsystem.ShooterPowers(
+                                        25,
+                                        0.4, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT = new ShooterSubsystem.ShooterPowers(
+                                        0, 0,
+                                        0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = new ShooterSubsystem.ShooterPowers(
+                                        76, 1,
+                                        .5, 0);
+                        public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = new ShooterSubsystem.ShooterPowers(
+                                        0, 1, 0,
+                                        0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK = new ShooterSubsystem.ShooterPowers(
+                                        25,
+                                        0.4, .5, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD = new ShooterSubsystem.ShooterPowers(
+                                        8,
+                                        2.5, .5, 0);
+                        public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
+                                        40, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(
+                                        30, 1, 0,
+                                        0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = new ShooterSubsystem.ShooterPowers(
+                                        30, 1,
+                                        0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = new ShooterSubsystem.ShooterPowers(
+                                        76, 1,
+                                        0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = new ShooterSubsystem.ShooterPowers(
+                                        30, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = new ShooterSubsystem.ShooterPowers(
+                                        30, 1,
+                                        0.5, 0);
+                }
+
+                public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23)
+                                .withKV(0.24);
+
+                public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
+        }
 
-    public static final class Shooter {
-        public static final class Ports {
-            public static final int TOP_SHOOTER_MOTOR_PORT = 20;
-            public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19;
-            public static final int ACCELERATOR_MOTOR_PORT = 17;
-            public static final int BEAM_BREAK_SENSOR_PORT = 8;
+        public static final class Pivot {
+                public static final class Ports {
+                        public static final int PIVOT_MOTOR_PORT = 18;
+                        public static final int CANCODER_PORT = 28;
+                        public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
+                }
+
+                public static final class MotorConfigs {
+                        public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR = new MagnetSensorConfigs()
+                                        .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+                                        .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
+                                        .withMagnetOffset(PIVOT_CANCODER_OFFSET);
+                        public static final CANcoderConfiguration CANCODER_CONFIG = new CANcoderConfiguration()
+                                        .withMagnetSensor(CANCODER_MAGNET_SENSOR);
+
+                        public static final FeedbackConfigs PIVOT_FEEDBACK = new FeedbackConfigs()
+                                        .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
+                                        .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+                                        .withSensorToMechanismRatio(1.0)
+                                        .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
+                        public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT = new SoftwareLimitSwitchConfigs()
+                                        .withForwardSoftLimitThreshold(0.25)
+                                        .withReverseSoftLimitThreshold(-0.015)
+                                        .withForwardSoftLimitEnable(false)
+                                        .withReverseSoftLimitEnable(false);
+                        public static final VoltageConfigs PIVOT_VOLTAGE = new VoltageConfigs()
+                                        .withPeakForwardVoltage(5)
+                                        .withPeakReverseVoltage(-5);
+                        public static final TalonFXConfiguration PIVOT_CONFIG = new TalonFXConfiguration()
+                                        .withFeedback(PIVOT_FEEDBACK)
+                                        .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
+                                        .withVoltage(PIVOT_VOLTAGE);
+                }
+
+                public static final class Setpoints {
+                        public static final int MINIMUM_ANGLE = 15;
+                        public static final int MAXIMUM_ANGLE = 125;
+
+                        public static final int MINIMUM_SAFE_THRESHOLD = 15;
+                        public static final int MAXIMUM_SAFE_THRESHOLD = 80;
+
+                        public static final int SPEAKER_ANGLE = 30;
+                }
+
+                public static final int EPSILON = 2;
+
+                public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
+                public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder
+                                                                                                // values
+                public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
+
+                public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
+                public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
+
+                public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
+                public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
+
+                public static final double GRAVITY = 9.80665; // meters per second
+
+                public static final double GRAVITY_VOLTAGE = 0.4;
+                public static final double PIVOT_MAX_VOLTAGE = 3.5;
         }
 
-        public static final class Modes {
-            public static final ShooterSubsystem.ShooterPowers INTAKE = new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
-            public static final ShooterSubsystem.ShooterPowers IDLE = new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
-            public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(76, 1,
-                    0, 0);
-            public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK = new ShooterSubsystem.ShooterPowers(25,
-                    0.4, 0, 0);
-            public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT = new ShooterSubsystem.ShooterPowers(0, 0,
-                    0, 0);
-            public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = new ShooterSubsystem.ShooterPowers(76, 1,
-                    .5, 0);
-            public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = new ShooterSubsystem.ShooterPowers(0, 1, 0,
-                    0);
-            public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK = new ShooterSubsystem.ShooterPowers(25,
-                    0.4, .5, 0);
-            public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD = new ShooterSubsystem.ShooterPowers(8,
-                    2.5, .5, 0);
-            public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
-                    40, 1, 0, 0);
-            public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(30, 1, 0,
-                    0);
-            public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = new ShooterSubsystem.ShooterPowers(30, 1,
-                    0.5, 0);
-            public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = new ShooterSubsystem.ShooterPowers(76, 1,
-                    0.5, 0);
-            public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = new ShooterSubsystem.ShooterPowers(
-                    30, 1, 0, 0);
-            public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = new ShooterSubsystem.ShooterPowers(30, 1,
-                    0.5, 0);
+        public static final class Vision {
+                public static record VisionSource(String name, Transform3d robotToCamera) {
+                }
+
+                public static final List<VisionSource> VISION_SOURCES = List.of(
+                                /*
+                                 * new VisionSource(
+                                 * "frontCam",
+                                 * new Transform3d(
+                                 * new Translation3d(
+                                 * -0.305, // front/back
+                                 * -0.2286, // left/right
+                                 * -0.2159 // up/down
+                                 * ),
+                                 * new Rotation3d(
+                                 * 0,
+                                 * Math.toRadians(30), // angle up/down
+                                 * Math.toRadians(180)))),
+                                 * new VisionSource(
+                                 * "backCam",
+                                 * new Transform3d(
+                                 * new Translation3d(
+                                 * -0.2796, // front/back
+                                 * -0.2286, // left/right
+                                 * -0.2159 // up/down
+                                 * ),
+                                 * new Rotation3d(
+                                 * 0,
+                                 * Math.toRadians(30), // angle up/down
+                                 * Math.toRadians(180)))),
+                                 */
+                                new VisionSource(
+                                                "backUpCam",
+                                                new Transform3d(
+                                                                new Translation3d(
+                                                                                -0.2796, // front/back
+                                                                                0.2286, // left/right
+                                                                                -0.2159 // up/down
+                                                                ),
+                                                                new Rotation3d(
+                                                                                0,
+                                                                                Math.toRadians(30), // angle up/down
+                                                                                Math.toRadians(180)))));
+
+                public static final int THREAD_SLEEP_DURATION_MS = 5;
         }
 
-        public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23).withKV(0.24);
+        public static final class PoseEstimator {
+                /**
+                 * Standard deviations of model states. Increase these numbers to trust your
+                 * model's state
+                 * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+                 * meters and radians.
+                 */
+                public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS = MatBuilder.fill(
+                                Nat.N3(),
+                                Nat.N1(),
+                                0.1, // x
+                                0.1, // y
+                                0.1 // theta
+                );
+
+                /**
+                 * Standard deviations of the vision measurements. Increase these numbers to
+                 * trust global
+                 * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ,
+                 * with units in
+                 * meters and radians.
+                 *
+                 * <p>
+                 * These are not actually used anymore, but the constructor for the pose
+                 * estimator wants
+                 * them. This value is calculated dynamically using the below list.
+                 */
+                public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS = MatBuilder.fill(
+                                Nat.N3(),
+                                Nat.N1(),
+                                1, // x
+                                1, // y
+                                1 * Math.PI // theta
+                );
 
-        public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
-    }
+                public static final double POSE_AMBIGUITY_CUTOFF = .05;
 
-    public static final class Pivot {
-        public static final class Ports {
-            public static final int PIVOT_MOTOR_PORT = 18;
-            public static final int CANCODER_PORT = 28;
-            public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
-        }
+                public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS = List.of(
+                                // 1 tag
+                                new TagCountDeviation(
+                                                new UnitDeviationParams(.25, .4, .9),
+                                                new UnitDeviationParams(.35, .5, 1.2),
+                                                new UnitDeviationParams(.5, .7, 1.5)),
 
-        public static final class MotorConfigs {
-            public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR = new MagnetSensorConfigs()
-                    .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
-                    .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
-                    .withMagnetOffset(PIVOT_CANCODER_OFFSET);
-            public static final CANcoderConfiguration CANCODER_CONFIG = new CANcoderConfiguration()
-                    .withMagnetSensor(CANCODER_MAGNET_SENSOR);
-
-            public static final FeedbackConfigs PIVOT_FEEDBACK = new FeedbackConfigs()
-                    .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
-                    .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
-                    .withSensorToMechanismRatio(1.0)
-                    .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
-            public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT = new SoftwareLimitSwitchConfigs()
-                    .withForwardSoftLimitThreshold(0.25)
-                    .withReverseSoftLimitThreshold(-0.015)
-                    .withForwardSoftLimitEnable(false)
-                    .withReverseSoftLimitEnable(false);
-            public static final VoltageConfigs PIVOT_VOLTAGE = new VoltageConfigs().withPeakForwardVoltage(5)
-                    .withPeakReverseVoltage(-5);
-            public static final TalonFXConfiguration PIVOT_CONFIG = new TalonFXConfiguration()
-                    .withFeedback(PIVOT_FEEDBACK)
-                    .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
-                    .withVoltage(PIVOT_VOLTAGE);
-        }
+                                // 2 tags
+                                new TagCountDeviation(
+                                                new UnitDeviationParams(.35, .1, .4),
+                                                new UnitDeviationParams(.5, .7, 1.5)),
+
+                                // 3+ tags
+                                new TagCountDeviation(
+                                                new UnitDeviationParams(.25, .07, .25),
+                                                new UnitDeviationParams(.15, 1, 1.5)));
 
-        public static final class Setpoints {
-            public static final int MINIMUM_ANGLE = 15;
-            public static final int MAXIMUM_ANGLE = 125;
+                /** about one inch */
+                public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
 
-            public static final int MINIMUM_SAFE_THRESHOLD = 15;
-            public static final int MAXIMUM_SAFE_THRESHOLD = 80;
+                public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
 
-            public static final int SPEAKER_ANGLE = 30;
+                public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS = List.of(Set.of(1, 2, 3, 4, 5),
+                                Set.of(6, 7, 8, 9, 10));
+
+                public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
+
+                public static final int MAX_FRAME_FIDS = 4;
         }
 
-        public static final int EPSILON = 2;
+        public static final class NetworkWatchdog {
+                /** The IP addresses to ping for testing bridging, on the second vlan. */
+                public static final List<IPv4> TEST_IP_ADDRESSES = List.of(IPv4.internal(17), IPv4.internal(18),
+                                IPv4.internal(19));
 
-        public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
-        public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder values
-        public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
+                /**
+                 * The number of ms (sleep delta using oshi system uptime) to wait before
+                 * beginning to ping the
+                 * test IP.
+                 */
+                public static final int BOOT_SCAN_DELAY_MS = 50_000;
 
-        public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
-        public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
+                /**
+                 * The number of seconds for ping to wait before giving up on reaching a device.
+                 */
+                public static final int PING_TIMEOUT_SECONDS = 2;
 
-        public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
-        public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
+                /** The number of ms to wait before retrying successful health checks. */
+                public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
 
-        public static final double GRAVITY = 9.80665; // meters per second
+                /**
+                 * The number of ms to leave the switching pdh port off before turning it back
+                 * on as part of
+                 * rebooting the network switch.
+                 */
+                public static final int REBOOT_DURATION_MS = 1_000;
 
-        public static final double GRAVITY_VOLTAGE = 0.4;
-        public static final double PIVOT_MAX_VOLTAGE = 3.5;
-    }
+                /**
+                 * The number of ms to wait before rerunning health checks after a failed check
+                 * which triggered
+                 * switch reboot.
+                 */
+                public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
+        }
 
-    public static final class Vision {
-        public static record VisionSource(String name, Transform3d robotToCamera) {
+        public static final class CANWatchdog {
+                public static final int SCAN_DELAY_MS = 100;
         }
 
-        public static final List<VisionSource> VISION_SOURCES = List.of(
-                /*
-                 * new VisionSource(
-                 * "frontCam",
-                 * new Transform3d(
-                 * new Translation3d(
-                 * -0.305, // front/back
-                 * -0.2286, // left/right
-                 * -0.2159 // up/down
-                 * ),
-                 * new Rotation3d(
-                 * 0,
-                 * Math.toRadians(30), // angle up/down
-                 * Math.toRadians(180)))),
-                 * new VisionSource(
-                 * "backCam",
-                 * new Transform3d(
-                 * new Translation3d(
-                 * -0.2796, // front/back
-                 * -0.2286, // left/right
-                 * -0.2159 // up/down
-                 * ),
-                 * new Rotation3d(
-                 * 0,
-                 * Math.toRadians(30), // angle up/down
-                 * Math.toRadians(180)))),
-                 */
-                new VisionSource(
-                        "backUpCam",
-                        new Transform3d(
-                                new Translation3d(
-                                        -0.2796, // front/back
-                                        0.2286, // left/right
-                                        -0.2159 // up/down
-                                ),
-                                new Rotation3d(
-                                        0,
-                                        Math.toRadians(30), // angle up/down
-                                        Math.toRadians(180)))));
-
-        public static final int THREAD_SLEEP_DURATION_MS = 5;
-    }
-
-    public static final class PoseEstimator {
-        /**
-         * Standard deviations of model states. Increase these numbers to trust your
-         * model's state
-         * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-         * meters and radians.
-         */
-        public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS = MatBuilder.fill(
-                Nat.N3(),
-                Nat.N1(),
-                0.1, // x
-                0.1, // y
-                0.1 // theta
-        );
-
-        /**
-         * Standard deviations of the vision measurements. Increase these numbers to
-         * trust global
-         * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ,
-         * with units in
-         * meters and radians.
-         *
-         * <p>
-         * These are not actually used anymore, but the constructor for the pose
-         * estimator wants
-         * them. This value is calculated dynamically using the below list.
-         */
-        public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS = MatBuilder.fill(
-                Nat.N3(),
-                Nat.N1(),
-                1, // x
-                1, // y
-                1 * Math.PI // theta
-        );
-
-        public static final double POSE_AMBIGUITY_CUTOFF = .05;
-
-        public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS = List.of(
-                // 1 tag
-                new TagCountDeviation(
-                        new UnitDeviationParams(.25, .4, .9),
-                        new UnitDeviationParams(.35, .5, 1.2),
-                        new UnitDeviationParams(.5, .7, 1.5)),
-
-                // 2 tags
-                new TagCountDeviation(
-                        new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)),
-
-                // 3+ tags
-                new TagCountDeviation(
-                        new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5)));
-
-        /** about one inch */
-        public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
-
-        public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
-
-        public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS = List.of(Set.of(1, 2, 3, 4, 5),
-                Set.of(6, 7, 8, 9, 10));
-
-        public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
-
-        public static final int MAX_FRAME_FIDS = 4;
-    }
-
-    public static final class NetworkWatchdog {
-        /** The IP addresses to ping for testing bridging, on the second vlan. */
-        public static final List<IPv4> TEST_IP_ADDRESSES = List.of(IPv4.internal(17), IPv4.internal(18),
-                IPv4.internal(19));
-
-        /**
-         * The number of ms (sleep delta using oshi system uptime) to wait before
-         * beginning to ping the
-         * test IP.
-         */
-        public static final int BOOT_SCAN_DELAY_MS = 50_000;
-
-        /**
-         * The number of seconds for ping to wait before giving up on reaching a device.
-         */
-        public static final int PING_TIMEOUT_SECONDS = 2;
-
-        /** The number of ms to wait before retrying successful health checks. */
-        public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
-
-        /**
-         * The number of ms to leave the switching pdh port off before turning it back
-         * on as part of
-         * rebooting the network switch.
-         */
-        public static final int REBOOT_DURATION_MS = 1_000;
-
-        /**
-         * The number of ms to wait before rerunning health checks after a failed check
-         * which triggered
-         * switch reboot.
-         */
-        public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
-    }
-
-    public static final class CANWatchdog {
-        public static final int SCAN_DELAY_MS = 100;
-    }
-
-    public static final class Lights {
-        public static final int CANDLE_ID = 34;
-        public static final int NUM_LEDS = 89
-                // 8 inside the candle, 8 more for balance
-                + 8 * 2;
-
-        public static final class Colors {
-            public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
-            public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
-            public static final RGBColor RED = new RGBColor(255, 0, 0);
-            public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
-            public static final RGBColor BLUE = new RGBColor(0, 0, 255);
-            public static final RGBColor PINK = new RGBColor(250, 35, 100);
-            public static final RGBColor MINT = new RGBColor(55, 255, 50);
-            public static final RGBColor TEAL = new RGBColor(0, 255, 255);
-            public static final RGBColor WHITE = new RGBColor(255, 255, 255);
+        public static final class Lights {
+                public static final int CANDLE_ID = 34;
+                public static final int NUM_LEDS = 89
+                                // 8 inside the candle, 8 more for balance
+                                + 8 * 2;
+
+                public static final class Colors {
+                        public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
+                        public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
+                        public static final RGBColor RED = new RGBColor(255, 0, 0);
+                        public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
+                        public static final RGBColor BLUE = new RGBColor(0, 0, 255);
+                        public static final RGBColor PINK = new RGBColor(250, 35, 100);
+                        public static final RGBColor MINT = new RGBColor(55, 255, 50);
+                        public static final RGBColor TEAL = new RGBColor(0, 255, 255);
+                        public static final RGBColor WHITE = new RGBColor(255, 255, 255);
+                }
+        }
+
+        public static final class AutoAlign {
+
+                public static final double FIELD_WIDTH = 16.54;
+
+                // Blue team:
+                // pose angles
+                public static final int BOTTOM_MID_ANGLE = 148;
+                public static final int STAGE_ANGLE = 0;
+                public static final int TOP_MID_ANGLE = 0;
+                public static final int AMP_ANGLE = 0;
+
+                // These poses have not been verified
+                public static final Pose2d BLUE_AMP = new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
+                public static final Pose2d BLUE_TOP_MID = new Pose2d(5.8, 7.0,
+                                Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
+
+                // These poses have been verified
+                public static final Pose2d BLUE_STAGE = new Pose2d(4.17, 4.74,
+                                Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
+                public static final Pose2d BLUE_BOTTOM_MID = new Pose2d(5.84, 1.18,
+                                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
+
+                // Red team:
+                // These poses have not been verified
+                public static final Pose2d RED_AMP = new Pose2d(FIELD_WIDTH - 2.75, 7.31,
+                                Rotation2d.fromDegrees(-AMP_ANGLE));
+                public static final Pose2d RED_TOP_MID = new Pose2d(FIELD_WIDTH - 5.8, 7.0,
+                                Rotation2d.fromDegrees(-TOP_MID_ANGLE));
+
+                // These poses have been verified
+                public static final Pose2d RED_STAGE = new Pose2d(FIELD_WIDTH - 4.17, 4.74,
+                                Rotation2d.fromDegrees(-STAGE_ANGLE));
+                public static final Pose2d RED_BOTTOM_MID = new Pose2d(FIELD_WIDTH - 5.84, 1.18,
+                                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
+
+                public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
+                public static final double TOP_MID_TARGET_ANGLE = 0;
+                public static final double STAGE_TARGET_ANGLE = 0;
+                public static final double AMP_TARGET_ANGLE = 0;
+                public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540),
+                                Units.degreesToRadians(720));
+        }
+
+        public static class Elevator { // Placeholder elevator constant (update as needed)
+                public static final double GEAR_RATIO = 0;
+                public static final double MAX_HEIGHT = 50;
         }
-    }
-
-    public static final class AutoAlign {
-
-        public static final double FIELD_WIDTH = 16.54;
-
-        // Blue team:
-        // pose angles
-        public static final int BOTTOM_MID_ANGLE = 148;
-        public static final int STAGE_ANGLE = 0;
-        public static final int TOP_MID_ANGLE = 0;
-        public static final int AMP_ANGLE = 0;
-
-        // These poses have not been verified
-        public static final Pose2d BLUE_AMP = new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
-        public static final Pose2d BLUE_TOP_MID = new Pose2d(5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
-
-        // These poses have been verified
-        public static final Pose2d BLUE_STAGE = new Pose2d(4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
-        public static final Pose2d BLUE_BOTTOM_MID = new Pose2d(5.84, 1.18,
-                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
-
-        // Red team:
-        // These poses have not been verified
-        public static final Pose2d RED_AMP = new Pose2d(FIELD_WIDTH - 2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE));
-        public static final Pose2d RED_TOP_MID = new Pose2d(FIELD_WIDTH - 5.8, 7.0,
-                Rotation2d.fromDegrees(-TOP_MID_ANGLE));
-
-        // These poses have been verified
-        public static final Pose2d RED_STAGE = new Pose2d(FIELD_WIDTH - 4.17, 4.74,
-                Rotation2d.fromDegrees(-STAGE_ANGLE));
-        public static final Pose2d RED_BOTTOM_MID = new Pose2d(FIELD_WIDTH - 5.84, 1.18,
-                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
-
-        public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
-        public static final double TOP_MID_TARGET_ANGLE = 0;
-        public static final double STAGE_TARGET_ANGLE = 0;
-        public static final double AMP_TARGET_ANGLE = 0;
-        public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540),
-                Units.degreesToRadians(720));
-    }
-
-    public static class Elevator { // Placeholder elevator constant (update as needed)
-        public static final double GEAR_RATIO = 0;
-    }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 127a3c9..2f23599 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -37,6 +37,7 @@
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
+import frc.robot.commands.HeightCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.MaintainShooterCommand;
 import frc.robot.commands.OuttakeCommand;
@@ -56,6 +57,7 @@
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.NetworkWatchdogSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
@@ -72,483 +74,491 @@
 import java.util.function.DoubleSupplier;
 
 /**
- * This class is where the bulk of the robot should be declared. Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * This class is where the bulk of the robot should be declared. Since
+ * Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in
+ * the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of
+ * the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-
-  private final VisionSubsystem visionSubsystem = new VisionSubsystem();
-
-  private final DrivebaseSubsystem drivebaseSubsystem = new DrivebaseSubsystem(visionSubsystem);
-
-  private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
-
-  private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
-
-  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
-
-  private final RGBSubsystem rgbSubsystem = new RGBSubsystem();
-
-  private final NetworkWatchdogSubsystem networkWatchdogSubsystem =
-      new NetworkWatchdogSubsystem(Optional.of(rgbSubsystem));
-
-  private final CANWatchdogSubsystem canWatchdogSubsystem = new CANWatchdogSubsystem(rgbSubsystem);
-
-  /** controller 1 */
-  private final CommandXboxController jacob = new CommandXboxController(1);
-  /** controller 1 layer */
-  private final Layer jacobLayer = new Layer(jacob.rightBumper());
-  /** controller 0 */
-  private final CommandXboxController anthony = new CommandXboxController(0);
-  /** controller 0 layer */
-  //   private final Layer anthonyLayer = new Layer(anthony.rightBumper());
-
-  /** the sendable chooser to select which auto to run. */
-  private final SendableChooser<Command> autoSelector;
-
-  private GenericEntry autoDelay;
-
-  private Pose2d desiredPose;
-
-  private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView");
-
-  /* drive joystick "y" is passed to x because controller is inverted */
-  private final DoubleSupplier translationXSupplier =
-      () -> (-modifyAxis(anthony.getLeftY()) * Drive.MAX_VELOCITY_METERS_PER_SECOND);
-  private final DoubleSupplier translationYSupplier =
-      () -> (-modifyAxis(anthony.getLeftX()) * Drive.MAX_VELOCITY_METERS_PER_SECOND);
-
-  /** The container for the robot. Contains subsystems, OI devices, and commands. */
-  public RobotContainer() {
-
-    // reigster commands for pathplanner
-    NamedCommands.registerCommand(
-        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-    NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem, intakeSubsystem));
-    NamedCommands.registerCommand(
-        "ShooterRampUpCommand",
-        new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
-    NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
-    NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
-    NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
-    NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
-    NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
-    NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
-    NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
-    NamedCommands.registerCommand(
-        "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
-    NamedCommands.registerCommand(
-        "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
-    NamedCommands.registerCommand(
-        "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
-    NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
-    NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
-    NamedCommands.registerCommand(
-        "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
-    NamedCommands.registerCommand(
-        "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
-    NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
-    NamedCommands.registerCommand(
-        "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
-    NamedCommands.registerCommand(
-        "ZeroSubwoofer1",
-        new InstantCommand(
-            () ->
-                drivebaseSubsystem.zeroGyroscopeOffset(
-                    DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
-    NamedCommands.registerCommand(
-        "ZeroSubwoofer3",
-        new InstantCommand(
-            () ->
-                drivebaseSubsystem.zeroGyroscopeOffset(
-                    DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
-    NamedCommands.registerCommand(
-        "recenterPose1",
-        DriverStation.getAlliance().get().equals(Alliance.Blue)
-            ? new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
-                drivebaseSubsystem)
-            : new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
-                drivebaseSubsystem));
-    NamedCommands.registerCommand(
-        "recenterPose2",
-        DriverStation.getAlliance().get().equals(Alliance.Blue)
-            ? new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
-                drivebaseSubsystem)
-            : new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
-                drivebaseSubsystem));
-    NamedCommands.registerCommand(
-        "recenterPose3",
-        DriverStation.getAlliance().get().equals(Alliance.Blue)
-            ? new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
-                drivebaseSubsystem)
-            : new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
-                drivebaseSubsystem));
-    NamedCommands.registerCommand(
-        "recenterPose4",
-        DriverStation.getAlliance().get().equals(Alliance.Blue)
-            ? new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
-                drivebaseSubsystem)
-            : new InstantCommand(
-                () ->
-                    drivebaseSubsystem.resetOdometryToPose(
-                        new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
-                drivebaseSubsystem));
-
-    // Set up the default command for the drivetrain.
-    // The controls are for field-oriented driving:
-    // Left stick Y axis -> forward and backwards movement
-    // Left stick X axis -> left and right movement
-    // Right stick X axis -> rotation
-    drivebaseSubsystem.setDefaultCommand(
-        new DefaultDriveCommand(
-            drivebaseSubsystem,
-            translationXSupplier,
-            translationYSupplier,
-            // anthony.rightBumper(),
-            anthony.leftBumper()));
-
-    rgbSubsystem.setDefaultCommand(
-        new RGBCommand(
-            shooterSubsystem,
-            intakeSubsystem,
-            rgbSubsystem,
-            pivotSubsystem,
-            drivebaseSubsystem,
-            visionSubsystem));
-
-    // pivotSubsystem.setDefaultCommand(
-    //     new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY()));
-
-    // Configure the button bindings
-    configureButtonBindings();
-
-    autoSelector = AutoBuilder.buildAutoChooser();
-
-    SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT);
-    SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA);
-    SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_INIT);
-
-    desiredPose = new Pose2d();
-    SmartDashboard.putString(
-        "desired pose",
-        String.format(
-            "(%2f %2f %2f)",
-            desiredPose.getX(), desiredPose.getY(), desiredPose.getRotation().getDegrees()));
-
-    if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
-      driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
-      driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
-      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
-      driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
-    }
-
-    // Create and put autonomous selector to dashboard
-    setupAutonomousCommands();
-  }
-
-  /**
-   * Use this method to do things as the drivers gain control of the robot. We use it to vibrate the
-   * driver b controller to notice accidental swaps.
-   *
-   * <p>Please use this very, very sparingly. It doesn't exist by default for good reason.
-   */
-  public void containerTeleopInit() {
-    // runs when teleop happens
-    CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jacob.getHID(), 5, .5));
-    // vibrate controller at 27 seconds left
-    CommandScheduler.getInstance()
-        .schedule(
-            new WaitCommand(108)
-                .andThen(
-                    new ParallelCommandGroup(
-                        new VibrateHIDCommand(anthony.getHID(), 3, 0.4),
-                        new VibrateHIDCommand(jacob.getHID(), 3, 0.4))));
-  }
-
-  /**
-   * Use this method to do things as soon as the robot starts being used. We use it to stop doing
-   * things that could be harmful or undesirable during game play--rebooting the network switch is a
-   * good example. Subsystems need to be explicitly wired up to this method.
-   *
-   * <p>Depending on which mode the robot is enabled in, this will either be called before auto or
-   * before teleop, whichever is first.
-   *
-   * <p>Please use this very, very sparingly. It doesn't exist by default for good reason.
-   */
-  public void containerMatchStarting() {
-    // runs when the match starts
-    networkWatchdogSubsystem.matchStarting();
-    canWatchdogSubsystem.matchStarting();
-  }
-
-  /**
-   * Use this method to define your button->command mappings. Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * 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() {
-    // vibrate jacob controller when in layer
-    jacobLayer.whenChanged(
-        (enabled) -> {
-          final double power = enabled ? .1 : 0;
-          jacob.getHID().setRumble(RumbleType.kLeftRumble, power);
-          jacob.getHID().setRumble(RumbleType.kRightRumble, power);
-        });
-
-    anthony
-        .start()
-        .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem));
-
-    jacob
-        .start()
-        .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem));
-
-    // STOP INTAKE-SHOOTER
-    jacob
-        .x()
-        .onTrue(
-            new StopShooterCommand(shooterSubsystem)
-                .alongWith(new StopIntakeCommand(intakeSubsystem)));
-    // OUTTAKE
-    jacob.rightBumper().onTrue(new OuttakeCommand(intakeSubsystem));
-
-    // INTAKE
-    anthony
-        .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-
-    jacob
-        .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-
-    // SHOOT
-    anthony
-        .rightBumper()
-        .onTrue(
-            new AccelNoteCommand(shooterSubsystem)
-                .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem))
-                .andThen(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
-
-    // SHOOT OVERRIDE
-    jacob
-        .rightTrigger()
-        .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem, intakeSubsystem)));
-
-    anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
-    anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
-    jacob
-        .y()
-        .whileTrue(
-            new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
-                .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
-
-    jacob
-        .a()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem,
-                    translationXSupplier,
-                    translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
-                .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
-                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE)));
-
-    jacob
-        .povDown()
-        .onTrue(
-            new PivotAngleCommand(pivotSubsystem, 15)
-                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
-
-    // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem, translationXSupplier,
-    // translationYSupplier, Setpoints.SPEAKER));
-
-    // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
-
-    // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
-    //     .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
-
-    DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
-
-    new Trigger(() -> Math.abs(pivotManualRate.getAsDouble()) > 0.07)
-        .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
-
-    // SOURCE
-    anthony
-        .y()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem,
-                    translationXSupplier,
-                    translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red)
-                        ? -Setpoints.SOURCE_DEGREES
-                        : Setpoints.SOURCE_DEGREES)
-                .alongWith(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
-
-    // SPEAKER FROM STAGE
-    anthony
-        .b()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem,
-                    translationXSupplier,
-                    translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red)
-                        ? -Setpoints.SPEAKER_DEGREES
-                        : Setpoints.SPEAKER_DEGREES)
-                .alongWith(new PivotAngleCommand(pivotSubsystem, 25.1)));
-
-    // AMP
-    jacob
-        .b()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem,
-                    translationXSupplier,
-                    translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -90 : 90)
-                .alongWith(new PivotAngleCommand(pivotSubsystem, 50)) // FIXME idk
-                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_BACK)));
-
-    /*    jacob
-            .a()
-            .onTrue(
-                new RotateAngleDriveCommand(
+    // The robot's subsystems and commands are defined here...
+
+    private final VisionSubsystem visionSubsystem = new VisionSubsystem();
+
+    private final DrivebaseSubsystem drivebaseSubsystem = new DrivebaseSubsystem(visionSubsystem);
+
+    private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+
+    private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
+
+    private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+
+    private final RGBSubsystem rgbSubsystem = new RGBSubsystem();
+
+    private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
+
+    private final NetworkWatchdogSubsystem networkWatchdogSubsystem = new NetworkWatchdogSubsystem(
+            Optional.of(rgbSubsystem));
+
+    private final CANWatchdogSubsystem canWatchdogSubsystem = new CANWatchdogSubsystem(rgbSubsystem);
+
+    /** controller 1 */
+    private final CommandXboxController jacob = new CommandXboxController(1);
+    /** controller 1 layer */
+    private final Layer jacobLayer = new Layer(jacob.rightBumper());
+    /** controller 0 */
+    private final CommandXboxController anthony = new CommandXboxController(0);
+    /** controller 0 layer */
+    // private final Layer anthonyLayer = new Layer(anthony.rightBumper());
+
+    /** the sendable chooser to select which auto to run. */
+    private final SendableChooser<Command> autoSelector;
+
+    private GenericEntry autoDelay;
+
+    private Pose2d desiredPose;
+
+    private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView");
+
+    /* drive joystick "y" is passed to x because controller is inverted */
+    private final DoubleSupplier translationXSupplier = () -> (-modifyAxis(anthony.getLeftY())
+            * Drive.MAX_VELOCITY_METERS_PER_SECOND);
+    private final DoubleSupplier translationYSupplier = () -> (-modifyAxis(anthony.getLeftX())
+            * Drive.MAX_VELOCITY_METERS_PER_SECOND);
+
+    /**
+     * The container for the robot. Contains subsystems, OI devices, and commands.
+     */
+    public RobotContainer() {
+
+        // reigster commands for pathplanner
+        NamedCommands.registerCommand(
+                "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem, intakeSubsystem));
+        NamedCommands.registerCommand(
+                "ShooterRampUpCommand",
+                new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+        NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
+        NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
+        NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
+        NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
+        NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
+        NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
+        NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
+        NamedCommands.registerCommand(
+                "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
+        NamedCommands.registerCommand(
+                "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
+        NamedCommands.registerCommand(
+                "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
+        NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
+        NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
+        NamedCommands.registerCommand(
+                "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
+        NamedCommands.registerCommand(
+                "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
+        NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
+        NamedCommands.registerCommand(
+                "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
+        NamedCommands.registerCommand(
+                "ZeroSubwoofer1",
+                new InstantCommand(
+                        () -> drivebaseSubsystem.zeroGyroscopeOffset(
+                                DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
+        NamedCommands.registerCommand(
+                "ZeroSubwoofer3",
+                new InstantCommand(
+                        () -> drivebaseSubsystem.zeroGyroscopeOffset(
+                                DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
+        NamedCommands.registerCommand(
+                "recenterPose1",
+                DriverStation.getAlliance().get().equals(Alliance.Blue)
+                        ? new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
+                                drivebaseSubsystem)
+                        : new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
+                                drivebaseSubsystem));
+        NamedCommands.registerCommand(
+                "recenterPose2",
+                DriverStation.getAlliance().get().equals(Alliance.Blue)
+                        ? new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
+                                drivebaseSubsystem)
+                        : new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
+                                drivebaseSubsystem));
+        NamedCommands.registerCommand(
+                "recenterPose3",
+                DriverStation.getAlliance().get().equals(Alliance.Blue)
+                        ? new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
+                                drivebaseSubsystem)
+                        : new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
+                                drivebaseSubsystem));
+        NamedCommands.registerCommand(
+                "recenterPose4",
+                DriverStation.getAlliance().get().equals(Alliance.Blue)
+                        ? new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
+                                drivebaseSubsystem)
+                        : new InstantCommand(
+                                () -> drivebaseSubsystem.resetOdometryToPose(
+                                        new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
+                                drivebaseSubsystem));
+
+        // Set up the default command for the drivetrain.
+        // The controls are for field-oriented driving:
+        // Left stick Y axis -> forward and backwards movement
+        // Left stick X axis -> left and right movement
+        // Right stick X axis -> rotation
+        drivebaseSubsystem.setDefaultCommand(
+                new DefaultDriveCommand(
                         drivebaseSubsystem,
                         translationXSupplier,
                         translationYSupplier,
-                        DriverStation.getAlliance().get().equals(Alliance.Red) ? 90 : -90)
-                    .alongWith(new PivotAngleCommand(pivotSubsystem, 138)) // FIXME idk
-                    .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_FRONT)));
-    */
-    // SPEAKER FROM SUBWOOFER
-    anthony
-        .a()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem, translationXSupplier, translationYSupplier, 0)
-                .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1)));
-
-    anthony
-        .x()
-        .onTrue(
-            new RotateAngleDriveCommand(
-                    drivebaseSubsystem,
-                    translationXSupplier,
-                    translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
-                .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1)));
-
-    DoubleSupplier rotation =
-        exponential(
-            () ->
-                ControllerUtil.deadband(
-                    (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1),
-            2);
-
-    DoubleSupplier rotationVelocity =
-        () -> -rotation.getAsDouble() * Drive.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * 0.8;
-
-    new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
-        .whileTrue(
-            new RotateVelocityDriveCommand(
-                drivebaseSubsystem,
-                translationXSupplier,
-                translationYSupplier,
-                rotationVelocity,
-                anthony.rightBumper()));
-
-    new Trigger(
-            () ->
-                Util.vectorMagnitude(anthony.getRightY(), anthony.getRightX())
-                    > Drive.ROTATE_VECTOR_MAGNITUDE)
-        .onTrue(
-            new RotateVectorDriveCommand(
-                drivebaseSubsystem,
-                translationXSupplier,
-                translationYSupplier,
-                anthony::getRightY,
-                anthony::getRightX,
-                anthony.rightBumper()));
-  }
-
-  /**
-   * Adds all autonomous routines to the autoSelector, and places the autoSelector on Shuffleboard.
-   */
-  private void setupAutonomousCommands() {
-    driverView.addString("NOTES", () -> "...win?\nor not.").withSize(4, 1).withPosition(7, 2);
-
-    driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);
-
-    autoDelay =
-        driverView
-            .add("auto delay", 0)
-            .withWidget(BuiltInWidgets.kNumberSlider)
-            .withProperties(Map.of("min", 0, "max", 15, "block increment", .1))
-            .withSize(4, 1)
-            .withPosition(7, 1)
-            .getEntry();
-  }
-
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
-  public Command getAutonomousCommand() {
-    double delay = autoDelay.getDouble(0);
-    return delay == 0
-        ? autoSelector.getSelected()
-        : new WaitCommand(delay).andThen(autoSelector.getSelected());
-  }
-
-  /**
-   * applies deadband and squares axis
-   *
-   * @param value the axis value to be modified
-   * @return the modified axis values
-   */
-  private static double modifyAxis(double value) {
-    // Deadband
-    value = ControllerUtil.deadband(value, 0.07);
-
-    // Square the axis
-    value = Math.copySign(value * value, value);
-
-    return value;
-  }
-
-  private static DoubleSupplier exponential(DoubleSupplier supplier, double exponential) {
-    return () -> {
-      double val = supplier.getAsDouble();
-      return Math.copySign(Math.pow(val, exponential), val);
-    };
-  }
+                        // anthony.rightBumper(),
+                        anthony.leftBumper()));
+
+        rgbSubsystem.setDefaultCommand(
+                new RGBCommand(
+                        shooterSubsystem,
+                        intakeSubsystem,
+                        rgbSubsystem,
+                        pivotSubsystem,
+                        drivebaseSubsystem,
+                        visionSubsystem));
+
+        // pivotSubsystem.setDefaultCommand(
+        // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY()));
+
+        // Configure the button bindings
+        configureButtonBindings();
+
+        autoSelector = AutoBuilder.buildAutoChooser();
+
+        SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT);
+        SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA);
+        SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_INIT);
+
+        desiredPose = new Pose2d();
+        SmartDashboard.putString(
+                "desired pose",
+                String.format(
+                        "(%2f %2f %2f)",
+                        desiredPose.getX(), desiredPose.getY(), desiredPose.getRotation().getDegrees()));
+
+        if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
+            driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
+            driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
+            driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
+            driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
+        }
+
+        // Create and put autonomous selector to dashboard
+        setupAutonomousCommands();
+    }
+
+    /**
+     * Use this method to do things as the drivers gain control of the robot. We use
+     * it to vibrate the
+     * driver b controller to notice accidental swaps.
+     *
+     * <p>
+     * Please use this very, very sparingly. It doesn't exist by default for good
+     * reason.
+     */
+    public void containerTeleopInit() {
+        // runs when teleop happens
+        CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jacob.getHID(), 5, .5));
+        // vibrate controller at 27 seconds left
+        CommandScheduler.getInstance()
+                .schedule(
+                        new WaitCommand(108)
+                                .andThen(
+                                        new ParallelCommandGroup(
+                                                new VibrateHIDCommand(anthony.getHID(), 3, 0.4),
+                                                new VibrateHIDCommand(jacob.getHID(), 3, 0.4))));
+    }
+
+    /**
+     * Use this method to do things as soon as the robot starts being used. We use
+     * it to stop doing
+     * things that could be harmful or undesirable during game play--rebooting the
+     * network switch is a
+     * good example. Subsystems need to be explicitly wired up to this method.
+     *
+     * <p>
+     * Depending on which mode the robot is enabled in, this will either be called
+     * before auto or
+     * before teleop, whichever is first.
+     *
+     * <p>
+     * Please use this very, very sparingly. It doesn't exist by default for good
+     * reason.
+     */
+    public void containerMatchStarting() {
+        // runs when the match starts
+        networkWatchdogSubsystem.matchStarting();
+        canWatchdogSubsystem.matchStarting();
+    }
+
+    /**
+     * Use this method to define your button->command mappings. Buttons can be
+     * created by
+     * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * 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() {
+        // vibrate jacob controller when in layer
+        jacobLayer.whenChanged(
+                (enabled) -> {
+                    final double power = enabled ? .1 : 0;
+                    jacob.getHID().setRumble(RumbleType.kLeftRumble, power);
+                    jacob.getHID().setRumble(RumbleType.kRightRumble, power);
+                });
+
+        anthony
+                .start()
+                .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem));
+
+        jacob
+                .start()
+                .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem));
+
+        // STOP INTAKE-SHOOTER
+        jacob
+                .x()
+                .onTrue(
+                        new StopShooterCommand(shooterSubsystem)
+                                .alongWith(new StopIntakeCommand(intakeSubsystem)));
+        // OUTTAKE
+        jacob.rightBumper().onTrue(new OuttakeCommand(intakeSubsystem));
+
+        // INTAKE
+        anthony
+                .leftBumper()
+                .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+
+        jacob
+                .leftBumper()
+                .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+
+        // SHOOT
+        anthony
+                .rightBumper()
+                .onTrue(
+                        new AccelNoteCommand(shooterSubsystem)
+                                .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem))
+                                .andThen(
+                                        new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+
+        // SHOOT OVERRIDE
+        jacob
+                .rightTrigger()
+                .onTrue(new AccelNoteCommand(shooterSubsystem)
+                        .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem)));
+
+        anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
+        anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
+        jacob
+                .y()
+                .whileTrue(
+                        new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
+                                .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
+
+        jacob
+                .a()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
+                                .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
+                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE)));
+
+        jacob
+                .povDown()
+                .onTrue(
+                        new PivotAngleCommand(pivotSubsystem, 15)
+                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
+
+        // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
+        // translationXSupplier,
+        // translationYSupplier, Setpoints.SPEAKER));
+
+        // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
+
+        // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
+        // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
+
+        DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
+
+        new Trigger(() -> Math.abs(pivotManualRate.getAsDouble()) > 0.07)
+                .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
+
+        // SOURCE
+        anthony
+                .y()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                DriverStation.getAlliance().get().equals(Alliance.Red)
+                                        ? -Setpoints.SOURCE_DEGREES
+                                        : Setpoints.SOURCE_DEGREES)
+                                .alongWith(
+                                        new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+
+        // SPEAKER FROM STAGE
+        anthony
+                .b()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                DriverStation.getAlliance().get().equals(Alliance.Red)
+                                        ? -Setpoints.SPEAKER_DEGREES
+                                        : Setpoints.SPEAKER_DEGREES)
+                                .alongWith(new PivotAngleCommand(pivotSubsystem, 25.1)));
+
+        // AMP
+        jacob
+                .b()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -90 : 90)
+                                .alongWith(new PivotAngleCommand(pivotSubsystem, 50)) // FIXME idk
+                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_BACK)));
+
+        /*
+         * jacob
+         * .a()
+         * .onTrue(
+         * new RotateAngleDriveCommand(
+         * drivebaseSubsystem,
+         * translationXSupplier,
+         * translationYSupplier,
+         * DriverStation.getAlliance().get().equals(Alliance.Red) ? 90 : -90)
+         * .alongWith(new PivotAngleCommand(pivotSubsystem, 138)) // FIXME idk
+         * .alongWith(new ShooterRampUpCommand(shooterSubsystem,
+         * ShooterMode.RAMP_AMP_FRONT)));
+         */
+        // SPEAKER FROM SUBWOOFER
+        anthony
+                .a()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem, translationXSupplier, translationYSupplier, 0)
+                                .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1)));
+
+        anthony
+                .x()
+                .onTrue(
+                        new RotateAngleDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+                                .alongWith(new HeightCommand(elevatorSubsystem, 20)));
+
+        DoubleSupplier rotation = exponential(
+                () -> ControllerUtil.deadband(
+                        (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1),
+                2);
+
+        DoubleSupplier rotationVelocity = () -> -rotation.getAsDouble() * Drive.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
+                * 0.8;
+
+        new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
+                .whileTrue(
+                        new RotateVelocityDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                rotationVelocity,
+                                anthony.rightBumper()));
+
+        new Trigger(
+                () -> Util.vectorMagnitude(anthony.getRightY(), anthony.getRightX()) > Drive.ROTATE_VECTOR_MAGNITUDE)
+                .onTrue(
+                        new RotateVectorDriveCommand(
+                                drivebaseSubsystem,
+                                translationXSupplier,
+                                translationYSupplier,
+                                anthony::getRightY,
+                                anthony::getRightX,
+                                anthony.rightBumper()));
+    }
+
+    /**
+     * Adds all autonomous routines to the autoSelector, and places the autoSelector
+     * on Shuffleboard.
+     */
+    private void setupAutonomousCommands() {
+        driverView.addString("NOTES", () -> "...win?\nor not.").withSize(4, 1).withPosition(7, 2);
+
+        driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);
+
+        autoDelay = driverView
+                .add("auto delay", 0)
+                .withWidget(BuiltInWidgets.kNumberSlider)
+                .withProperties(Map.of("min", 0, "max", 15, "block increment", .1))
+                .withSize(4, 1)
+                .withPosition(7, 1)
+                .getEntry();
+    }
+
+    /**
+     * Use this to pass the autonomous command to the main {@link Robot} class.
+     *
+     * @return the command to run in autonomous
+     */
+    public Command getAutonomousCommand() {
+        double delay = autoDelay.getDouble(0);
+        return delay == 0
+                ? autoSelector.getSelected()
+                : new WaitCommand(delay).andThen(autoSelector.getSelected());
+    }
+
+    /**
+     * applies deadband and squares axis
+     *
+     * @param value the axis value to be modified
+     * @return the modified axis values
+     */
+    private static double modifyAxis(double value) {
+        // Deadband
+        value = ControllerUtil.deadband(value, 0.07);
+
+        // Square the axis
+        value = Math.copySign(value * value, value);
+
+        return value;
+    }
+
+    private static DoubleSupplier exponential(DoubleSupplier supplier, double exponential) {
+        return () -> {
+            double val = supplier.getAsDouble();
+            return Math.copySign(Math.pow(val, exponential), val);
+        };
+    }
 }

From dc278ff0d33bc28bf8eb9f7d746aa51eb3df7553 Mon Sep 17 00:00:00 2001
From: Samantha <27nguyens28@stu.smuhsd.org>
Date: Tue, 10 Sep 2024 18:50:22 -0700
Subject: [PATCH 07/29] Added TransferNoteCommand

Moves note from serializer to accelerator
---
 src/main/java/frc/robot/Constants.java        |  2 +
 .../robot/commands/TransferNoteCommand.java   | 43 +++++++++++++++++++
 .../robot/subsystems/ShooterSubsystem.java    |  3 +-
 3 files changed, 47 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/commands/TransferNoteCommand.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 033bec7..3689471 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -304,6 +304,8 @@ public static final class Modes {
                         public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = new ShooterSubsystem.ShooterPowers(
                                         30, 1,
                                         0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = new ShooterSubsystem.ShooterPowers(
+                                        0, 0, 0.5, 0.5);
                 }
 
                 public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23)
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
new file mode 100644
index 0000000..b04f4ef
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -0,0 +1,43 @@
+package frc.robot.commands;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.DrivebaseSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
+
+public class TransferNoteCommand extends Command {
+
+  ShooterSubsystem shooterSubsystem;
+  DrivebaseSubsystem drivebaseSubsystem;
+  DoubleSupplier translationXSupplier;
+  DoubleSupplier translationYSupplier;
+
+  public TransferNoteCommand(DrivebaseSubsystem drivebaseSubsystem, DoubleSupplier translationXSupplier,
+      DoubleSupplier translationYSupplier) {
+    this.drivebaseSubsystem = drivebaseSubsystem;
+    this.translationXSupplier = translationXSupplier;
+    this.translationYSupplier = translationYSupplier;
+
+  }
+
+  @Override
+  public void execute() {
+
+  }
+
+  @Override
+  public void initialize() {
+    shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  @Override
+  public boolean isFinished() {
+    return shooterSubsystem.isReadyToShoot();
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 3be61fd..44f181e 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -57,7 +57,8 @@ public enum ShooterMode {
     SHOOT_SHUTTLE(Shooter.Modes.SHOOT_SHUTTLE),
     ACCEL_SECURE(Shooter.Modes.ACCEL_SECURE),
     VARIABLE_VELOCITY(Shooter.Modes.VARIABLE_VELOCITY),
-    SHOOT_VAR(Shooter.Modes.SHOOT_VAR);
+    SHOOT_VAR(Shooter.Modes.SHOOT_VAR),
+    LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER);
 
     public final ShooterPowers shooterPowers;
 

From 3ed4742ac90ba3a94fc56c4b9a4ddae439de6182 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Tue, 10 Sep 2024 19:56:02 -0700
Subject: [PATCH 08/29] Elevator subsystem uses gear ratios and works better

---
 src/main/java/frc/robot/Constants.java        | 24 +++++--
 .../robot/subsystems/ElevatorSubsystem.java   | 63 +++++++++++--------
 2 files changed, 56 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 033bec7..0db874f 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -359,7 +359,8 @@ public static final class Setpoints {
                 public static final int EPSILON = 2;
 
                 public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
-                public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder
+                /** degrees per rotation */
+                public static final double PIVOT_GEAR_RATIO = 5+(1/3); //not used 
                                                                                                 // values
                 public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
 
@@ -590,8 +591,23 @@ public static final class AutoAlign {
                                 Units.degreesToRadians(720));
         }
 
-        public static class Elevator { // Placeholder elevator constant (update as needed)
-                public static final double GEAR_RATIO = 0;
-                public static final double MAX_HEIGHT = 50;
+        public static class Elevator { 
+                public static final class Ports {
+                        public static final int ARM_MOTOR_PORT = 18;
+                        public static final int ELEVATOR_MOTOR_PORT = 28;
+                }
+                /**inches per rotation */
+                public static final double ELEVATOR_GEAR_RATIO = 0.0906065;
+                /**FIXME */
+                public static final double MAX_HEIGHT = 50; 
+                /**FIXME */
+                public static final double ELEVATOR_FEEDFORWARD = 0.01;
+
+                public static final class Arm{
+                        /**FIXME */
+                        public static final double MAX_ANGLE = 50; 
+                        /**inches per rotation */
+                        public static final double ARM_GEAR_RATIO = 0.0231481481;
+                }
         }
 }
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 92846f2..1823e7b 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -1,6 +1,8 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import edu.wpi.first.math.MathUtil;
 // import com.ctre.phoenix6.signals.NeutralModeValue;
 // import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -13,11 +15,13 @@
 
 public class ElevatorSubsystem extends SubsystemBase {
 
-    private TalonFX motor;
+    private TalonFX armMotor;
+    private TalonFX elevatorMotor;
     private PIDController controller;
     private double targetHeight;
     private double targetAngle;
-    private double motorPower;
+    private double elevatorMotorPower;
+    private double armMotorPower;
     private double currentHeight;
     private double currentAngle;
     private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
@@ -25,13 +29,17 @@ public class ElevatorSubsystem extends SubsystemBase {
     /** Creates a new ElevatorSubsystem. */
     public ElevatorSubsystem() {
 
-        motor = new TalonFX(15);
+        elevatorMotor = new TalonFX(Elevator.Ports.ELEVATOR_MOTOR_PORT);
+        armMotor = new TalonFX(Elevator.Ports.ARM_MOTOR_PORT);
+
+        elevatorMotor.clearStickyFaults();
+        armMotor.clearStickyFaults();
+        elevatorMotor.setPosition(0);
+        armMotor.setPosition(0);
 
-        motor.clearStickyFaults();
 
         controller = new PIDController(0.4, 0, 0.0125);
 
-        ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
         ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
         ElevatorTab.addNumber("Target Angle", () -> this.targetAngle);
         // ElevatorTab.addNumber("PID output", () -> this.controller);
@@ -45,47 +53,48 @@ public ElevatorSubsystem() {
     }
 
     public static double inchesToRotations(double height) {
-        return height / Elevator.GEAR_RATIO;
+        return height / Elevator.ELEVATOR_GEAR_RATIO;
     }
 
     public static double rotationsToInches(double rotations) {
-        return rotations * Elevator.GEAR_RATIO;
+        return rotations * Elevator.ELEVATOR_GEAR_RATIO;
     }
 
-    public static double degreesToRotations(double degrees) {
-        return degrees / Elevator.GEAR_RATIO;
+    public static double armInchesToRotations(double degrees) {
+        return degrees / Elevator.Arm.ARM_GEAR_RATIO;
     }
 
-    public static double rotationsToDegrees(double rotations) {
-        return rotations * Elevator.GEAR_RATIO;
+    public static double rotationsToArmInches(double rotations) {
+        return rotations * Elevator.Arm.ARM_GEAR_RATIO;
     }
 
     public void setTargetHeight(double targetHeight) {
-        this.targetHeight = targetHeight;
-    }
+        this.targetHeight = MathUtil.clamp(targetHeight, Elevator.MAX_HEIGHT, 0);
 
-    public void setTargetAngle(double targetAngle) {
-        this.targetAngle = targetAngle;
     }
 
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
-        motorPower = controller.calculate(inchesToRotations(targetHeight), targetHeight);
-        motorPower = controller.calculate(degreesToRotations(targetAngle), targetAngle);
+    public void setTargetAngle(double targetAngle) {
+        this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0);
     }
 
-    public boolean nearTargetHeight() {
-        if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) {
+    public boolean nearTarget(double target, double current) {
+        if (target - 0.5 <= current && current <= target + 0.5) {
             return true;
         }
         return false;
     }
 
-    public boolean nearTargetAngle() {
-        if (targetAngle - 1 <= currentAngle && currentAngle <= targetAngle + 1) {
-            return true;
-        }
-        return false;
+    public double calculateFeedforward(){
+        return 0; //FIXME
+    }
+
+    @Override
+    public void periodic() {
+        // This method will be called once per scheduler run
+        elevatorMotorPower = controller.calculate(inchesToRotations(elevatorMotor.getPosition().getValueAsDouble()), targetHeight);
+        armMotorPower = controller.calculate(armInchesToRotations(armMotor.getPosition().getValueAsDouble()), targetAngle);
+        elevatorMotor.setVoltage(MathUtil.clamp(elevatorMotorPower+Elevator.ELEVATOR_FEEDFORWARD, -10, 10));
+        armMotor.setVoltage(MathUtil.clamp(armMotorPower+calculateFeedforward(), -10, 10));
     }
+
 }

From c02da348d740d1b25f30f8973e93cb1d84c836f5 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Tue, 10 Sep 2024 20:42:40 -0700
Subject: [PATCH 09/29] Changed subsystem of serializer beam break to shooter
 subsystem

---
 src/main/java/frc/robot/Constants.java        |  2 ++
 src/main/java/frc/robot/RobotContainer.java   | 13 +++-------
 .../frc/robot/commands/IntakeCommand.java     |  2 +-
 .../java/frc/robot/commands/RGBCommand.java   | 16 ++++++-------
 .../frc/robot/commands/ShootAmpCommand.java   |  2 +-
 .../java/frc/robot/commands/ShootCommand.java |  2 +-
 .../robot/commands/TransferNoteCommand.java   | 18 +++++++-------
 .../frc/robot/subsystems/IntakeSubsystem.java |  7 ++----
 .../robot/subsystems/ShooterSubsystem.java    | 24 +++++++++++++------
 9 files changed, 43 insertions(+), 43 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 3689471..002c26a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -306,6 +306,8 @@ public static final class Modes {
                                         0.5, 0);
                         public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = new ShooterSubsystem.ShooterPowers(
                                         0, 0, 0.5, 0.5);
+                        public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD = new ShooterSubsystem.ShooterPowers(
+                                        0, 0, -0.5, -0.5);
                 }
 
                 public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23)
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2f23599..46799a8 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -54,6 +54,7 @@
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
 import frc.robot.commands.TargetLockCommand;
+import frc.robot.commands.TransferNoteCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -424,19 +425,11 @@ private void configureButtonBindings() {
                                 .alongWith(
                                         new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
 
-        // SPEAKER FROM STAGE
+        // NOTE TO SHOOTER OR SERIALIZER
         anthony
                 .b()
                 .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                DriverStation.getAlliance().get().equals(Alliance.Red)
-                                        ? -Setpoints.SPEAKER_DEGREES
-                                        : Setpoints.SPEAKER_DEGREES)
-                                .alongWith(new PivotAngleCommand(pivotSubsystem, 25.1)));
-
+                        new TransferNoteCommand(shooterSubsystem));
         // AMP
         jacob
                 .b()
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index 41187ed..a3e34a3 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -53,6 +53,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return intakeSubsystem.isBeamBreakSensorTriggered();
+    return shooterSubsystem.isSerializerBeamBreakSensorTriggered();
   }
 }
diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java
index e9335d7..000077a 100644
--- a/src/main/java/frc/robot/commands/RGBCommand.java
+++ b/src/main/java/frc/robot/commands/RGBCommand.java
@@ -58,8 +58,8 @@ public void initialize() {}
   public void execute() {
     // two note = yellow
 
-    if (shooterSubsystem.isBeamBreakSensorTriggered()
-        && intakeSubsystem.isBeamBreakSensorTriggered()
+    if (shooterSubsystem.isShooterBeamBreakSensorTriggered()
+        && shooterSubsystem.isSerializerBeamBreakSensorTriggered()
         && twoNoteMsg.isEmpty()) {
       twoNoteMsg =
           Optional.of(
@@ -67,15 +67,15 @@ public void execute() {
                   Constants.Lights.Colors.YELLOW,
                   RGBSubsystem.PatternTypes.PULSE,
                   RGBSubsystem.MessagePriority.C_TWO_NOTE_WARNING));
-    } else if (!(shooterSubsystem.isBeamBreakSensorTriggered()
-        && intakeSubsystem.isBeamBreakSensorTriggered())) {
+    } else if (!(shooterSubsystem.isShooterBeamBreakSensorTriggered()
+        && shooterSubsystem.isSerializerBeamBreakSensorTriggered())) {
       twoNoteMsg.ifPresent(RGBMessage::expire);
       twoNoteMsg = Optional.empty();
     }
 
     // serializer = blue
-    if ((intakeSubsystem.isBeamBreakSensorTriggered()
-            || shooterSubsystem.isBeamBreakSensorTriggered())
+    if ((shooterSubsystem.isShooterBeamBreakSensorTriggered()
+            || shooterSubsystem.isSerializerBeamBreakSensorTriggered())
         && noteInRobotMsg.isEmpty()) {
       /*|| shooterSubsystem.isBeamBreakSensorTriggered()*/
       noteInRobotMsg =
@@ -84,8 +84,8 @@ public void execute() {
                   Constants.Lights.Colors.WHITE,
                   RGBSubsystem.PatternTypes.STROBE,
                   RGBSubsystem.MessagePriority.F_NOTE_IN_ROBOT));
-    } else if (!(intakeSubsystem.isBeamBreakSensorTriggered()
-        || shooterSubsystem.isBeamBreakSensorTriggered())) {
+    } else if (!(shooterSubsystem.isShooterBeamBreakSensorTriggered()
+        || shooterSubsystem.isSerializerBeamBreakSensorTriggered())) {
       noteInRobotMsg.ifPresent(RGBMessage::expire);
       noteInRobotMsg = Optional.empty();
     }
diff --git a/src/main/java/frc/robot/commands/ShootAmpCommand.java b/src/main/java/frc/robot/commands/ShootAmpCommand.java
index aa7136e..0a9582c 100644
--- a/src/main/java/frc/robot/commands/ShootAmpCommand.java
+++ b/src/main/java/frc/robot/commands/ShootAmpCommand.java
@@ -39,6 +39,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return !shooterSubsystem.isBeamBreakSensorTriggered();
+    return !shooterSubsystem.isShooterBeamBreakSensorTriggered();
   }
 }
diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index 4778a69..2aaa59e 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -44,6 +44,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return !shooterSubsystem.isBeamBreakSensorTriggered();
+    return !shooterSubsystem.isShooterBeamBreakSensorTriggered();
   }
 }
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index b04f4ef..4671d85 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -10,16 +10,9 @@
 public class TransferNoteCommand extends Command {
 
   ShooterSubsystem shooterSubsystem;
-  DrivebaseSubsystem drivebaseSubsystem;
-  DoubleSupplier translationXSupplier;
-  DoubleSupplier translationYSupplier;
-
-  public TransferNoteCommand(DrivebaseSubsystem drivebaseSubsystem, DoubleSupplier translationXSupplier,
-      DoubleSupplier translationYSupplier) {
-    this.drivebaseSubsystem = drivebaseSubsystem;
-    this.translationXSupplier = translationXSupplier;
-    this.translationYSupplier = translationYSupplier;
 
+  public TransferNoteCommand(ShooterSubsystem shooterSubsystem) {
+    this.shooterSubsystem = shooterSubsystem;
   }
 
   @Override
@@ -29,7 +22,12 @@ public void execute() {
 
   @Override
   public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
+      shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+    }
+    else{
+      shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
+    }
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 6144ee1..ba27594 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -61,7 +61,7 @@ public IntakeSubsystem() {
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble());
       tab.addString("Current Mode", () -> intakeMode.toString());
-      tab.addBoolean("Intake Sensor", this::isBeamBreakSensorTriggered);
+
     }
   }
 
@@ -69,10 +69,7 @@ public void setIntakeMode(IntakeMode intakeMode) {
     this.intakeMode = intakeMode;
   }
 
-  public boolean isBeamBreakSensorTriggered() {
-    // if is triggered return true
-    return !noteSensor.get();
-  }
+
 
   private IntakeMode getIntakeMode() {
     return intakeMode;
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 44f181e..dc57d63 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -25,7 +25,9 @@ public class ShooterSubsystem extends SubsystemBase {
   private final TalonFX acceleratorMotor;
   private final TalonFX serializerMotor;
 
-  private DigitalInput noteSensor;
+  private DigitalInput shooterSensor;
+  private DigitalInput serializerSensor;
+
 
   private ShooterMode shooterMode;
 
@@ -58,7 +60,8 @@ public enum ShooterMode {
     ACCEL_SECURE(Shooter.Modes.ACCEL_SECURE),
     VARIABLE_VELOCITY(Shooter.Modes.VARIABLE_VELOCITY),
     SHOOT_VAR(Shooter.Modes.SHOOT_VAR),
-    LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER);
+    LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER),
+    SHOOTER_UNLOAD(Shooter.Modes.SHOOTER_UNLOAD);
 
     public final ShooterPowers shooterPowers;
 
@@ -82,7 +85,9 @@ public ShooterSubsystem() {
     acceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT);
     serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT);
 
-    noteSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+
 
     // rollerMotorTop.getConfigurator().apply(new TalonFXConfiguration());
     // rollerMotorBottom.getConfigurator().apply(new TalonFXConfiguration());
@@ -108,7 +113,7 @@ public ShooterSubsystem() {
     shooterMode = ShooterMode.IDLE;
 
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
-      shooterTab.addBoolean("Sensor Input", this::isBeamBreakSensorTriggered);
+      shooterTab.addBoolean("Sensor Input", this::isShooterBeamBreakSensorTriggered);
       shooterTab
           .addDouble(
               "Top Roller Velocity (RPS)", () -> rollerMotorTop.getVelocity().getValueAsDouble())
@@ -157,12 +162,17 @@ public boolean isShooterUpToSpeed() {
         && rollerMotorTop.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD;
   }
 
-  public boolean isBeamBreakSensorTriggered() {
-    return !noteSensor.get();
+  public boolean isShooterBeamBreakSensorTriggered() {
+    return !shooterSensor.get();
+  }
+
+  public boolean isSerializerBeamBreakSensorTriggered() {
+    // if is triggered return true
+    return !serializerSensor.get();
   }
 
   public boolean isReadyToShoot() {
-    return isBeamBreakSensorTriggered() && isShooterUpToSpeed();
+    return isShooterBeamBreakSensorTriggered() && isShooterUpToSpeed();
   }
 
   public void haltAccelerator() {

From f5740da51ec66926675c96b5f6f9f65de5aa0297 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Tue, 10 Sep 2024 21:09:21 -0700
Subject: [PATCH 10/29] Serializer to shooter and vise versa code

---
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../robot/commands/LoadShooterCommand.java    | 37 ++++++++++++++
 .../robot/commands/TransferNoteCommand.java   | 40 ++++------------
 .../robot/commands/UnloadShooterCommand.java  | 48 +++++++++++++++++++
 4 files changed, 95 insertions(+), 32 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/LoadShooterCommand.java
 create mode 100644 src/main/java/frc/robot/commands/UnloadShooterCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 46799a8..dd44370 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -429,7 +429,7 @@ private void configureButtonBindings() {
         anthony
                 .b()
                 .onTrue(
-                        new TransferNoteCommand(shooterSubsystem));
+                        new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem));
         // AMP
         jacob
                 .b()
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
new file mode 100644
index 0000000..a0172e4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -0,0 +1,37 @@
+// 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.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
+
+public class LoadShooterCommand extends Command {
+  /** Creates a new LoadShooterCommand. */
+  ShooterSubsystem shooterSubsystem;
+  public LoadShooterCommand(ShooterSubsystem shooterSubsystem) {
+    this.shooterSubsystem = shooterSubsystem;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return shooterSubsystem.isShooterBeamBreakSensorTriggered();
+  }
+}
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index 4671d85..5107094 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -1,41 +1,19 @@
 package frc.robot.commands;
 
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.DrivebaseSubsystem;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-public class TransferNoteCommand extends Command {
-
-  ShooterSubsystem shooterSubsystem;
 
-  public TransferNoteCommand(ShooterSubsystem shooterSubsystem) {
-    this.shooterSubsystem = shooterSubsystem;
-  }
-
-  @Override
-  public void execute() {
+public class TransferNoteCommand extends SequentialCommandGroup {
 
-  }
-
-  @Override
-  public void initialize() {
+  public TransferNoteCommand(
+    ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, PivotSubsystem pivotSubsystem) {
     if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
-      shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+      addCommands(new LoadShooterCommand(shooterSubsystem));
     }
-    else{
-      shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
+    else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()){
+      addCommands(new UnloadShooterCommand(shooterSubsystem, pivotSubsystem).andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
     }
   }
-
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  @Override
-  public boolean isFinished() {
-    return shooterSubsystem.isReadyToShoot();
-  }
 }
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
new file mode 100644
index 0000000..bbd50eb
--- /dev/null
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -0,0 +1,48 @@
+// 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.subsystems.PivotSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
+
+public class UnloadShooterCommand extends Command {
+  /** Creates a new UnloadShooterCommand. */
+  ShooterSubsystem shooterSubsystem;
+  PivotSubsystem pivotSubsystem;
+  boolean pass;
+  public UnloadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem) {
+    this.shooterSubsystem = shooterSubsystem;
+    this.pivotSubsystem = pivotSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered();
+    shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
+    pivotSubsystem.setTargetDegrees(20);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()&&pass==false){
+      pass=true;
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return !shooterSubsystem.isSerializerBeamBreakSensorTriggered()&&pass==true;
+  }
+}

From 06d11d5e250f2da0dde81d5eab9071c478e1307b Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Wed, 11 Sep 2024 19:48:35 -0700
Subject: [PATCH 11/29] Removed some unused stuff and made shoot command

---
 src/main/java/frc/robot/Constants.java        | 25 ++++++-----------
 src/main/java/frc/robot/RobotContainer.java   | 28 ++-----------------
 .../frc/robot/commands/IntakeCommand.java     |  2 +-
 .../robot/commands/LoadShooterCommand.java    |  4 ++-
 .../java/frc/robot/commands/RGBCommand.java   |  4 +--
 .../frc/robot/commands/ShootAmpCommand.java   |  6 +---
 .../java/frc/robot/commands/ShootCommand.java |  5 ----
 .../robot/commands/UnloadShooterCommand.java  |  4 ++-
 .../frc/robot/subsystems/IntakeSubsystem.java |  3 --
 .../robot/subsystems/ShooterSubsystem.java    | 15 ++++------
 10 files changed, 25 insertions(+), 71 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 002c26a..a7c4864 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -95,7 +95,7 @@ public static final class Config {
         }
 
         public static final class Drive {
-                public static final int PIGEON_PORT = 0; // placeholder
+                public static final int PIGEON_PORT = 0; // FIXME
                 public static final String SWERVE_CANBUS = "rio"; // placeholder
 
                 // max voltage delivered to drivebase
@@ -247,8 +247,8 @@ public static final class Modes {
                         public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
                         public static final IntakePowers HOLD = new IntakePowers(0, 0d);
                         public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-                        public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 1);
-                        public static final IntakePowers SHOOT_AMP = new IntakePowers(0, 1);
+                        public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 0.7);
+                        public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1);
                 }
 
                 public static final boolean IS_BEAMBREAK = true;
@@ -270,24 +270,15 @@ public static final class Modes {
                         public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(
                                         76, 1,
                                         0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP_BACK = new ShooterSubsystem.ShooterPowers(
-                                        25,
-                                        0.4, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP_FRONT = new ShooterSubsystem.ShooterPowers(
-                                        0, 0,
-                                        0, 0);
                         public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = new ShooterSubsystem.ShooterPowers(
                                         76, 1,
                                         .5, 0);
                         public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = new ShooterSubsystem.ShooterPowers(
                                         0, 1, 0,
                                         0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_BACK = new ShooterSubsystem.ShooterPowers(
-                                        25,
-                                        0.4, .5, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP_FORWARD = new ShooterSubsystem.ShooterPowers(
-                                        8,
-                                        2.5, .5, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = new ShooterSubsystem.ShooterPowers(
+                                        0,
+                                        0, 0, 0);
                         public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
                                         40, 1, 0, 0);
                         public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(
@@ -363,7 +354,7 @@ public static final class Setpoints {
                 public static final int EPSILON = 2;
 
                 public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
-                public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); // FIXME placeholder
+                public static final double PIVOT_GEAR_RATIO = (60 / 8) * (60 / 16) * (72 / 15); //  not used
                                                                                                 // values
                 public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
 
@@ -594,7 +585,7 @@ public static final class AutoAlign {
                                 Units.degreesToRadians(720));
         }
 
-        public static class Elevator { // Placeholder elevator constant (update as needed)
+        public static class Elevator { // FIXME elevator constant (update as needed)
                 public static final double GEAR_RATIO = 0;
                 public static final double MAX_HEIGHT = 50;
         }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index dd44370..88f036e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -430,31 +430,9 @@ private void configureButtonBindings() {
                 .b()
                 .onTrue(
                         new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem));
-        // AMP
-        jacob
-                .b()
-                .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -90 : 90)
-                                .alongWith(new PivotAngleCommand(pivotSubsystem, 50)) // FIXME idk
-                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_AMP_BACK)));
-
-        /*
-         * jacob
-         * .a()
-         * .onTrue(
-         * new RotateAngleDriveCommand(
-         * drivebaseSubsystem,
-         * translationXSupplier,
-         * translationYSupplier,
-         * DriverStation.getAlliance().get().equals(Alliance.Red) ? 90 : -90)
-         * .alongWith(new PivotAngleCommand(pivotSubsystem, 138)) // FIXME idk
-         * .alongWith(new ShooterRampUpCommand(shooterSubsystem,
-         * ShooterMode.RAMP_AMP_FRONT)));
-         */
+
+
+
         // SPEAKER FROM SUBWOOFER
         anthony
                 .a()
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index a3e34a3..3ab8173 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -46,7 +46,7 @@ public void execute() {}
   @Override
   public void end(boolean interrupted) {
     intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD);
-    shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER);
+    shooterSubsystem.setShooterMode(ShooterMode.IDLE);
     shooterSubsystem.haltAccelerator();
   }
 
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index a0172e4..2b09144 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -27,7 +27,9 @@ public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER);
+  }
 
   // Returns true when the command should end.
   @Override
diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java
index 000077a..528ccac 100644
--- a/src/main/java/frc/robot/commands/RGBCommand.java
+++ b/src/main/java/frc/robot/commands/RGBCommand.java
@@ -102,9 +102,7 @@ public void execute() {
                   Constants.Lights.Colors.RED,
                   RGBSubsystem.PatternTypes.STROBE,
                   RGBSubsystem.MessagePriority.D_READY_TO_SHOOT));
-    } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER) || //if it shot the note, it is not ready to shoot 
-      shooterSubsystem.getMode().equals(ShooterMode.SHOOT_AMP_BACK)||           //instead of relying on beam break sensor input
-      shooterSubsystem.getMode().equals(ShooterMode.SHOOT_AMP_FORWARD)||
+    } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER) || //if it shot the note, it is not ready to shoot instead of relying on beam break sensor input
       shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)||
       shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)||
       pivotSubsystem.isAtTargetDegrees()){
diff --git a/src/main/java/frc/robot/commands/ShootAmpCommand.java b/src/main/java/frc/robot/commands/ShootAmpCommand.java
index 0a9582c..a178b05 100644
--- a/src/main/java/frc/robot/commands/ShootAmpCommand.java
+++ b/src/main/java/frc/robot/commands/ShootAmpCommand.java
@@ -21,11 +21,7 @@ public ShootAmpCommand(ShooterSubsystem shooterSubsystem) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    if (shooterSubsystem.getMode().equals(ShooterMode.RAMP_AMP_BACK)) {
-      shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP_BACK);
-    } else {
-      shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP_FORWARD);
-    }
+    shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index 2aaa59e..a63beca 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -8,16 +8,13 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 
 public class ShootCommand extends Command {
   private ShooterSubsystem shooterSubsystem;
-  private IntakeSubsystem intakeSubsystem;
 
   /** Creates a new ShootCommand. */
   public ShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
-    this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     addRequirements(shooterSubsystem);
   }
@@ -26,8 +23,6 @@ public ShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSub
   @Override
   public void initialize() {
     shooterSubsystem.advanceToShootMode();
-    intakeSubsystem.setIntakeMode(IntakeMode.SHOOT_SPEAKER);
-    intakeSubsystem.setIntakeMode(IntakeMode.SHOOT_AMP);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
index bbd50eb..37992b4 100644
--- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -38,7 +38,9 @@ public void execute() {
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    shooterSubsystem.setShooterMode(ShooterMode.IDLE);
+  }
 
   // Returns true when the command should end.
   @Override
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index ba27594..af35d82 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -6,7 +6,6 @@
 
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -17,7 +16,6 @@ public class IntakeSubsystem extends SubsystemBase {
 
   private final TalonFX intakeMotor;
   private final ShuffleboardTab tab = Shuffleboard.getTab("Intake");
-  private final DigitalInput noteSensor;
   private IntakeMode intakeMode;
   private IntakeMode pastMode;
   private double timeSincePenaltyHazard;
@@ -48,7 +46,6 @@ public IntakePowers(double intakeSpeed, double serializerSpeed) {
   public IntakeSubsystem() {
 
     intakeMotor = new TalonFX(Intake.Ports.INTAKE_MOTOR_PORT);
-    noteSensor = new DigitalInput(Intake.Ports.INTAKE_SENSOR_PORT);
     intakeMotor.clearStickyFaults();
 
     intakeMotor.setNeutralMode(NeutralModeValue.Brake);
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index dc57d63..7ed9c2a 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -49,11 +49,8 @@ public enum ShooterMode {
     INTAKE(Shooter.Modes.INTAKE),
     IDLE(Shooter.Modes.IDLE),
     RAMP_SPEAKER(Shooter.Modes.RAMP_SPEAKER),
-    RAMP_AMP_BACK(Shooter.Modes.RAMP_AMP_BACK),
-    RAMP_AMP_FRONT(Shooter.Modes.RAMP_AMP_FRONT),
     SHOOT_SPEAKER(Shooter.Modes.SHOOT_SPEAKER),
-    SHOOT_AMP_BACK(Shooter.Modes.SHOOT_AMP_BACK),
-    SHOOT_AMP_FORWARD(Shooter.Modes.SHOOT_AMP_FORWARD),
+    SHOOT_AMP(Shooter.Modes.SHOOT_AMP),
     MAINTAIN_VELOCITY(Shooter.Modes.MAINTAIN_VELOCITY),
     SHUTTLE(Shooter.Modes.SHUTTLE),
     SHOOT_SHUTTLE(Shooter.Modes.SHOOT_SHUTTLE),
@@ -191,12 +188,10 @@ public void setVariableVelocity(double velocity) {
 
   public void advanceToShootMode() {
     switch (shooterMode) {
-      case RAMP_AMP_FRONT:
-        shooterMode = ShooterMode.SHOOT_AMP_FORWARD;
-        break;
-      case RAMP_AMP_BACK:
-        shooterMode = ShooterMode.SHOOT_AMP_BACK;
-        break;
+      case IDLE:
+      if (isSerializerBeamBreakSensorTriggered()){
+        shooterMode = ShooterMode.SHOOT_AMP;
+      }
       case SHUTTLE:
         shooterMode = ShooterMode.SHOOT_SHUTTLE;
         break;

From c2a85077eb3d4da7241111a6cf7e9b7f97dda2fc Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Wed, 11 Sep 2024 20:27:09 -0700
Subject: [PATCH 12/29] Added saftey requirements

Elevator and Pivot have to be in correct places to move note between
---
 src/main/java/frc/robot/Constants.java        |  5 +++-
 src/main/java/frc/robot/RobotContainer.java   |  2 +-
 .../frc/robot/commands/IntakeCommand.java     |  2 +-
 .../robot/commands/LoadShooterCommand.java    | 12 +++++++++-
 .../robot/commands/TransferNoteCommand.java   |  7 +++---
 .../robot/commands/UnloadShooterCommand.java  |  7 +++++-
 .../robot/subsystems/ElevatorSubsystem.java   | 24 +++++++++++++++----
 .../robot/subsystems/ShooterSubsystem.java    |  7 +++---
 8 files changed, 50 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c22274c..ba27211 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -267,6 +267,9 @@ public static final class Modes {
                                         76, 1, 0, 0);
                         public static final ShooterSubsystem.ShooterPowers IDLE = new ShooterSubsystem.ShooterPowers(0,
                                         0, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP = new ShooterSubsystem.ShooterPowers(
+                                0,
+                                0, 0, 0);
                         public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(
                                         76, 1,
                                         0, 0);
@@ -278,7 +281,7 @@ public static final class Modes {
                                         0);
                         public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = new ShooterSubsystem.ShooterPowers(
                                         0,
-                                        0, 0, 0);
+                                        0, 0, -1);
                         public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
                                         40, 1, 0, 0);
                         public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 88f036e..a1d457e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -429,7 +429,7 @@ private void configureButtonBindings() {
         anthony
                 .b()
                 .onTrue(
-                        new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem));
+                        new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem));
 
 
 
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index 3ab8173..a0e0bcc 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -25,7 +25,7 @@ public IntakeCommand(
     this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
-
+        addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
     // Use addRequirements() here to declare subsystem dependencies.
     // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
   }
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index 2b09144..a5405d3 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -5,20 +5,30 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 
 public class LoadShooterCommand extends Command {
   /** Creates a new LoadShooterCommand. */
   ShooterSubsystem shooterSubsystem;
-  public LoadShooterCommand(ShooterSubsystem shooterSubsystem) {
+  ElevatorSubsystem elevatorSubsystem;
+  PivotSubsystem pivotSubsystem;
+  public LoadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
     this.shooterSubsystem = shooterSubsystem;
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.pivotSubsystem = pivotSubsystem;
+    addRequirements(elevatorSubsystem, shooterSubsystem, pivotSubsystem);
+
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+    elevatorSubsystem.setTargetHeight(0);
+    pivotSubsystem.setTargetDegrees(20);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index 5107094..5d21734 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -1,6 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -8,12 +9,12 @@
 public class TransferNoteCommand extends SequentialCommandGroup {
 
   public TransferNoteCommand(
-    ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, PivotSubsystem pivotSubsystem) {
+    ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
     if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
-      addCommands(new LoadShooterCommand(shooterSubsystem));
+      addCommands(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     }
     else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()){
-      addCommands(new UnloadShooterCommand(shooterSubsystem, pivotSubsystem).andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+      addCommands(new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem).andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
index 37992b4..c689b26 100644
--- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
@@ -13,10 +14,13 @@ public class UnloadShooterCommand extends Command {
   /** Creates a new UnloadShooterCommand. */
   ShooterSubsystem shooterSubsystem;
   PivotSubsystem pivotSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
   boolean pass;
-  public UnloadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem) {
+  public UnloadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem, shooterSubsystem, pivotSubsystem);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -26,6 +30,7 @@ public void initialize() {
     pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered();
     shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
     pivotSubsystem.setTargetDegrees(20);
+    elevatorSubsystem.setTargetHeight(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 1823e7b..f6e90e4 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -52,6 +52,14 @@ public ElevatorSubsystem() {
         currentAngle = 0;
     }
 
+    public double getArmPosition(){
+        return armInchesToRotations(armMotor.getPosition().getValueAsDouble());
+    }
+
+    public double getElevatorPosition(){
+        return inchesToRotations(elevatorMotor.getPosition().getValueAsDouble());
+    }
+
     public static double inchesToRotations(double height) {
         return height / Elevator.ELEVATOR_GEAR_RATIO;
     }
@@ -77,8 +85,16 @@ public void setTargetAngle(double targetAngle) {
         this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0);
     }
 
-    public boolean nearTarget(double target, double current) {
-        if (target - 0.5 <= current && current <= target + 0.5) {
+    public boolean nearTargetAngle() {
+        if (targetAngle - 0.5 <= getArmPosition() && getArmPosition() <= targetAngle + 0.5) {
+            return true;
+        }
+        return false;
+    }
+
+
+    public boolean nearTargetHeight() {
+        if (targetHeight - 0.5 <= getElevatorPosition() && getElevatorPosition() <= targetHeight + 0.5) {
             return true;
         }
         return false;
@@ -91,8 +107,8 @@ public double calculateFeedforward(){
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
-        elevatorMotorPower = controller.calculate(inchesToRotations(elevatorMotor.getPosition().getValueAsDouble()), targetHeight);
-        armMotorPower = controller.calculate(armInchesToRotations(armMotor.getPosition().getValueAsDouble()), targetAngle);
+        elevatorMotorPower = controller.calculate(getElevatorPosition(), targetHeight);
+        armMotorPower = controller.calculate(getArmPosition(), targetAngle);
         elevatorMotor.setVoltage(MathUtil.clamp(elevatorMotorPower+Elevator.ELEVATOR_FEEDFORWARD, -10, 10));
         armMotor.setVoltage(MathUtil.clamp(armMotorPower+calculateFeedforward(), -10, 10));
     }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 7ed9c2a..5ad0a5d 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -58,7 +58,8 @@ public enum ShooterMode {
     VARIABLE_VELOCITY(Shooter.Modes.VARIABLE_VELOCITY),
     SHOOT_VAR(Shooter.Modes.SHOOT_VAR),
     LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER),
-    SHOOTER_UNLOAD(Shooter.Modes.SHOOTER_UNLOAD);
+    SHOOTER_UNLOAD(Shooter.Modes.SHOOTER_UNLOAD),
+    RAMP_AMP(Shooter.Modes.RAMP_AMP);
 
     public final ShooterPowers shooterPowers;
 
@@ -188,10 +189,8 @@ public void setVariableVelocity(double velocity) {
 
   public void advanceToShootMode() {
     switch (shooterMode) {
-      case IDLE:
-      if (isSerializerBeamBreakSensorTriggered()){
+      case RAMP_AMP:
         shooterMode = ShooterMode.SHOOT_AMP;
-      }
       case SHUTTLE:
         shooterMode = ShooterMode.SHOOT_SHUTTLE;
         break;

From 72fe070bf9d0ef53d1aae228ceb05d4e697bb15e Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 12 Sep 2024 17:45:38 -0700
Subject: [PATCH 13/29] Shooter ready to be tested

---
 src/main/java/frc/robot/RobotContainer.java   | 29 ++++++++++++-------
 .../java/frc/robot/commands/ShootCommand.java |  3 +-
 2 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a1d457e..eddff8b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -380,16 +380,16 @@ private void configureButtonBindings() {
                         new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
                                 .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
 
-        jacob
-                .a()
-                .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
-                                .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
-                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE)));
+        // jacob
+        //         .a()
+        //         .onTrue(
+        //                 new RotateAngleDriveCommand(
+        //                         drivebaseSubsystem,
+        //                         translationXSupplier,
+        //                         translationYSupplier,
+        //                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
+        //                         .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
+        //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE)));
 
         jacob
                 .povDown()
@@ -431,7 +431,14 @@ private void configureButtonBindings() {
                 .onTrue(
                         new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem));
 
-
+        jacob
+                .b()
+                .onTrue(
+                        new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+        jacob
+                .a()
+                .onTrue(
+                        new ShootCommand(shooterSubsystem));
 
         // SPEAKER FROM SUBWOOFER
         anthony
diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index a63beca..e59b963 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -5,7 +5,6 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 
@@ -13,7 +12,7 @@ public class ShootCommand extends Command {
   private ShooterSubsystem shooterSubsystem;
 
   /** Creates a new ShootCommand. */
-  public ShootCommand(ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem) {
+  public ShootCommand(ShooterSubsystem shooterSubsystem) {
     // Use addRequirements() here to declare subsystem dependencies.
     this.shooterSubsystem = shooterSubsystem;
     addRequirements(shooterSubsystem);

From 2fadc5de163e59fd29cf611939d97d6812a6eb2c Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 12 Sep 2024 18:15:54 -0700
Subject: [PATCH 14/29] Pivot works without cancoder for test

---
 src/main/java/frc/robot/Constants.java                 | 2 +-
 src/main/java/frc/robot/RobotContainer.java            | 6 +++---
 src/main/java/frc/robot/subsystems/PivotSubsystem.java | 6 +++---
 3 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ba27211..95900e4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -358,7 +358,7 @@ public static final class Setpoints {
 
                 public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
                 /** degrees per rotation */
-                public static final double PIVOT_GEAR_RATIO = 5+(1/3); //not used 
+                public static final double PIVOT_GEAR_RATIO = 5.33333; //not used 
                                                                                                 // values
                 public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index eddff8b..3897679 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -137,7 +137,7 @@ public RobotContainer() {
         // reigster commands for pathplanner
         NamedCommands.registerCommand(
                 "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-        NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem, intakeSubsystem));
+        NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
         NamedCommands.registerCommand(
                 "ShooterRampUpCommand",
                 new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
@@ -362,7 +362,7 @@ private void configureButtonBindings() {
                 .rightBumper()
                 .onTrue(
                         new AccelNoteCommand(shooterSubsystem)
-                                .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem))
+                                .andThen(new ShootCommand(shooterSubsystem))
                                 .andThen(
                                         new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
 
@@ -370,7 +370,7 @@ private void configureButtonBindings() {
         jacob
                 .rightTrigger()
                 .onTrue(new AccelNoteCommand(shooterSubsystem)
-                        .andThen(new ShootCommand(shooterSubsystem, intakeSubsystem)));
+                        .andThen(new ShootCommand(shooterSubsystem)));
 
         anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
         anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index 36b280a..2deb6a9 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -96,7 +96,7 @@ public double getCurrentError() {
   }
 
   public double getCurrentAngle() {
-    return rotationsToDegrees(pivotCANcoder.getAbsolutePosition().getValueAsDouble());
+    return rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble());
   }
 
   public double getTargetDegrees() {
@@ -117,7 +117,7 @@ public void setTargetDegrees(double degrees) {
   }
 
   private static double rotationsToDegrees(double rotations) {
-    return (rotations * 360);
+    return rotations*Pivot.PIVOT_GEAR_RATIO;
   }
 
   public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) {
@@ -168,7 +168,7 @@ public void periodic() {
             getCurrentAngle(),
             MathUtil.clamp(targetDegrees, Setpoints.MINIMUM_ANGLE, Setpoints.MAXIMUM_ANGLE));
 
-    pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
+    pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -5, 5);
 
     pivotMotor.setVoltage(pidVoltageOutput);
   }

From 8fe29470094f8f6afb59b76efbbed903afb757ed Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 12 Sep 2024 20:43:41 -0700
Subject: [PATCH 15/29] Pivot goes up and down for testing

---
 src/main/java/frc/robot/Constants.java        |   4 +-
 src/main/java/frc/robot/RobotContainer.java   | 170 +++++++++---------
 .../robot/commands/PivotManualCommand.java    |   2 +-
 .../robot/commands/SpeakerLockCommand.java    |   5 +-
 .../frc/robot/subsystems/PivotSubsystem.java  |  10 +-
 .../robot/subsystems/ShooterSubsystem.java    |  12 +-
 6 files changed, 106 insertions(+), 97 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 95900e4..c740dd0 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -256,8 +256,8 @@ public static final class Modes {
 
         public static final class Shooter {
                 public static final class Ports {
-                        public static final int TOP_SHOOTER_MOTOR_PORT = 20;
-                        public static final int BOTTOM_SHOOTER_MOTOR_PORT = 19;
+                        public static final int TOP_SHOOTER_MOTOR_PORT = 0;
+                        public static final int BOTTOM_SHOOTER_MOTOR_PORT = 5;
                         public static final int ACCELERATOR_MOTOR_PORT = 17;
                         public static final int BEAM_BREAK_SENSOR_PORT = 8;
                 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3897679..6a76939 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -134,89 +134,91 @@ public class RobotContainer {
      */
     public RobotContainer() {
 
-        // reigster commands for pathplanner
-        NamedCommands.registerCommand(
-                "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-        NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
-        NamedCommands.registerCommand(
-                "ShooterRampUpCommand",
-                new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
-        NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
-        NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
-        NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
-        NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
-        NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
-        NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
-        NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
-        NamedCommands.registerCommand(
-                "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
-        NamedCommands.registerCommand(
-                "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
-        NamedCommands.registerCommand(
-                "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
-        NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
-        NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
-        NamedCommands.registerCommand(
-                "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
-        NamedCommands.registerCommand(
-                "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
-        NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
-        NamedCommands.registerCommand(
-                "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
-        NamedCommands.registerCommand(
-                "ZeroSubwoofer1",
-                new InstantCommand(
-                        () -> drivebaseSubsystem.zeroGyroscopeOffset(
-                                DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
-        NamedCommands.registerCommand(
-                "ZeroSubwoofer3",
-                new InstantCommand(
-                        () -> drivebaseSubsystem.zeroGyroscopeOffset(
-                                DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
-        NamedCommands.registerCommand(
-                "recenterPose1",
-                DriverStation.getAlliance().get().equals(Alliance.Blue)
-                        ? new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
-                                drivebaseSubsystem)
-                        : new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
-                                drivebaseSubsystem));
-        NamedCommands.registerCommand(
-                "recenterPose2",
-                DriverStation.getAlliance().get().equals(Alliance.Blue)
-                        ? new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
-                                drivebaseSubsystem)
-                        : new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
-                                drivebaseSubsystem));
-        NamedCommands.registerCommand(
-                "recenterPose3",
-                DriverStation.getAlliance().get().equals(Alliance.Blue)
-                        ? new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
-                                drivebaseSubsystem)
-                        : new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
-                                drivebaseSubsystem));
-        NamedCommands.registerCommand(
-                "recenterPose4",
-                DriverStation.getAlliance().get().equals(Alliance.Blue)
-                        ? new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
-                                drivebaseSubsystem)
-                        : new InstantCommand(
-                                () -> drivebaseSubsystem.resetOdometryToPose(
-                                        new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
-                                drivebaseSubsystem));
+        // // reigster commands for pathplanner
+        // NamedCommands.registerCommand(
+        //         "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        // NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
+        // NamedCommands.registerCommand(
+        //         "ShooterRampUpCommand",
+        //         new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+        // NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
+        // NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
+        // NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
+        // NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
+        // NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
+        // NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
+        // NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
+        // NamedCommands.registerCommand(
+        //         "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
+        // NamedCommands.registerCommand(
+        //         "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
+        // NamedCommands.registerCommand(
+        //         "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
+        // NamedCommands.registerCommand(
+        //         "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
+        // NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
+        // NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
+        // NamedCommands.registerCommand(
+        //         "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
+        // NamedCommands.registerCommand(
+        //         "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
+        // NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
+        // NamedCommands.registerCommand(
+        //         "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
+        // NamedCommands.registerCommand(
+        //         "ZeroSubwoofer1",
+        //         new InstantCommand(
+        //                 () -> drivebaseSubsystem.zeroGyroscopeOffset(
+        //                         DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
+        // NamedCommands.registerCommand(
+        //         "ZeroSubwoofer3",
+        //         new InstantCommand(
+        //                 () -> drivebaseSubsystem.zeroGyroscopeOffset(
+        //                         DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
+        // NamedCommands.registerCommand(
+        //         "recenterPose1",
+        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
+        //                 ? new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
+        //                         drivebaseSubsystem)
+        //                 : new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
+        //                         drivebaseSubsystem));
+        // NamedCommands.registerCommand(
+        //         "recenterPose2",
+        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
+        //                 ? new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
+        //                         drivebaseSubsystem)
+        //                 : new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
+        //                         drivebaseSubsystem));
+        // NamedCommands.registerCommand(
+        //         "recenterPose3",
+        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
+        //                 ? new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
+        //                         drivebaseSubsystem)
+        //                 : new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
+        //                         drivebaseSubsystem));
+        // NamedCommands.registerCommand(
+        //         "recenterPose4",
+        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
+        //                 ? new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
+        //                         drivebaseSubsystem)
+        //                 : new InstantCommand(
+        //                         () -> drivebaseSubsystem.resetOdometryToPose(
+        //                                 new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
+        //                         drivebaseSubsystem));
 
         // Set up the default command for the drivetrain.
         // The controls are for field-oriented driving:
@@ -408,7 +410,7 @@ private void configureButtonBindings() {
 
         DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
 
-        new Trigger(() -> Math.abs(pivotManualRate.getAsDouble()) > 0.07)
+        new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
                 .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
 
         // SOURCE
diff --git a/src/main/java/frc/robot/commands/PivotManualCommand.java b/src/main/java/frc/robot/commands/PivotManualCommand.java
index 67b3690..0bc78d3 100644
--- a/src/main/java/frc/robot/commands/PivotManualCommand.java
+++ b/src/main/java/frc/robot/commands/PivotManualCommand.java
@@ -19,7 +19,7 @@ public PivotManualCommand(PivotSubsystem pivotSubsystem, DoubleSupplier joystick
 
   @Override
   public void execute() {
-    pivotSubsystem.setTargetDegrees(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
+    pivotSubsystem.setPower(joystickRate.getAsDouble());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/SpeakerLockCommand.java b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
index c0e54e5..dab0c0b 100644
--- a/src/main/java/frc/robot/commands/SpeakerLockCommand.java
+++ b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
@@ -7,7 +7,8 @@
 public class SpeakerLockCommand extends ParallelCommandGroup {
   public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) {
     addCommands(
-        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
-        new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
+        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0)
+        // new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)
+        );
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index 2deb6a9..58b52a3 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -40,6 +40,7 @@ public class PivotSubsystem extends SubsystemBase {
   private Pose2d pose;
   private double distance;
   private GenericEntry debugTarget;
+  private double power;
 
   private double pastDebugTarget = 0;
   private boolean listenToDebug = true;
@@ -76,6 +77,7 @@ public PivotSubsystem() {
       pivotTab.addNumber("Applied Voltage", () -> pivotMotor.getMotorVoltage().getValueAsDouble());
       pivotTab.addDouble("PID Voltage Output", () -> pidVoltageOutput);
       pivotTab.addDouble("Calculated Target Angle", () -> calculatedTargetDegrees);
+      pivotTab.addDouble("Power", () -> power);
       pivotTab.addDouble("Distance", () -> distance);
       pivotTab.add(pidController);
       debugTarget =
@@ -153,6 +155,10 @@ public double getAngularError() {
     return targetDegrees - getCurrentAngle();
   }
 
+  public void setPower(double power){
+    this.power = power;
+  }
+
   @Override
   public void periodic() {
 
@@ -168,8 +174,8 @@ public void periodic() {
             getCurrentAngle(),
             MathUtil.clamp(targetDegrees, Setpoints.MINIMUM_ANGLE, Setpoints.MAXIMUM_ANGLE));
 
-    pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -5, 5);
+    pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
 
-    pivotMotor.setVoltage(pidVoltageOutput);
+    pivotMotor.set(power);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 5ad0a5d..d19f6c3 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -83,8 +83,8 @@ public ShooterSubsystem() {
     acceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT);
     serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT);
 
-    shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
-    serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    // shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    // serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
 
 
     // rollerMotorTop.getConfigurator().apply(new TalonFXConfiguration());
@@ -161,12 +161,12 @@ public boolean isShooterUpToSpeed() {
   }
 
   public boolean isShooterBeamBreakSensorTriggered() {
-    return !shooterSensor.get();
+    return true;
   }
 
   public boolean isSerializerBeamBreakSensorTriggered() {
     // if is triggered return true
-    return !serializerSensor.get();
+    return true;
   }
 
   public boolean isReadyToShoot() {
@@ -225,7 +225,7 @@ public void periodic() {
               shooterMode.shooterPowers.roller() * shooterMode.shooterPowers.topToBottomRatio()));
     }
 
-    acceleratorMotor.set(shooterMode.shooterPowers.accelerator());
-    serializerMotor.set(shooterMode.shooterPowers.serializerSpeed());
+    acceleratorMotor.set(0);
+    serializerMotor.set(1);
   }
 }

From 0ad38443eadf0c6606d341886a53e3619434dea2 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Sun, 15 Sep 2024 15:41:09 -0700
Subject: [PATCH 16/29] Added feedforward to pivot

---
 src/main/java/frc/robot/subsystems/PivotSubsystem.java | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index 58b52a3..493de72 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -58,6 +58,7 @@ public PivotSubsystem() {
     pivotMotor.setInverted(true);
     pivotMotor.clearStickyFaults();
     pivotMotor.set(0);
+    pivotMotor.setPosition(0);
 
     pivotMotor.setNeutralMode(NeutralModeValue.Brake);
 
@@ -159,6 +160,7 @@ public void setPower(double power){
     this.power = power;
   }
 
+
   @Override
   public void periodic() {
 
@@ -176,6 +178,6 @@ public void periodic() {
 
     pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
 
-    pivotMotor.set(power);
+    pivotMotor.set(getFeedForward()+power);
   }
 }

From 85b49d356213e668b9bd9df57fdc349b85181b5e Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Sun, 15 Sep 2024 16:06:32 -0700
Subject: [PATCH 17/29] Formatting

---
 src/main/java/frc/robot/Constants.java | 69 +++++++++++---------------
 1 file changed, 30 insertions(+), 39 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c740dd0..f50c476 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -263,45 +263,36 @@ public static final class Ports {
                 }
 
                 public static final class Modes {
-                        public static final ShooterSubsystem.ShooterPowers INTAKE = new ShooterSubsystem.ShooterPowers(
-                                        76, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers IDLE = new ShooterSubsystem.ShooterPowers(0,
-                                        0, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP = new ShooterSubsystem.ShooterPowers(
-                                0,
-                                0, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = new ShooterSubsystem.ShooterPowers(
-                                        76, 1,
-                                        0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = new ShooterSubsystem.ShooterPowers(
-                                        76, 1,
-                                        .5, 0);
-                        public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = new ShooterSubsystem.ShooterPowers(
-                                        0, 1, 0,
-                                        0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = new ShooterSubsystem.ShooterPowers(
-                                        0,
-                                        0, 0, -1);
-                        public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = new ShooterSubsystem.ShooterPowers(
-                                        40, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHUTTLE = new ShooterSubsystem.ShooterPowers(
-                                        30, 1, 0,
-                                        0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = new ShooterSubsystem.ShooterPowers(
-                                        30, 1,
-                                        0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = new ShooterSubsystem.ShooterPowers(
-                                        76, 1,
-                                        0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = new ShooterSubsystem.ShooterPowers(
-                                        30, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = new ShooterSubsystem.ShooterPowers(
-                                        30, 1,
-                                        0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = new ShooterSubsystem.ShooterPowers(
-                                        0, 0, 0.5, 0.5);
-                        public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD = new ShooterSubsystem.ShooterPowers(
-                                        0, 0, -0.5, -0.5);
+                        public static final ShooterSubsystem.ShooterPowers INTAKE = 
+                                new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers IDLE = 
+                                new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP = 
+                                new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = 
+                                new ShooterSubsystem.ShooterPowers( 76, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = 
+                                new ShooterSubsystem.ShooterPowers( 76, 1, .5, 0);
+                        public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = 
+                                new ShooterSubsystem.ShooterPowers(0, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = 
+                                new ShooterSubsystem.ShooterPowers(0, 0, 0, -1);
+                        public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = 
+                                new ShooterSubsystem.ShooterPowers(40, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHUTTLE = 
+                                new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = 
+                                new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = 
+                                new ShooterSubsystem.ShooterPowers(76, 1, 0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = 
+                                new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
+                        public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = 
+                                new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
+                        public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = 
+                                new ShooterSubsystem.ShooterPowers(0, 0, 0.5, 0.5);
+                        public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD = 
+                                new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5);
                 }
 
                 public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23)

From 0f32cc62c16f712aae3c62dbc815077772c83923 Mon Sep 17 00:00:00 2001
From: audrey <audreypjohnson@gmail.com>
Date: Tue, 17 Sep 2024 19:03:02 -0700
Subject: [PATCH 18/29] uncomment autos, spotless

---
 src/main/java/frc/robot/Constants.java        | 1077 ++++++++---------
 src/main/java/frc/robot/RobotContainer.java   |  915 +++++++-------
 .../robot/commands/AdvancedIntakeCommand.java |    2 +-
 .../java/frc/robot/commands/AngleCommand.java |   76 +-
 .../frc/robot/commands/HeightCommand.java     |   76 +-
 .../frc/robot/commands/IntakeCommand.java     |    2 +-
 .../robot/commands/LoadShooterCommand.java    |    8 +-
 .../java/frc/robot/commands/RGBCommand.java   |   27 +-
 .../java/frc/robot/commands/ShootCommand.java |    3 +-
 .../robot/commands/TransferNoteCommand.java   |   14 +-
 .../robot/commands/UnloadShooterCommand.java  |   13 +-
 .../robot/subsystems/ElevatorSubsystem.java   |  189 ++-
 .../frc/robot/subsystems/IntakeSubsystem.java |    3 -
 .../frc/robot/subsystems/PivotSubsystem.java  |    7 +-
 .../robot/subsystems/ShooterSubsystem.java    |   48 +-
 15 files changed, 1225 insertions(+), 1235 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f50c476..1a76881 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -51,552 +51,545 @@
 
 @SuppressWarnings("java:S1118")
 /**
- * The Constants class provides a convenient place for teams to hold robot-wide
- * numerical or boolean
- * constants. This class should not be used for any other purpose. All constants
- * should be declared
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
  * globally (i.e. public static). Do not put anything functional in this class.
  *
- * <p>
- * It is advised to statically import this class (or one of its inner classes)
- * wherever the
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
 
-        public static final class Config {
-                // maybe tune PID values?
-                public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG = new HolonomicPathFollowerConfig(
-                                new PIDConstants(20, 0, 0),
-                                new PIDConstants(10, 0, 0),
-                                Drive.MAX_VELOCITY_METERS_PER_SECOND,
-                                Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
-                                new ReplanningConfig());
-
-                /** turn this off before comp. */
-                public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
-
-                /** turn this off! only use on practice eboard testing. */
-                public static final boolean DISABLE_SWERVE_INIT = false;
-
-                /** keep this on for pigeon, disable if absolutely necessary */
-                public static final boolean FLIP_GYROSCOPE = true;
-
-                /**
-                 * def turn this off unless you are using it, generates in excess of 100k rows
-                 * for a match.
-                 */
-                public static final boolean WRITE_APRILTAG_DATA = false;
-
-                public static final Path APRILTAG_DATA_PATH = Filesystem.getDeployDirectory().toPath()
-                                .resolve("poseEstimationsAtDistances.csv");
-                public static final double REAL_X = 0.0;
-                public static final double REAL_Y = 0.0;
-        }
-
-        public static final class Drive {
-                public static final int PIGEON_PORT = 0; // FIXME
-                public static final String SWERVE_CANBUS = "rio"; // placeholder
-
-                // max voltage delivered to drivebase
-                // supposedly useful to limit speed for testing
-                public static final double MAX_VOLTAGE = 12.0;
-                // maximum velocity
-                // FIXME measure this value experimentally
-                public static final double MAX_VELOCITY_METERS_PER_SECOND = 6380.0 // falcon 500 free speed rpm
-                                / 60.0
-                                * 0.10033
-                                * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
-                                * Math.PI;
-                // theoretical value
-                // FIXME measure and validate experimentally
-                public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = MAX_VELOCITY_METERS_PER_SECOND
-                                / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
-                                * .5;
-
-                /** the maximum amount of angular error pid loops will tolerate for rotation */
-                public static final double ANGULAR_ERROR = 1.0;
-                /**
-                 * the minimum magnitude of the right stick for it to be used as a new rotation
-                 * angle
-                 */
-                public static final double ROTATE_VECTOR_MAGNITUDE = .7;
-
-                public static final class Dims {
-                        // FIXME validate with hardware
-                        public static final double TRACKWIDTH_METERS = .5207; // 20.5 inches (source: cad) converted to
-                                                                              // meters
-                        public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
-
-                        public static final double BUMPER_WIDTH_METERS_X = .9779;
-                        public static final double BUMPER_WIDTH_METERS_Y = .8382;
-                }
-
-                /*
-                 * module layout:
-                 * |──────
-                 * |->│# ##steer motor
-                 * │ │ ##cancoder
-                 * │ │##drive motor
-                 * module number
-                 * 
-                 * steer is always left
-                 * from corner perspective
-                 * 
-                 * robot visualization:
-                 * |──────────────────────|
-                 * │2 10 04 1│
-                 * │ 25 24 │
-                 * │11 S D 03│
-                 * │ D S │
-                 * │ │
-                 * │ │
-                 * │ S D │
-                 * │ D S │
-                 * │12 |────────| 02│
-                 * │ 26 │ │ 27 │
-                 * │3 13│ batt │01 4│
-                 * |──────┴───┬┬───┴──────|
-                 * ││
-                 * ││
-                 * ▼▼
-                 * software front
-                 */
-
-                public static final class Modules {
-                        public static final class Params {
-                                public static final double WHEEL_RADIUS = 2; // also in INCHES
-                                public static final double COUPLING_GEAR_RATIO = 3.125;
-                                public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
-                                public static final double STEER_GEAR_RATIO = 21.428571428571427;
-                                public static final Slot0Configs DRIVE_MOTOR_GAINS = new Slot0Configs().withKP(3)
-                                                .withKI(0).withKD(0)
-                                                .withKS(0.32).withKV(0.11).withKA(0);
-                                public static final Slot0Configs STEER_MOTOR_GAINS = new Slot0Configs().withKP(11)
-                                                .withKI(0).withKD(0)
-                                                .withKS(0.4).withKV(0.6).withKA(0);
-                                public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
-                                public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.Voltage;
-                                public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
-                                public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
-                                public static final double SLIP_CURRENT = 0; // optional, unused rn
-                                public static final boolean STEER_MOTOR_INVERTED = true;
-
-                                public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
-                                public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
-                        }
-
-                        public static final class Module1 { // back left
-                                public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
-                                public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
-                                public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
-
-                                public static final double STEER_OFFSET = IS_COMP_BOT
-                                                ? 0.41943359375 // comp bot offset
-                                                : 0.0595703125; // practice bot offset
-                        }
-
-                        public static final class Module2 { // back right
-                                public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
-                                public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
-                                public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
-
-                                public static final double STEER_OFFSET = IS_COMP_BOT
-                                                ? -0.39990234375 // comp bot offset
-                                                : 0.262451171875; // practice bot offset
-                        }
-
-                        public static final class Module3 { // front right
-                                public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
-                                public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
-                                public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
-
-                                public static final double STEER_OFFSET = IS_COMP_BOT
-                                                ? 0.225341796875 // comp bot offset
-                                                : -0.20825195312; // practice bot offset
-                        }
-
-                        public static final class Module4 { // front left
-                                public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
-                                public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
-                                public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
-
-                                public static final double STEER_OFFSET = IS_COMP_BOT
-                                                ? 0.316650390625 // comp bot offset
-                                                : -0.3564453125 + 180; // practice bot offset
-                        }
-                }
-
-                public static final class Setpoints {
-                        public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
-
-                        public static final int SOURCE_DEGREES = 39;
-                        public static final int SPEAKER_DEGREES = 11;
-                        public static final int EPSILON = 3;
-                }
-        }
-
-        public static final class Intake {
-                public static final class Ports {
-                        public static final int INTAKE_MOTOR_PORT = 15;
-                        public static final int SERIALIZER_MOTOR_PORT = 16;
-                        public static final int INTAKE_SENSOR_PORT = 9;
-                }
-
-                public static final class Modes {
-                        public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
-                        public static final IntakePowers HOLD = new IntakePowers(0, 0d);
-                        public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-                        public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 0.7);
-                        public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1);
-                }
-
-                public static final boolean IS_BEAMBREAK = true;
-        }
-
-        public static final class Shooter {
-                public static final class Ports {
-                        public static final int TOP_SHOOTER_MOTOR_PORT = 0;
-                        public static final int BOTTOM_SHOOTER_MOTOR_PORT = 5;
-                        public static final int ACCELERATOR_MOTOR_PORT = 17;
-                        public static final int BEAM_BREAK_SENSOR_PORT = 8;
-                }
-
-                public static final class Modes {
-                        public static final ShooterSubsystem.ShooterPowers INTAKE = 
-                                new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers IDLE = 
-                                new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_AMP = 
-                                new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER = 
-                                new ShooterSubsystem.ShooterPowers( 76, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER = 
-                                new ShooterSubsystem.ShooterPowers( 76, 1, .5, 0);
-                        public static final ShooterSubsystem.ShooterPowers TARGET_LOCK = 
-                                new ShooterSubsystem.ShooterPowers(0, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_AMP = 
-                                new ShooterSubsystem.ShooterPowers(0, 0, 0, -1);
-                        public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY = 
-                                new ShooterSubsystem.ShooterPowers(40, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHUTTLE = 
-                                new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE = 
-                                new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE = 
-                                new ShooterSubsystem.ShooterPowers(76, 1, 0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY = 
-                                new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
-                        public static final ShooterSubsystem.ShooterPowers SHOOT_VAR = 
-                                new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
-                        public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER = 
-                                new ShooterSubsystem.ShooterPowers(0, 0, 0.5, 0.5);
-                        public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD = 
-                                new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5);
-                }
-
-                public static final Slot0Configs ROLLER_PID_CONFIG = new Slot0Configs().withKP(0.2).withKS(0.23)
-                                .withKV(0.24);
-
-                public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
-        }
-
-        public static final class Pivot {
-                public static final class Ports {
-                        public static final int PIVOT_MOTOR_PORT = 18;
-                        public static final int CANCODER_PORT = 28;
-                        public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
-                }
-
-                public static final class MotorConfigs {
-                        public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR = new MagnetSensorConfigs()
-                                        .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
-                                        .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
-                                        .withMagnetOffset(PIVOT_CANCODER_OFFSET);
-                        public static final CANcoderConfiguration CANCODER_CONFIG = new CANcoderConfiguration()
-                                        .withMagnetSensor(CANCODER_MAGNET_SENSOR);
-
-                        public static final FeedbackConfigs PIVOT_FEEDBACK = new FeedbackConfigs()
-                                        .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
-                                        .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
-                                        .withSensorToMechanismRatio(1.0)
-                                        .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
-                        public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT = new SoftwareLimitSwitchConfigs()
-                                        .withForwardSoftLimitThreshold(0.25)
-                                        .withReverseSoftLimitThreshold(-0.015)
-                                        .withForwardSoftLimitEnable(false)
-                                        .withReverseSoftLimitEnable(false);
-                        public static final VoltageConfigs PIVOT_VOLTAGE = new VoltageConfigs()
-                                        .withPeakForwardVoltage(5)
-                                        .withPeakReverseVoltage(-5);
-                        public static final TalonFXConfiguration PIVOT_CONFIG = new TalonFXConfiguration()
-                                        .withFeedback(PIVOT_FEEDBACK)
-                                        .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
-                                        .withVoltage(PIVOT_VOLTAGE);
-                }
-
-                public static final class Setpoints {
-                        public static final int MINIMUM_ANGLE = 15;
-                        public static final int MAXIMUM_ANGLE = 125;
-
-                        public static final int MINIMUM_SAFE_THRESHOLD = 15;
-                        public static final int MAXIMUM_SAFE_THRESHOLD = 80;
-
-                        public static final int SPEAKER_ANGLE = 30;
-                }
-
-                public static final int EPSILON = 2;
-
-                public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
-                /** degrees per rotation */
-                public static final double PIVOT_GEAR_RATIO = 5.33333; //not used 
-                                                                                                // values
-                public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
-
-                public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
-                public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
-
-                public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
-                public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
-
-                public static final double GRAVITY = 9.80665; // meters per second
-
-                public static final double GRAVITY_VOLTAGE = 0.4;
-                public static final double PIVOT_MAX_VOLTAGE = 3.5;
-        }
-
-        public static final class Vision {
-                public static record VisionSource(String name, Transform3d robotToCamera) {
-                }
-
-                public static final List<VisionSource> VISION_SOURCES = List.of(
-                                /*
-                                 * new VisionSource(
-                                 * "frontCam",
-                                 * new Transform3d(
-                                 * new Translation3d(
-                                 * -0.305, // front/back
-                                 * -0.2286, // left/right
-                                 * -0.2159 // up/down
-                                 * ),
-                                 * new Rotation3d(
-                                 * 0,
-                                 * Math.toRadians(30), // angle up/down
-                                 * Math.toRadians(180)))),
-                                 * new VisionSource(
-                                 * "backCam",
-                                 * new Transform3d(
-                                 * new Translation3d(
-                                 * -0.2796, // front/back
-                                 * -0.2286, // left/right
-                                 * -0.2159 // up/down
-                                 * ),
-                                 * new Rotation3d(
-                                 * 0,
-                                 * Math.toRadians(30), // angle up/down
-                                 * Math.toRadians(180)))),
-                                 */
-                                new VisionSource(
-                                                "backUpCam",
-                                                new Transform3d(
-                                                                new Translation3d(
-                                                                                -0.2796, // front/back
-                                                                                0.2286, // left/right
-                                                                                -0.2159 // up/down
-                                                                ),
-                                                                new Rotation3d(
-                                                                                0,
-                                                                                Math.toRadians(30), // angle up/down
-                                                                                Math.toRadians(180)))));
-
-                public static final int THREAD_SLEEP_DURATION_MS = 5;
-        }
-
-        public static final class PoseEstimator {
-                /**
-                 * Standard deviations of model states. Increase these numbers to trust your
-                 * model's state
-                 * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
-                 * meters and radians.
-                 */
-                public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS = MatBuilder.fill(
-                                Nat.N3(),
-                                Nat.N1(),
-                                0.1, // x
-                                0.1, // y
-                                0.1 // theta
-                );
-
-                /**
-                 * Standard deviations of the vision measurements. Increase these numbers to
-                 * trust global
-                 * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ,
-                 * with units in
-                 * meters and radians.
-                 *
-                 * <p>
-                 * These are not actually used anymore, but the constructor for the pose
-                 * estimator wants
-                 * them. This value is calculated dynamically using the below list.
-                 */
-                public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS = MatBuilder.fill(
-                                Nat.N3(),
-                                Nat.N1(),
-                                1, // x
-                                1, // y
-                                1 * Math.PI // theta
-                );
-
-                public static final double POSE_AMBIGUITY_CUTOFF = .05;
-
-                public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS = List.of(
-                                // 1 tag
-                                new TagCountDeviation(
-                                                new UnitDeviationParams(.25, .4, .9),
-                                                new UnitDeviationParams(.35, .5, 1.2),
-                                                new UnitDeviationParams(.5, .7, 1.5)),
-
-                                // 2 tags
-                                new TagCountDeviation(
-                                                new UnitDeviationParams(.35, .1, .4),
-                                                new UnitDeviationParams(.5, .7, 1.5)),
-
-                                // 3+ tags
-                                new TagCountDeviation(
-                                                new UnitDeviationParams(.25, .07, .25),
-                                                new UnitDeviationParams(.15, 1, 1.5)));
-
-                /** about one inch */
-                public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
-
-                public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
-
-                public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS = List.of(Set.of(1, 2, 3, 4, 5),
-                                Set.of(6, 7, 8, 9, 10));
-
-                public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
-
-                public static final int MAX_FRAME_FIDS = 4;
-        }
-
-        public static final class NetworkWatchdog {
-                /** The IP addresses to ping for testing bridging, on the second vlan. */
-                public static final List<IPv4> TEST_IP_ADDRESSES = List.of(IPv4.internal(17), IPv4.internal(18),
-                                IPv4.internal(19));
-
-                /**
-                 * The number of ms (sleep delta using oshi system uptime) to wait before
-                 * beginning to ping the
-                 * test IP.
-                 */
-                public static final int BOOT_SCAN_DELAY_MS = 50_000;
-
-                /**
-                 * The number of seconds for ping to wait before giving up on reaching a device.
-                 */
-                public static final int PING_TIMEOUT_SECONDS = 2;
-
-                /** The number of ms to wait before retrying successful health checks. */
-                public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
-
-                /**
-                 * The number of ms to leave the switching pdh port off before turning it back
-                 * on as part of
-                 * rebooting the network switch.
-                 */
-                public static final int REBOOT_DURATION_MS = 1_000;
-
-                /**
-                 * The number of ms to wait before rerunning health checks after a failed check
-                 * which triggered
-                 * switch reboot.
-                 */
-                public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
-        }
-
-        public static final class CANWatchdog {
-                public static final int SCAN_DELAY_MS = 100;
-        }
-
-        public static final class Lights {
-                public static final int CANDLE_ID = 34;
-                public static final int NUM_LEDS = 89
-                                // 8 inside the candle, 8 more for balance
-                                + 8 * 2;
-
-                public static final class Colors {
-                        public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
-                        public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
-                        public static final RGBColor RED = new RGBColor(255, 0, 0);
-                        public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
-                        public static final RGBColor BLUE = new RGBColor(0, 0, 255);
-                        public static final RGBColor PINK = new RGBColor(250, 35, 100);
-                        public static final RGBColor MINT = new RGBColor(55, 255, 50);
-                        public static final RGBColor TEAL = new RGBColor(0, 255, 255);
-                        public static final RGBColor WHITE = new RGBColor(255, 255, 255);
-                }
-        }
-
-        public static final class AutoAlign {
-
-                public static final double FIELD_WIDTH = 16.54;
-
-                // Blue team:
-                // pose angles
-                public static final int BOTTOM_MID_ANGLE = 148;
-                public static final int STAGE_ANGLE = 0;
-                public static final int TOP_MID_ANGLE = 0;
-                public static final int AMP_ANGLE = 0;
-
-                // These poses have not been verified
-                public static final Pose2d BLUE_AMP = new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
-                public static final Pose2d BLUE_TOP_MID = new Pose2d(5.8, 7.0,
-                                Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
-
-                // These poses have been verified
-                public static final Pose2d BLUE_STAGE = new Pose2d(4.17, 4.74,
-                                Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
-                public static final Pose2d BLUE_BOTTOM_MID = new Pose2d(5.84, 1.18,
-                                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
-
-                // Red team:
-                // These poses have not been verified
-                public static final Pose2d RED_AMP = new Pose2d(FIELD_WIDTH - 2.75, 7.31,
-                                Rotation2d.fromDegrees(-AMP_ANGLE));
-                public static final Pose2d RED_TOP_MID = new Pose2d(FIELD_WIDTH - 5.8, 7.0,
-                                Rotation2d.fromDegrees(-TOP_MID_ANGLE));
-
-                // These poses have been verified
-                public static final Pose2d RED_STAGE = new Pose2d(FIELD_WIDTH - 4.17, 4.74,
-                                Rotation2d.fromDegrees(-STAGE_ANGLE));
-                public static final Pose2d RED_BOTTOM_MID = new Pose2d(FIELD_WIDTH - 5.84, 1.18,
-                                Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
-
-                public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
-                public static final double TOP_MID_TARGET_ANGLE = 0;
-                public static final double STAGE_TARGET_ANGLE = 0;
-                public static final double AMP_TARGET_ANGLE = 0;
-                public static final PathConstraints CONSTRAINTS = new PathConstraints(3, 3, Units.degreesToRadians(540),
-                                Units.degreesToRadians(720));
-        }
-
-        public static class Elevator { 
-                public static final class Ports {
-                        public static final int ARM_MOTOR_PORT = 18;
-                        public static final int ELEVATOR_MOTOR_PORT = 28;
-                }
-                /**inches per rotation */
-                public static final double ELEVATOR_GEAR_RATIO = 0.0906065;
-                /**FIXME */
-                public static final double MAX_HEIGHT = 50; 
-                /**FIXME */
-                public static final double ELEVATOR_FEEDFORWARD = 0.01;
-
-                public static final class Arm{
-                        /**FIXME */
-                        public static final double MAX_ANGLE = 50; 
-                        /**inches per rotation */
-                        public static final double ARM_GEAR_RATIO = 0.0231481481;
-                }
-        }
+  public static final class Config {
+    // maybe tune PID values?
+    public static final HolonomicPathFollowerConfig PATH_FOLLOWER_CONFIG =
+        new HolonomicPathFollowerConfig(
+            new PIDConstants(20, 0, 0),
+            new PIDConstants(10, 0, 0),
+            Drive.MAX_VELOCITY_METERS_PER_SECOND,
+            Math.sqrt(Math.pow(Dims.TRACKWIDTH_METERS, 2) * 2),
+            new ReplanningConfig());
+
+    /** turn this off before comp. */
+    public static final boolean SHOW_SHUFFLEBOARD_DEBUG_DATA = true;
+
+    /** turn this off! only use on practice eboard testing. */
+    public static final boolean DISABLE_SWERVE_INIT = false;
+
+    /** keep this on for pigeon, disable if absolutely necessary */
+    public static final boolean FLIP_GYROSCOPE = true;
+
+    /** def turn this off unless you are using it, generates in excess of 100k rows for a match. */
+    public static final boolean WRITE_APRILTAG_DATA = false;
+
+    public static final Path APRILTAG_DATA_PATH =
+        Filesystem.getDeployDirectory().toPath().resolve("poseEstimationsAtDistances.csv");
+    public static final double REAL_X = 0.0;
+    public static final double REAL_Y = 0.0;
+  }
+
+  public static final class Drive {
+    public static final int PIGEON_PORT = 0; // FIXME
+    public static final String SWERVE_CANBUS = "rio"; // placeholder
+
+    // max voltage delivered to drivebase
+    // supposedly useful to limit speed for testing
+    public static final double MAX_VOLTAGE = 12.0;
+    // maximum velocity
+    // FIXME measure this value experimentally
+    public static final double MAX_VELOCITY_METERS_PER_SECOND =
+        6380.0 // falcon 500 free speed rpm
+            / 60.0
+            * 0.10033
+            * (1 / 6.12) // mk4i l3 16t falcon drive reduction (sourced from adrian)
+            * Math.PI;
+    // theoretical value
+    // FIXME measure and validate experimentally
+    public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND =
+        MAX_VELOCITY_METERS_PER_SECOND
+            / Math.hypot(Dims.TRACKWIDTH_METERS / 2.0, Dims.WHEELBASE_METERS / 2.0)
+            * .5;
+
+    /** the maximum amount of angular error pid loops will tolerate for rotation */
+    public static final double ANGULAR_ERROR = 1.0;
+    /** the minimum magnitude of the right stick for it to be used as a new rotation angle */
+    public static final double ROTATE_VECTOR_MAGNITUDE = .7;
+
+    public static final class Dims {
+      // FIXME validate with hardware
+      public static final double TRACKWIDTH_METERS =
+          .5207; // 20.5 inches (source: cad) converted to
+      // meters
+      public static final double WHEELBASE_METERS = TRACKWIDTH_METERS; // robot is square
+
+      public static final double BUMPER_WIDTH_METERS_X = .9779;
+      public static final double BUMPER_WIDTH_METERS_Y = .8382;
+    }
+
+    /*
+     * module layout:
+     * |──────
+     * |->│# ##steer motor
+     * │ │ ##cancoder
+     * │ │##drive motor
+     * module number
+     *
+     * steer is always left
+     * from corner perspective
+     *
+     * robot visualization:
+     * |──────────────────────|
+     * │2 10 04 1│
+     * │ 25 24 │
+     * │11 S D 03│
+     * │ D S │
+     * │ │
+     * │ │
+     * │ S D │
+     * │ D S │
+     * │12 |────────| 02│
+     * │ 26 │ │ 27 │
+     * │3 13│ batt │01 4│
+     * |──────┴───┬┬───┴──────|
+     * ││
+     * ││
+     * ▼▼
+     * software front
+     */
+
+    public static final class Modules {
+      public static final class Params {
+        public static final double WHEEL_RADIUS = 2; // also in INCHES
+        public static final double COUPLING_GEAR_RATIO = 3.125;
+        public static final double DRIVE_GEAR_RATIO = 5.357142857142857;
+        public static final double STEER_GEAR_RATIO = 21.428571428571427;
+        public static final Slot0Configs DRIVE_MOTOR_GAINS =
+            new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0.32).withKV(0.11).withKA(0);
+        public static final Slot0Configs STEER_MOTOR_GAINS =
+            new Slot0Configs().withKP(11).withKI(0).withKD(0).withKS(0.4).withKV(0.6).withKA(0);
+        public static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT =
+            ClosedLoopOutputType.Voltage;
+        public static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT =
+            ClosedLoopOutputType.Voltage;
+        public static final SteerFeedbackType FEEDBACK_SOURCE = SteerFeedbackType.FusedCANcoder;
+        public static final double SPEED_TWELVE_VOLTS = MAX_VELOCITY_METERS_PER_SECOND;
+        public static final double SLIP_CURRENT = 0; // optional, unused rn
+        public static final boolean STEER_MOTOR_INVERTED = true;
+
+        public static final DriveRequestType driveRequestType = DriveRequestType.OpenLoopVoltage;
+        public static final SteerRequestType steerRequestType = SteerRequestType.MotionMagic;
+      }
+
+      public static final class Module1 { // back left
+        public static final int DRIVE_MOTOR = CAN.at(4, "module 1 drive motor");
+        public static final int STEER_MOTOR = CAN.at(3, "module 1 steer motor");
+        public static final int STEER_ENCODER = CAN.at(24, "module 1 steer encoder");
+
+        public static final double STEER_OFFSET =
+            IS_COMP_BOT
+                ? 0.41943359375 // comp bot offset
+                : 0.0595703125; // practice bot offset
+      }
+
+      public static final class Module2 { // back right
+        public static final int DRIVE_MOTOR = CAN.at(11, "module 2 drive motor");
+        public static final int STEER_MOTOR = CAN.at(10, "module 2 steer motor");
+        public static final int STEER_ENCODER = CAN.at(25, "module 2 steer encoder");
+
+        public static final double STEER_OFFSET =
+            IS_COMP_BOT
+                ? -0.39990234375 // comp bot offset
+                : 0.262451171875; // practice bot offset
+      }
+
+      public static final class Module3 { // front right
+        public static final int DRIVE_MOTOR = CAN.at(13, "module 3 drive motor");
+        public static final int STEER_MOTOR = CAN.at(12, "module 3 steer motor");
+        public static final int STEER_ENCODER = CAN.at(26, "module 3 steer encoder");
+
+        public static final double STEER_OFFSET =
+            IS_COMP_BOT
+                ? 0.225341796875 // comp bot offset
+                : -0.20825195312; // practice bot offset
+      }
+
+      public static final class Module4 { // front left
+        public static final int DRIVE_MOTOR = CAN.at(2, "module 4 drive motor");
+        public static final int STEER_MOTOR = CAN.at(1, "module 4 steer motor");
+        public static final int STEER_ENCODER = CAN.at(27, "module 4 steer encoder");
+
+        public static final double STEER_OFFSET =
+            IS_COMP_BOT
+                ? 0.316650390625 // comp bot offset
+                : -0.3564453125 + 180; // practice bot offset
+      }
+    }
+
+    public static final class Setpoints {
+      public static final Translation2d SPEAKER = new Translation2d(0, 5.5);
+
+      public static final int SOURCE_DEGREES = 39;
+      public static final int SPEAKER_DEGREES = 11;
+      public static final int EPSILON = 3;
+    }
+  }
+
+  public static final class Intake {
+    public static final class Ports {
+      public static final int INTAKE_MOTOR_PORT = 15;
+      public static final int SERIALIZER_MOTOR_PORT = 16;
+      public static final int INTAKE_SENSOR_PORT = 9;
+    }
+
+    public static final class Modes {
+      public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
+      public static final IntakePowers HOLD = new IntakePowers(0, 0d);
+      public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
+      public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 0.7);
+      public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1);
+    }
+
+    public static final boolean IS_BEAMBREAK = true;
+  }
+
+  public static final class Shooter {
+    public static final class Ports {
+      public static final int TOP_SHOOTER_MOTOR_PORT = 0;
+      public static final int BOTTOM_SHOOTER_MOTOR_PORT = 5;
+      public static final int ACCELERATOR_MOTOR_PORT = 17;
+      public static final int BEAM_BREAK_SENSOR_PORT = 8;
+    }
+
+    public static final class Modes {
+      public static final ShooterSubsystem.ShooterPowers INTAKE =
+          new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers IDLE =
+          new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers RAMP_AMP =
+          new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers RAMP_SPEAKER =
+          new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER =
+          new ShooterSubsystem.ShooterPowers(76, 1, .5, 0);
+      public static final ShooterSubsystem.ShooterPowers TARGET_LOCK =
+          new ShooterSubsystem.ShooterPowers(0, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers SHOOT_AMP =
+          new ShooterSubsystem.ShooterPowers(0, 0, 0, -1);
+      public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY =
+          new ShooterSubsystem.ShooterPowers(40, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers SHUTTLE =
+          new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE =
+          new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
+      public static final ShooterSubsystem.ShooterPowers ACCEL_SECURE =
+          new ShooterSubsystem.ShooterPowers(76, 1, 0.5, 0);
+      public static final ShooterSubsystem.ShooterPowers VARIABLE_VELOCITY =
+          new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
+      public static final ShooterSubsystem.ShooterPowers SHOOT_VAR =
+          new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
+      public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER =
+          new ShooterSubsystem.ShooterPowers(0, 0, 0.5, 0.5);
+      public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD =
+          new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5);
+    }
+
+    public static final Slot0Configs ROLLER_PID_CONFIG =
+        new Slot0Configs().withKP(0.2).withKS(0.23).withKV(0.24);
+
+    public static final double SHOOTER_VELOCITY_THRESHOLD = 30;
+  }
+
+  public static final class Pivot {
+    public static final class Ports {
+      public static final int PIVOT_MOTOR_PORT = 18;
+      public static final int CANCODER_PORT = 28;
+      public static final int INDUCTIVE_PROXIMITY_SENSOR_PORT = 40;
+    }
+
+    public static final class MotorConfigs {
+      public static final MagnetSensorConfigs CANCODER_MAGNET_SENSOR =
+          new MagnetSensorConfigs()
+              .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+              .withSensorDirection(SensorDirectionValue.Clockwise_Positive)
+              .withMagnetOffset(PIVOT_CANCODER_OFFSET);
+      public static final CANcoderConfiguration CANCODER_CONFIG =
+          new CANcoderConfiguration().withMagnetSensor(CANCODER_MAGNET_SENSOR);
+
+      public static final FeedbackConfigs PIVOT_FEEDBACK =
+          new FeedbackConfigs()
+              .withFeedbackRemoteSensorID(Ports.CANCODER_PORT)
+              .withFeedbackSensorSource(FeedbackSensorSourceValue.RemoteCANcoder)
+              .withSensorToMechanismRatio(1.0)
+              .withRotorToSensorRatio(PIVOT_GEAR_RATIO);
+      public static final SoftwareLimitSwitchConfigs PIVOT_SOFTWARE_LIMIT =
+          new SoftwareLimitSwitchConfigs()
+              .withForwardSoftLimitThreshold(0.25)
+              .withReverseSoftLimitThreshold(-0.015)
+              .withForwardSoftLimitEnable(false)
+              .withReverseSoftLimitEnable(false);
+      public static final VoltageConfigs PIVOT_VOLTAGE =
+          new VoltageConfigs().withPeakForwardVoltage(5).withPeakReverseVoltage(-5);
+      public static final TalonFXConfiguration PIVOT_CONFIG =
+          new TalonFXConfiguration()
+              .withFeedback(PIVOT_FEEDBACK)
+              .withSoftwareLimitSwitch(PIVOT_SOFTWARE_LIMIT)
+              .withVoltage(PIVOT_VOLTAGE);
+    }
+
+    public static final class Setpoints {
+      public static final int MINIMUM_ANGLE = 15;
+      public static final int MAXIMUM_ANGLE = 125;
+
+      public static final int MINIMUM_SAFE_THRESHOLD = 15;
+      public static final int MAXIMUM_SAFE_THRESHOLD = 80;
+
+      public static final int SPEAKER_ANGLE = 30;
+    }
+
+    public static final int EPSILON = 2;
+
+    public static final double PIVOT_CANCODER_OFFSET = -0.625977 + (0.070139 - 0.031250);
+    /** degrees per rotation */
+    public static final double PIVOT_GEAR_RATIO = 5.33333; // not used
+    // values
+    public static final double CENTER_OF_ROBOT_TO_BUMPER = 0.41275;
+
+    public static final Pose2d RED_SPEAKER_POSE = new Pose2d(16.45, 5.5, null);
+    public static final Pose2d BLUE_SPEAKER_POSE = new Pose2d(0, 5.5, null);
+
+    public static final Pose2d BLUE_SHUTTLE_POSE = new Pose2d(1, 7.25, null);
+    public static final Pose2d RED_SHUTTLE_POSE = new Pose2d(15.5, 7.25, null);
+
+    public static final double GRAVITY = 9.80665; // meters per second
+
+    public static final double GRAVITY_VOLTAGE = 0.4;
+    public static final double PIVOT_MAX_VOLTAGE = 3.5;
+  }
+
+  public static final class Vision {
+    public static record VisionSource(String name, Transform3d robotToCamera) {}
+
+    public static final List<VisionSource> VISION_SOURCES =
+        List.of(
+            /*
+             * new VisionSource(
+             * "frontCam",
+             * new Transform3d(
+             * new Translation3d(
+             * -0.305, // front/back
+             * -0.2286, // left/right
+             * -0.2159 // up/down
+             * ),
+             * new Rotation3d(
+             * 0,
+             * Math.toRadians(30), // angle up/down
+             * Math.toRadians(180)))),
+             * new VisionSource(
+             * "backCam",
+             * new Transform3d(
+             * new Translation3d(
+             * -0.2796, // front/back
+             * -0.2286, // left/right
+             * -0.2159 // up/down
+             * ),
+             * new Rotation3d(
+             * 0,
+             * Math.toRadians(30), // angle up/down
+             * Math.toRadians(180)))),
+             */
+            new VisionSource(
+                "backUpCam",
+                new Transform3d(
+                    new Translation3d(
+                        -0.2796, // front/back
+                        0.2286, // left/right
+                        -0.2159 // up/down
+                        ),
+                    new Rotation3d(
+                        0,
+                        Math.toRadians(30), // angle up/down
+                        Math.toRadians(180)))));
+
+    public static final int THREAD_SLEEP_DURATION_MS = 5;
+  }
+
+  public static final class PoseEstimator {
+    /**
+     * Standard deviations of model states. Increase these numbers to trust your model's state
+     * estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    public static final Matrix<N3, N1> STATE_STANDARD_DEVIATIONS =
+        MatBuilder.fill(
+            Nat.N3(),
+            Nat.N1(),
+            0.1, // x
+            0.1, // y
+            0.1 // theta
+            );
+
+    /**
+     * Standard deviations of the vision measurements. Increase these numbers to trust global
+     * measurements from vision less. This matrix is in the form [x, y, theta]ᵀ, with units in
+     * meters and radians.
+     *
+     * <p>These are not actually used anymore, but the constructor for the pose estimator wants
+     * them. This value is calculated dynamically using the below list.
+     */
+    public static final Matrix<N3, N1> VISION_MEASUREMENT_STANDARD_DEVIATIONS =
+        MatBuilder.fill(
+            Nat.N3(),
+            Nat.N1(),
+            1, // x
+            1, // y
+            1 * Math.PI // theta
+            );
+
+    public static final double POSE_AMBIGUITY_CUTOFF = .05;
+
+    public static final List<TagCountDeviation> TAG_COUNT_DEVIATION_PARAMS =
+        List.of(
+            // 1 tag
+            new TagCountDeviation(
+                new UnitDeviationParams(.25, .4, .9),
+                new UnitDeviationParams(.35, .5, 1.2),
+                new UnitDeviationParams(.5, .7, 1.5)),
+
+            // 2 tags
+            new TagCountDeviation(
+                new UnitDeviationParams(.35, .1, .4), new UnitDeviationParams(.5, .7, 1.5)),
+
+            // 3+ tags
+            new TagCountDeviation(
+                new UnitDeviationParams(.25, .07, .25), new UnitDeviationParams(.15, 1, 1.5)));
+
+    /** about one inch */
+    public static final double DRIVE_TO_POSE_XY_ERROR_MARGIN_METERS = .025;
+
+    public static final double DRIVE_TO_POSE_THETA_ERROR_MARGIN_DEGREES = 2;
+
+    public static final List<Set<Integer>> POSSIBLE_FRAME_FID_COMBOS =
+        List.of(Set.of(1, 2, 3, 4, 5), Set.of(6, 7, 8, 9, 10));
+
+    public static final List<Set<Integer>> SPEAKER_FIDS = List.of(Set.of(3, 4), Set.of(7, 8));
+
+    public static final int MAX_FRAME_FIDS = 4;
+  }
+
+  public static final class NetworkWatchdog {
+    /** The IP addresses to ping for testing bridging, on the second vlan. */
+    public static final List<IPv4> TEST_IP_ADDRESSES =
+        List.of(IPv4.internal(17), IPv4.internal(18), IPv4.internal(19));
+
+    /**
+     * The number of ms (sleep delta using oshi system uptime) to wait before beginning to ping the
+     * test IP.
+     */
+    public static final int BOOT_SCAN_DELAY_MS = 50_000;
+
+    /** The number of seconds for ping to wait before giving up on reaching a device. */
+    public static final int PING_TIMEOUT_SECONDS = 2;
+
+    /** The number of ms to wait before retrying successful health checks. */
+    public static final int HEALTHY_CHECK_INTERVAL_MS = 5_000;
+
+    /**
+     * The number of ms to leave the switching pdh port off before turning it back on as part of
+     * rebooting the network switch.
+     */
+    public static final int REBOOT_DURATION_MS = 1_000;
+
+    /**
+     * The number of ms to wait before rerunning health checks after a failed check which triggered
+     * switch reboot.
+     */
+    public static final int SWITCH_POWERCYCLE_SCAN_DELAY_MS = 25_000;
+  }
+
+  public static final class CANWatchdog {
+    public static final int SCAN_DELAY_MS = 100;
+  }
+
+  public static final class Lights {
+    public static final int CANDLE_ID = 34;
+    public static final int NUM_LEDS =
+        89
+            // 8 inside the candle, 8 more for balance
+            + 8 * 2;
+
+    public static final class Colors {
+      public static final RGBColor YELLOW = new RGBColor(255, 107, 0);
+      public static final RGBColor PURPLE = new RGBColor(127, 0, 127);
+      public static final RGBColor RED = new RGBColor(255, 0, 0);
+      public static final RGBColor ORANGE = new RGBColor(255, 35, 0);
+      public static final RGBColor BLUE = new RGBColor(0, 0, 255);
+      public static final RGBColor PINK = new RGBColor(250, 35, 100);
+      public static final RGBColor MINT = new RGBColor(55, 255, 50);
+      public static final RGBColor TEAL = new RGBColor(0, 255, 255);
+      public static final RGBColor WHITE = new RGBColor(255, 255, 255);
+    }
+  }
+
+  public static final class AutoAlign {
+
+    public static final double FIELD_WIDTH = 16.54;
+
+    // Blue team:
+    // pose angles
+    public static final int BOTTOM_MID_ANGLE = 148;
+    public static final int STAGE_ANGLE = 0;
+    public static final int TOP_MID_ANGLE = 0;
+    public static final int AMP_ANGLE = 0;
+
+    // These poses have not been verified
+    public static final Pose2d BLUE_AMP =
+        new Pose2d(2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE + 180));
+    public static final Pose2d BLUE_TOP_MID =
+        new Pose2d(5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE + 180));
+
+    // These poses have been verified
+    public static final Pose2d BLUE_STAGE =
+        new Pose2d(4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE + 180));
+    public static final Pose2d BLUE_BOTTOM_MID =
+        new Pose2d(5.84, 1.18, Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE + 180));
+
+    // Red team:
+    // These poses have not been verified
+    public static final Pose2d RED_AMP =
+        new Pose2d(FIELD_WIDTH - 2.75, 7.31, Rotation2d.fromDegrees(-AMP_ANGLE));
+    public static final Pose2d RED_TOP_MID =
+        new Pose2d(FIELD_WIDTH - 5.8, 7.0, Rotation2d.fromDegrees(-TOP_MID_ANGLE));
+
+    // These poses have been verified
+    public static final Pose2d RED_STAGE =
+        new Pose2d(FIELD_WIDTH - 4.17, 4.74, Rotation2d.fromDegrees(-STAGE_ANGLE));
+    public static final Pose2d RED_BOTTOM_MID =
+        new Pose2d(FIELD_WIDTH - 5.84, 1.18, Rotation2d.fromDegrees(-BOTTOM_MID_ANGLE));
+
+    public static final double BOTTOM_MID_TARGET_ANGLE = 23.5;
+    public static final double TOP_MID_TARGET_ANGLE = 0;
+    public static final double STAGE_TARGET_ANGLE = 0;
+    public static final double AMP_TARGET_ANGLE = 0;
+    public static final PathConstraints CONSTRAINTS =
+        new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720));
+  }
+
+  public static class Elevator {
+    public static final class Ports {
+      public static final int ARM_MOTOR_PORT = 18;
+      public static final int ELEVATOR_MOTOR_PORT = 28;
+    }
+    /** inches per rotation */
+    public static final double ELEVATOR_GEAR_RATIO = 0.0906065;
+    /** FIXME */
+    public static final double MAX_HEIGHT = 50;
+    /** FIXME */
+    public static final double ELEVATOR_FEEDFORWARD = 0.01;
+
+    public static final class Arm {
+      /** FIXME */
+      public static final double MAX_ANGLE = 50;
+      /** inches per rotation */
+      public static final double ARM_GEAR_RATIO = 0.0231481481;
+    }
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 6a76939..a96ef14 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -75,470 +75,463 @@
 import java.util.function.DoubleSupplier;
 
 /**
- * This class is where the bulk of the robot should be declared. Since
- * Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in
- * the {@link Robot}
- * periodic methods (other than the scheduler calls). Instead, the structure of
- * the robot (including
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
-    // The robot's subsystems and commands are defined here...
-
-    private final VisionSubsystem visionSubsystem = new VisionSubsystem();
-
-    private final DrivebaseSubsystem drivebaseSubsystem = new DrivebaseSubsystem(visionSubsystem);
-
-    private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
-
-    private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
-
-    private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
-
-    private final RGBSubsystem rgbSubsystem = new RGBSubsystem();
-
-    private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
-
-    private final NetworkWatchdogSubsystem networkWatchdogSubsystem = new NetworkWatchdogSubsystem(
-            Optional.of(rgbSubsystem));
-
-    private final CANWatchdogSubsystem canWatchdogSubsystem = new CANWatchdogSubsystem(rgbSubsystem);
-
-    /** controller 1 */
-    private final CommandXboxController jacob = new CommandXboxController(1);
-    /** controller 1 layer */
-    private final Layer jacobLayer = new Layer(jacob.rightBumper());
-    /** controller 0 */
-    private final CommandXboxController anthony = new CommandXboxController(0);
-    /** controller 0 layer */
-    // private final Layer anthonyLayer = new Layer(anthony.rightBumper());
-
-    /** the sendable chooser to select which auto to run. */
-    private final SendableChooser<Command> autoSelector;
-
-    private GenericEntry autoDelay;
-
-    private Pose2d desiredPose;
-
-    private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView");
-
-    /* drive joystick "y" is passed to x because controller is inverted */
-    private final DoubleSupplier translationXSupplier = () -> (-modifyAxis(anthony.getLeftY())
-            * Drive.MAX_VELOCITY_METERS_PER_SECOND);
-    private final DoubleSupplier translationYSupplier = () -> (-modifyAxis(anthony.getLeftX())
-            * Drive.MAX_VELOCITY_METERS_PER_SECOND);
-
-    /**
-     * The container for the robot. Contains subsystems, OI devices, and commands.
-     */
-    public RobotContainer() {
-
-        // // reigster commands for pathplanner
-        // NamedCommands.registerCommand(
-        //         "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-        // NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
-        // NamedCommands.registerCommand(
-        //         "ShooterRampUpCommand",
-        //         new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
-        // NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
-        // NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
-        // NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
-        // NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
-        // NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
-        // NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
-        // NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
-        // NamedCommands.registerCommand(
-        //         "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
-        // NamedCommands.registerCommand(
-        //         "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
-        // NamedCommands.registerCommand(
-        //         "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
-        // NamedCommands.registerCommand(
-        //         "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
-        // NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
-        // NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
-        // NamedCommands.registerCommand(
-        //         "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
-        // NamedCommands.registerCommand(
-        //         "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
-        // NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
-        // NamedCommands.registerCommand(
-        //         "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
-        // NamedCommands.registerCommand(
-        //         "ZeroSubwoofer1",
-        //         new InstantCommand(
-        //                 () -> drivebaseSubsystem.zeroGyroscopeOffset(
-        //                         DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
-        // NamedCommands.registerCommand(
-        //         "ZeroSubwoofer3",
-        //         new InstantCommand(
-        //                 () -> drivebaseSubsystem.zeroGyroscopeOffset(
-        //                         DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
-        // NamedCommands.registerCommand(
-        //         "recenterPose1",
-        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
-        //                 ? new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
-        //                         drivebaseSubsystem)
-        //                 : new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
-        //                         drivebaseSubsystem));
-        // NamedCommands.registerCommand(
-        //         "recenterPose2",
-        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
-        //                 ? new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
-        //                         drivebaseSubsystem)
-        //                 : new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
-        //                         drivebaseSubsystem));
-        // NamedCommands.registerCommand(
-        //         "recenterPose3",
-        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
-        //                 ? new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
-        //                         drivebaseSubsystem)
-        //                 : new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
-        //                         drivebaseSubsystem));
-        // NamedCommands.registerCommand(
-        //         "recenterPose4",
-        //         DriverStation.getAlliance().get().equals(Alliance.Blue)
-        //                 ? new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
-        //                         drivebaseSubsystem)
-        //                 : new InstantCommand(
-        //                         () -> drivebaseSubsystem.resetOdometryToPose(
-        //                                 new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
-        //                         drivebaseSubsystem));
-
-        // Set up the default command for the drivetrain.
-        // The controls are for field-oriented driving:
-        // Left stick Y axis -> forward and backwards movement
-        // Left stick X axis -> left and right movement
-        // Right stick X axis -> rotation
-        drivebaseSubsystem.setDefaultCommand(
-                new DefaultDriveCommand(
-                        drivebaseSubsystem,
-                        translationXSupplier,
-                        translationYSupplier,
-                        // anthony.rightBumper(),
-                        anthony.leftBumper()));
-
-        rgbSubsystem.setDefaultCommand(
-                new RGBCommand(
-                        shooterSubsystem,
-                        intakeSubsystem,
-                        rgbSubsystem,
-                        pivotSubsystem,
-                        drivebaseSubsystem,
-                        visionSubsystem));
-
-        // pivotSubsystem.setDefaultCommand(
-        // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY()));
-
-        // Configure the button bindings
-        configureButtonBindings();
-
-        autoSelector = AutoBuilder.buildAutoChooser();
-
-        SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT);
-        SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA);
-        SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_INIT);
-
-        desiredPose = new Pose2d();
-        SmartDashboard.putString(
-                "desired pose",
-                String.format(
-                        "(%2f %2f %2f)",
-                        desiredPose.getX(), desiredPose.getY(), desiredPose.getRotation().getDegrees()));
-
-        if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
-            driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
-            driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
-            driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
-            driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
-        }
-
-        // Create and put autonomous selector to dashboard
-        setupAutonomousCommands();
+  // The robot's subsystems and commands are defined here...
+
+  private final VisionSubsystem visionSubsystem = new VisionSubsystem();
+
+  private final DrivebaseSubsystem drivebaseSubsystem = new DrivebaseSubsystem(visionSubsystem);
+
+  private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+
+  private final PivotSubsystem pivotSubsystem = new PivotSubsystem();
+
+  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+
+  private final RGBSubsystem rgbSubsystem = new RGBSubsystem();
+
+  private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
+
+  private final NetworkWatchdogSubsystem networkWatchdogSubsystem =
+      new NetworkWatchdogSubsystem(Optional.of(rgbSubsystem));
+
+  private final CANWatchdogSubsystem canWatchdogSubsystem = new CANWatchdogSubsystem(rgbSubsystem);
+
+  /** controller 1 */
+  private final CommandXboxController jacob = new CommandXboxController(1);
+  /** controller 1 layer */
+  private final Layer jacobLayer = new Layer(jacob.rightBumper());
+  /** controller 0 */
+  private final CommandXboxController anthony = new CommandXboxController(0);
+  /** controller 0 layer */
+  // private final Layer anthonyLayer = new Layer(anthony.rightBumper());
+
+  /** the sendable chooser to select which auto to run. */
+  private final SendableChooser<Command> autoSelector;
+
+  private GenericEntry autoDelay;
+
+  private Pose2d desiredPose;
+
+  private final ShuffleboardTab driverView = Shuffleboard.getTab("DriverView");
+
+  /* drive joystick "y" is passed to x because controller is inverted */
+  private final DoubleSupplier translationXSupplier =
+      () -> (-modifyAxis(anthony.getLeftY()) * Drive.MAX_VELOCITY_METERS_PER_SECOND);
+  private final DoubleSupplier translationYSupplier =
+      () -> (-modifyAxis(anthony.getLeftX()) * Drive.MAX_VELOCITY_METERS_PER_SECOND);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+
+    // reigster commands for pathplanner
+    NamedCommands.registerCommand(
+        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+    NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
+    NamedCommands.registerCommand(
+        "ShooterRampUpCommand",
+        new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+    NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
+    NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
+    NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
+    NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
+    NamedCommands.registerCommand("AngleAt3", new PivotAngleCommand(pivotSubsystem, 32));
+    NamedCommands.registerCommand("AngleAtFar", new PivotAngleCommand(pivotSubsystem, 30));
+    NamedCommands.registerCommand("AngleAtCenter1", new PivotAngleCommand(pivotSubsystem, 22.5));
+    NamedCommands.registerCommand(
+        "Heading4Note1", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -90));
+    NamedCommands.registerCommand(
+        "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
+    NamedCommands.registerCommand(
+        "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
+    NamedCommands.registerCommand(
+        "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
+    NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
+    NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
+    NamedCommands.registerCommand(
+        "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
+    NamedCommands.registerCommand(
+        "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
+    NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
+    NamedCommands.registerCommand(
+        "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
+    NamedCommands.registerCommand(
+        "ZeroSubwoofer1",
+        new InstantCommand(
+            () ->
+                drivebaseSubsystem.zeroGyroscopeOffset(
+                    DriverStation.getAlliance().get().equals(Alliance.Blue) ? -60 : 60)));
+    NamedCommands.registerCommand(
+        "ZeroSubwoofer3",
+        new InstantCommand(
+            () ->
+                drivebaseSubsystem.zeroGyroscopeOffset(
+                    DriverStation.getAlliance().get().equals(Alliance.Blue) ? 60 : -60)));
+    NamedCommands.registerCommand(
+        "recenterPose1",
+        DriverStation.getAlliance().get().equals(Alliance.Blue)
+            ? new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(1.12, 6.68), Rotation2d.fromDegrees(56.93))),
+                drivebaseSubsystem)
+            : new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(-56.93))),
+                drivebaseSubsystem));
+    NamedCommands.registerCommand(
+        "recenterPose2",
+        DriverStation.getAlliance().get().equals(Alliance.Blue)
+            ? new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(1.4, 5.56), Rotation2d.fromDegrees(0))),
+                drivebaseSubsystem)
+            : new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(15.2, 5.56), Rotation2d.fromDegrees(180))),
+                drivebaseSubsystem));
+    NamedCommands.registerCommand(
+        "recenterPose3",
+        DriverStation.getAlliance().get().equals(Alliance.Blue)
+            ? new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(1.12, 4.36), Rotation2d.fromDegrees(-98.90))),
+                drivebaseSubsystem)
+            : new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(15.6, 6.68), Rotation2d.fromDegrees(98.90))),
+                drivebaseSubsystem));
+    NamedCommands.registerCommand(
+        "recenterPose4",
+        DriverStation.getAlliance().get().equals(Alliance.Blue)
+            ? new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(1, 2.5), Rotation2d.fromDegrees(0))),
+                drivebaseSubsystem)
+            : new InstantCommand(
+                () ->
+                    drivebaseSubsystem.resetOdometryToPose(
+                        new Pose2d(new Translation2d(15.5, 2.5), Rotation2d.fromDegrees(180))),
+                drivebaseSubsystem));
+
+    // Set up the default command for the drivetrain.
+    // The controls are for field-oriented driving:
+    // Left stick Y axis -> forward and backwards movement
+    // Left stick X axis -> left and right movement
+    // Right stick X axis -> rotation
+    drivebaseSubsystem.setDefaultCommand(
+        new DefaultDriveCommand(
+            drivebaseSubsystem,
+            translationXSupplier,
+            translationYSupplier,
+            // anthony.rightBumper(),
+            anthony.leftBumper()));
+
+    rgbSubsystem.setDefaultCommand(
+        new RGBCommand(
+            shooterSubsystem,
+            intakeSubsystem,
+            rgbSubsystem,
+            pivotSubsystem,
+            drivebaseSubsystem,
+            visionSubsystem));
+
+    // pivotSubsystem.setDefaultCommand(
+    // new PivotManualCommand(pivotSubsystem, () -> -jacob.getLeftY()));
+
+    // Configure the button bindings
+    configureButtonBindings();
+
+    autoSelector = AutoBuilder.buildAutoChooser();
+
+    SmartDashboard.putBoolean("is comp bot", MacUtil.IS_COMP_BOT);
+    SmartDashboard.putBoolean("show debug data", Config.SHOW_SHUFFLEBOARD_DEBUG_DATA);
+    SmartDashboard.putBoolean("don't init swerve modules", Config.DISABLE_SWERVE_INIT);
+
+    desiredPose = new Pose2d();
+    SmartDashboard.putString(
+        "desired pose",
+        String.format(
+            "(%2f %2f %2f)",
+            desiredPose.getX(), desiredPose.getY(), desiredPose.getRotation().getDegrees()));
+
+    if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
+      driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
+      driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
+      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
+      driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
     }
 
-    /**
-     * Use this method to do things as the drivers gain control of the robot. We use
-     * it to vibrate the
-     * driver b controller to notice accidental swaps.
-     *
-     * <p>
-     * Please use this very, very sparingly. It doesn't exist by default for good
-     * reason.
-     */
-    public void containerTeleopInit() {
-        // runs when teleop happens
-        CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jacob.getHID(), 5, .5));
-        // vibrate controller at 27 seconds left
-        CommandScheduler.getInstance()
-                .schedule(
-                        new WaitCommand(108)
-                                .andThen(
-                                        new ParallelCommandGroup(
-                                                new VibrateHIDCommand(anthony.getHID(), 3, 0.4),
-                                                new VibrateHIDCommand(jacob.getHID(), 3, 0.4))));
-    }
-
-    /**
-     * Use this method to do things as soon as the robot starts being used. We use
-     * it to stop doing
-     * things that could be harmful or undesirable during game play--rebooting the
-     * network switch is a
-     * good example. Subsystems need to be explicitly wired up to this method.
-     *
-     * <p>
-     * Depending on which mode the robot is enabled in, this will either be called
-     * before auto or
-     * before teleop, whichever is first.
-     *
-     * <p>
-     * Please use this very, very sparingly. It doesn't exist by default for good
-     * reason.
-     */
-    public void containerMatchStarting() {
-        // runs when the match starts
-        networkWatchdogSubsystem.matchStarting();
-        canWatchdogSubsystem.matchStarting();
-    }
-
-    /**
-     * Use this method to define your button->command mappings. Buttons can be
-     * created by
-     * instantiating a {@link GenericHID} or one of its subclasses ({@link
-     * 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() {
-        // vibrate jacob controller when in layer
-        jacobLayer.whenChanged(
-                (enabled) -> {
-                    final double power = enabled ? .1 : 0;
-                    jacob.getHID().setRumble(RumbleType.kLeftRumble, power);
-                    jacob.getHID().setRumble(RumbleType.kRightRumble, power);
-                });
-
-        anthony
-                .start()
-                .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem));
-
-        jacob
-                .start()
-                .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem));
-
-        // STOP INTAKE-SHOOTER
-        jacob
-                .x()
-                .onTrue(
-                        new StopShooterCommand(shooterSubsystem)
-                                .alongWith(new StopIntakeCommand(intakeSubsystem)));
-        // OUTTAKE
-        jacob.rightBumper().onTrue(new OuttakeCommand(intakeSubsystem));
-
-        // INTAKE
-        anthony
-                .leftBumper()
-                .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-
-        jacob
-                .leftBumper()
-                .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
-
-        // SHOOT
-        anthony
-                .rightBumper()
-                .onTrue(
-                        new AccelNoteCommand(shooterSubsystem)
-                                .andThen(new ShootCommand(shooterSubsystem))
-                                .andThen(
-                                        new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
-
-        // SHOOT OVERRIDE
-        jacob
-                .rightTrigger()
-                .onTrue(new AccelNoteCommand(shooterSubsystem)
-                        .andThen(new ShootCommand(shooterSubsystem)));
-
-        anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
-        anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
-        jacob
-                .y()
-                .whileTrue(
-                        new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
-                                .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
-
-        // jacob
-        //         .a()
-        //         .onTrue(
-        //                 new RotateAngleDriveCommand(
-        //                         drivebaseSubsystem,
-        //                         translationXSupplier,
-        //                         translationYSupplier,
-        //                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
-        //                         .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
-        //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.SHUTTLE)));
-
-        jacob
-                .povDown()
-                .onTrue(
-                        new PivotAngleCommand(pivotSubsystem, 15)
-                                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
-
-        // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
-        // translationXSupplier,
-        // translationYSupplier, Setpoints.SPEAKER));
-
-        // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
-
-        // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
-        // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
-
-        DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
-
-        new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
-                .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
-
-        // SOURCE
-        anthony
-                .y()
-                .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                DriverStation.getAlliance().get().equals(Alliance.Red)
-                                        ? -Setpoints.SOURCE_DEGREES
-                                        : Setpoints.SOURCE_DEGREES)
-                                .alongWith(
-                                        new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
-
-        // NOTE TO SHOOTER OR SERIALIZER
-        anthony
-                .b()
-                .onTrue(
-                        new TransferNoteCommand(shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem));
-
-        jacob
-                .b()
-                .onTrue(
-                        new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
-        jacob
-                .a()
-                .onTrue(
-                        new ShootCommand(shooterSubsystem));
-
-        // SPEAKER FROM SUBWOOFER
-        anthony
-                .a()
-                .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem, translationXSupplier, translationYSupplier, 0)
-                                .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1)));
-
-        anthony
-                .x()
-                .onTrue(
-                        new RotateAngleDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
-                                .alongWith(new HeightCommand(elevatorSubsystem, 20)));
-
-        DoubleSupplier rotation = exponential(
-                () -> ControllerUtil.deadband(
-                        (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1),
-                2);
-
-        DoubleSupplier rotationVelocity = () -> -rotation.getAsDouble() * Drive.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND
-                * 0.8;
-
-        new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
-                .whileTrue(
-                        new RotateVelocityDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                rotationVelocity,
-                                anthony.rightBumper()));
-
-        new Trigger(
-                () -> Util.vectorMagnitude(anthony.getRightY(), anthony.getRightX()) > Drive.ROTATE_VECTOR_MAGNITUDE)
-                .onTrue(
-                        new RotateVectorDriveCommand(
-                                drivebaseSubsystem,
-                                translationXSupplier,
-                                translationYSupplier,
-                                anthony::getRightY,
-                                anthony::getRightX,
-                                anthony.rightBumper()));
-    }
-
-    /**
-     * Adds all autonomous routines to the autoSelector, and places the autoSelector
-     * on Shuffleboard.
-     */
-    private void setupAutonomousCommands() {
-        driverView.addString("NOTES", () -> "...win?\nor not.").withSize(4, 1).withPosition(7, 2);
-
-        driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);
-
-        autoDelay = driverView
-                .add("auto delay", 0)
-                .withWidget(BuiltInWidgets.kNumberSlider)
-                .withProperties(Map.of("min", 0, "max", 15, "block increment", .1))
-                .withSize(4, 1)
-                .withPosition(7, 1)
-                .getEntry();
-    }
-
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-        double delay = autoDelay.getDouble(0);
-        return delay == 0
-                ? autoSelector.getSelected()
-                : new WaitCommand(delay).andThen(autoSelector.getSelected());
-    }
-
-    /**
-     * applies deadband and squares axis
-     *
-     * @param value the axis value to be modified
-     * @return the modified axis values
-     */
-    private static double modifyAxis(double value) {
-        // Deadband
-        value = ControllerUtil.deadband(value, 0.07);
-
-        // Square the axis
-        value = Math.copySign(value * value, value);
-
-        return value;
-    }
-
-    private static DoubleSupplier exponential(DoubleSupplier supplier, double exponential) {
-        return () -> {
-            double val = supplier.getAsDouble();
-            return Math.copySign(Math.pow(val, exponential), val);
-        };
-    }
+    // Create and put autonomous selector to dashboard
+    setupAutonomousCommands();
+  }
+
+  /**
+   * Use this method to do things as the drivers gain control of the robot. We use it to vibrate the
+   * driver b controller to notice accidental swaps.
+   *
+   * <p>Please use this very, very sparingly. It doesn't exist by default for good reason.
+   */
+  public void containerTeleopInit() {
+    // runs when teleop happens
+    CommandScheduler.getInstance().schedule(new VibrateHIDCommand(jacob.getHID(), 5, .5));
+    // vibrate controller at 27 seconds left
+    CommandScheduler.getInstance()
+        .schedule(
+            new WaitCommand(108)
+                .andThen(
+                    new ParallelCommandGroup(
+                        new VibrateHIDCommand(anthony.getHID(), 3, 0.4),
+                        new VibrateHIDCommand(jacob.getHID(), 3, 0.4))));
+  }
+
+  /**
+   * Use this method to do things as soon as the robot starts being used. We use it to stop doing
+   * things that could be harmful or undesirable during game play--rebooting the network switch is a
+   * good example. Subsystems need to be explicitly wired up to this method.
+   *
+   * <p>Depending on which mode the robot is enabled in, this will either be called before auto or
+   * before teleop, whichever is first.
+   *
+   * <p>Please use this very, very sparingly. It doesn't exist by default for good reason.
+   */
+  public void containerMatchStarting() {
+    // runs when the match starts
+    networkWatchdogSubsystem.matchStarting();
+    canWatchdogSubsystem.matchStarting();
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * 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() {
+    // vibrate jacob controller when in layer
+    jacobLayer.whenChanged(
+        (enabled) -> {
+          final double power = enabled ? .1 : 0;
+          jacob.getHID().setRumble(RumbleType.kLeftRumble, power);
+          jacob.getHID().setRumble(RumbleType.kRightRumble, power);
+        });
+
+    anthony
+        .start()
+        .onTrue(new InstantCommand(drivebaseSubsystem::zeroGyroscope, drivebaseSubsystem));
+
+    jacob
+        .start()
+        .onTrue(new InstantCommand(drivebaseSubsystem::smartZeroGyroscope, drivebaseSubsystem));
+
+    // STOP INTAKE-SHOOTER
+    jacob
+        .x()
+        .onTrue(
+            new StopShooterCommand(shooterSubsystem)
+                .alongWith(new StopIntakeCommand(intakeSubsystem)));
+    // OUTTAKE
+    jacob.rightBumper().onTrue(new OuttakeCommand(intakeSubsystem));
+
+    // INTAKE
+    anthony
+        .leftBumper()
+        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+
+    jacob
+        .leftBumper()
+        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+
+    // SHOOT
+    anthony
+        .rightBumper()
+        .onTrue(
+            new AccelNoteCommand(shooterSubsystem)
+                .andThen(new ShootCommand(shooterSubsystem))
+                .andThen(
+                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+
+    // SHOOT OVERRIDE
+    jacob
+        .rightTrigger()
+        .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem)));
+
+    anthony.rightStick().onTrue(new DefenseModeCommand(drivebaseSubsystem));
+    anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem));
+    jacob
+        .y()
+        .whileTrue(
+            new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
+                .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
+
+    // jacob
+    //         .a()
+    //         .onTrue(
+    //                 new RotateAngleDriveCommand(
+    //                         drivebaseSubsystem,
+    //                         translationXSupplier,
+    //                         translationYSupplier,
+    //                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
+    //                         .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
+    //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem,
+    // ShooterMode.SHUTTLE)));
+
+    jacob
+        .povDown()
+        .onTrue(
+            new PivotAngleCommand(pivotSubsystem, 15)
+                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
+
+    // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
+    // translationXSupplier,
+    // translationYSupplier, Setpoints.SPEAKER));
+
+    // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
+
+    // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
+    // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
+
+    DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
+
+    new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
+        .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
+
+    // SOURCE
+    anthony
+        .y()
+        .onTrue(
+            new RotateAngleDriveCommand(
+                    drivebaseSubsystem,
+                    translationXSupplier,
+                    translationYSupplier,
+                    DriverStation.getAlliance().get().equals(Alliance.Red)
+                        ? -Setpoints.SOURCE_DEGREES
+                        : Setpoints.SOURCE_DEGREES)
+                .alongWith(
+                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+
+    // NOTE TO SHOOTER OR SERIALIZER
+    anthony
+        .b()
+        .onTrue(
+            new TransferNoteCommand(
+                shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem));
+
+    jacob.b().onTrue(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+    jacob.a().onTrue(new ShootCommand(shooterSubsystem));
+
+    // SPEAKER FROM SUBWOOFER
+    anthony
+        .a()
+        .onTrue(
+            new RotateAngleDriveCommand(
+                    drivebaseSubsystem, translationXSupplier, translationYSupplier, 0)
+                .alongWith(new PivotAngleCommand(pivotSubsystem, 53.1)));
+
+    anthony
+        .x()
+        .onTrue(
+            new RotateAngleDriveCommand(
+                    drivebaseSubsystem,
+                    translationXSupplier,
+                    translationYSupplier,
+                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+                .alongWith(new HeightCommand(elevatorSubsystem, 20)));
+
+    DoubleSupplier rotation =
+        exponential(
+            () ->
+                ControllerUtil.deadband(
+                    (anthony.getRightTriggerAxis() + -anthony.getLeftTriggerAxis()), .1),
+            2);
+
+    DoubleSupplier rotationVelocity =
+        () -> -rotation.getAsDouble() * Drive.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * 0.8;
+
+    new Trigger(() -> Math.abs(rotation.getAsDouble()) > 0)
+        .whileTrue(
+            new RotateVelocityDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                rotationVelocity,
+                anthony.rightBumper()));
+
+    new Trigger(
+            () ->
+                Util.vectorMagnitude(anthony.getRightY(), anthony.getRightX())
+                    > Drive.ROTATE_VECTOR_MAGNITUDE)
+        .onTrue(
+            new RotateVectorDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                anthony::getRightY,
+                anthony::getRightX,
+                anthony.rightBumper()));
+  }
+
+  /**
+   * Adds all autonomous routines to the autoSelector, and places the autoSelector on Shuffleboard.
+   */
+  private void setupAutonomousCommands() {
+    driverView.addString("NOTES", () -> "...win?\nor not.").withSize(4, 1).withPosition(7, 2);
+
+    driverView.add("auto selector", autoSelector).withSize(4, 1).withPosition(7, 0);
+
+    autoDelay =
+        driverView
+            .add("auto delay", 0)
+            .withWidget(BuiltInWidgets.kNumberSlider)
+            .withProperties(Map.of("min", 0, "max", 15, "block increment", .1))
+            .withSize(4, 1)
+            .withPosition(7, 1)
+            .getEntry();
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    double delay = autoDelay.getDouble(0);
+    return delay == 0
+        ? autoSelector.getSelected()
+        : new WaitCommand(delay).andThen(autoSelector.getSelected());
+  }
+
+  /**
+   * applies deadband and squares axis
+   *
+   * @param value the axis value to be modified
+   * @return the modified axis values
+   */
+  private static double modifyAxis(double value) {
+    // Deadband
+    value = ControllerUtil.deadband(value, 0.07);
+
+    // Square the axis
+    value = Math.copySign(value * value, value);
+
+    return value;
+  }
+
+  private static DoubleSupplier exponential(DoubleSupplier supplier, double exponential) {
+    return () -> {
+      double val = supplier.getAsDouble();
+      return Math.copySign(Math.pow(val, exponential), val);
+    };
+  }
 }
diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
index 9e2a86a..b0bdeab 100644
--- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
+++ b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
@@ -17,7 +17,7 @@ public AdvancedIntakeCommand(
     addCommands(
         new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem),
         new ParallelCommandGroup(
-                (new StopIntakeCommand(intakeSubsystem)),
+            (new StopIntakeCommand(intakeSubsystem)),
             new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
   }
 }
diff --git a/src/main/java/frc/robot/commands/AngleCommand.java b/src/main/java/frc/robot/commands/AngleCommand.java
index 509d2b7..66984cf 100644
--- a/src/main/java/frc/robot/commands/AngleCommand.java
+++ b/src/main/java/frc/robot/commands/AngleCommand.java
@@ -6,48 +6,46 @@
 
 // import frc.robot.subsystems.ElevatorSubsystem;
 
-import frc.robot.subsystems.ElevatorSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
 
 /** An example command that uses an example subsystem. */
 public class AngleCommand extends Command {
 
-    ElevatorSubsystem elevatorSubsystem;
-
-    /**
-     * Creates a new ExampleCommand.
-     *
-     * @param subsystem The subsystem used by this command.
-     */
-    double angle;
-
-    public AngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) {
-        this.elevatorSubsystem = elevatorSubsystem;
-        this.angle = angle;
-        // Use addRequirements() here to declare subsystem dependencies.
-        addRequirements(elevatorSubsystem);
-    }
-
-    // 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() {
-
-        elevatorSubsystem.setTargetAngle(angle);
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return elevatorSubsystem.nearTargetAngle();
-    }
+  ElevatorSubsystem elevatorSubsystem;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double angle;
+
+  public AngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.angle = angle;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(elevatorSubsystem);
+  }
+
+  // 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() {
+
+    elevatorSubsystem.setTargetAngle(angle);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.nearTargetAngle();
+  }
 }
diff --git a/src/main/java/frc/robot/commands/HeightCommand.java b/src/main/java/frc/robot/commands/HeightCommand.java
index f2beb09..434ffe1 100644
--- a/src/main/java/frc/robot/commands/HeightCommand.java
+++ b/src/main/java/frc/robot/commands/HeightCommand.java
@@ -6,48 +6,46 @@
 
 // import frc.robot.subsystems.ElevatorSubsystem;
 
-import frc.robot.subsystems.ElevatorSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
 
 /** An example command that uses an example subsystem. */
 public class HeightCommand extends Command {
 
-    ElevatorSubsystem elevatorSubsystem;
-
-    /**
-     * Creates a new ExampleCommand.
-     *
-     * @param subsystem The subsystem used by this command.
-     */
-    double height;
-
-    public HeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
-        this.elevatorSubsystem = elevatorSubsystem;
-        this.height = height;
-        // Use addRequirements() here to declare subsystem dependencies.
-        addRequirements(elevatorSubsystem);
-    }
-
-    // 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() {
-
-        elevatorSubsystem.setTargetHeight(height);
-    }
-
-    // Called once the command ends or is interrupted.
-    @Override
-    public void end(boolean interrupted) {
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        return elevatorSubsystem.nearTargetHeight();
-    }
+  ElevatorSubsystem elevatorSubsystem;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  double height;
+
+  public HeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.height = height;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(elevatorSubsystem);
+  }
+
+  // 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() {
+
+    elevatorSubsystem.setTargetHeight(height);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.nearTargetHeight();
+  }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index a0e0bcc..ee4d0d0 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -25,7 +25,7 @@ public IntakeCommand(
     this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
-        addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
+    addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
     // Use addRequirements() here to declare subsystem dependencies.
     // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
   }
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index a5405d3..3a8a8a4 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -13,14 +13,18 @@
 public class LoadShooterCommand extends Command {
   /** Creates a new LoadShooterCommand. */
   ShooterSubsystem shooterSubsystem;
+
   ElevatorSubsystem elevatorSubsystem;
   PivotSubsystem pivotSubsystem;
-  public LoadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
+
+  public LoadShooterCommand(
+      ShooterSubsystem shooterSubsystem,
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
     this.shooterSubsystem = shooterSubsystem;
     this.elevatorSubsystem = elevatorSubsystem;
     this.pivotSubsystem = pivotSubsystem;
     addRequirements(elevatorSubsystem, shooterSubsystem, pivotSubsystem);
-
   }
 
   // Called when the command is initially scheduled.
diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java
index 528ccac..85ff0e5 100644
--- a/src/main/java/frc/robot/commands/RGBCommand.java
+++ b/src/main/java/frc/robot/commands/RGBCommand.java
@@ -11,8 +11,8 @@
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.RGBSubsystem;
 import frc.robot.subsystems.RGBSubsystem.RGBMessage;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 import frc.robot.subsystems.VisionSubsystem;
 import java.util.Optional;
 
@@ -102,23 +102,24 @@ public void execute() {
                   Constants.Lights.Colors.RED,
                   RGBSubsystem.PatternTypes.STROBE,
                   RGBSubsystem.MessagePriority.D_READY_TO_SHOOT));
-    } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER) || //if it shot the note, it is not ready to shoot instead of relying on beam break sensor input
-      shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)||
-      shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)||
-      pivotSubsystem.isAtTargetDegrees()){
-        readyToShootMsg.ifPresent(RGBMessage::expire);
-        readyToShootMsg = Optional.empty();
+    } else if (shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SPEAKER)
+        || // if it shot the note, it is not ready to shoot instead of relying on beam break sensor
+        // input
+        shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)
+        || shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)
+        || pivotSubsystem.isAtTargetDegrees()) {
+      readyToShootMsg.ifPresent(RGBMessage::expire);
+      readyToShootMsg = Optional.empty();
     }
   }
 
-  private boolean isReadyToShootInSun(){
-    if(shooterSubsystem.isReadyToShoot()){
-      sensorCounter +=1;
+  private boolean isReadyToShootInSun() {
+    if (shooterSubsystem.isReadyToShoot()) {
+      sensorCounter += 1;
     }
-    if (sensorCounter>=38){//waits 0.75 seconds to varify the note did not leave robot
+    if (sensorCounter >= 38) { // waits 0.75 seconds to varify the note did not leave robot
       return true;
-    }
-    else{
+    } else {
       sensorCounter = 0;
       return false;
     }
diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java
index e59b963..238d9e8 100644
--- a/src/main/java/frc/robot/commands/ShootCommand.java
+++ b/src/main/java/frc/robot/commands/ShootCommand.java
@@ -26,8 +26,7 @@ public void initialize() {
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index 5d21734..a4ec6e1 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -9,12 +9,16 @@
 public class TransferNoteCommand extends SequentialCommandGroup {
 
   public TransferNoteCommand(
-    ShooterSubsystem shooterSubsystem, IntakeSubsystem intakeSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
+      ShooterSubsystem shooterSubsystem,
+      IntakeSubsystem intakeSubsystem,
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) {
       addCommands(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-    }
-    else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()){
-      addCommands(new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem).andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+    } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) {
+      addCommands(
+          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
+              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
index c689b26..e8c18ec 100644
--- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -13,10 +13,15 @@
 public class UnloadShooterCommand extends Command {
   /** Creates a new UnloadShooterCommand. */
   ShooterSubsystem shooterSubsystem;
+
   PivotSubsystem pivotSubsystem;
   ElevatorSubsystem elevatorSubsystem;
   boolean pass;
-  public UnloadShooterCommand(ShooterSubsystem shooterSubsystem, PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
+
+  public UnloadShooterCommand(
+      ShooterSubsystem shooterSubsystem,
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
     this.elevatorSubsystem = elevatorSubsystem;
@@ -36,8 +41,8 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()&&pass==false){
-      pass=true;
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) {
+      pass = true;
     }
   }
 
@@ -50,6 +55,6 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return !shooterSubsystem.isSerializerBeamBreakSensorTriggered()&&pass==true;
+    return !shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == true;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index f6e90e4..23f8f2e 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -1,7 +1,6 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.TalonFX;
-
 import edu.wpi.first.math.MathUtil;
 // import com.ctre.phoenix6.signals.NeutralModeValue;
 // import edu.wpi.first.math.MathUtil;
@@ -15,102 +14,100 @@
 
 public class ElevatorSubsystem extends SubsystemBase {
 
-    private TalonFX armMotor;
-    private TalonFX elevatorMotor;
-    private PIDController controller;
-    private double targetHeight;
-    private double targetAngle;
-    private double elevatorMotorPower;
-    private double armMotorPower;
-    private double currentHeight;
-    private double currentAngle;
-    private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
-
-    /** Creates a new ElevatorSubsystem. */
-    public ElevatorSubsystem() {
-
-        elevatorMotor = new TalonFX(Elevator.Ports.ELEVATOR_MOTOR_PORT);
-        armMotor = new TalonFX(Elevator.Ports.ARM_MOTOR_PORT);
-
-        elevatorMotor.clearStickyFaults();
-        armMotor.clearStickyFaults();
-        elevatorMotor.setPosition(0);
-        armMotor.setPosition(0);
-
-
-        controller = new PIDController(0.4, 0, 0.0125);
-
-        ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
-        ElevatorTab.addNumber("Target Angle", () -> this.targetAngle);
-        // ElevatorTab.addNumber("PID output", () -> this.controller);
-        ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
-        ElevatorTab.add(controller);
-
-        targetHeight = 0;
-        currentHeight = 0;
-        targetAngle = 0;
-        currentAngle = 0;
-    }
-
-    public double getArmPosition(){
-        return armInchesToRotations(armMotor.getPosition().getValueAsDouble());
-    }
-
-    public double getElevatorPosition(){
-        return inchesToRotations(elevatorMotor.getPosition().getValueAsDouble());
-    }
-
-    public static double inchesToRotations(double height) {
-        return height / Elevator.ELEVATOR_GEAR_RATIO;
-    }
-
-    public static double rotationsToInches(double rotations) {
-        return rotations * Elevator.ELEVATOR_GEAR_RATIO;
-    }
-
-    public static double armInchesToRotations(double degrees) {
-        return degrees / Elevator.Arm.ARM_GEAR_RATIO;
-    }
-
-    public static double rotationsToArmInches(double rotations) {
-        return rotations * Elevator.Arm.ARM_GEAR_RATIO;
+  private TalonFX armMotor;
+  private TalonFX elevatorMotor;
+  private PIDController controller;
+  private double targetHeight;
+  private double targetAngle;
+  private double elevatorMotorPower;
+  private double armMotorPower;
+  private double currentHeight;
+  private double currentAngle;
+  private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
+
+  /** Creates a new ElevatorSubsystem. */
+  public ElevatorSubsystem() {
+
+    elevatorMotor = new TalonFX(Elevator.Ports.ELEVATOR_MOTOR_PORT);
+    armMotor = new TalonFX(Elevator.Ports.ARM_MOTOR_PORT);
+
+    elevatorMotor.clearStickyFaults();
+    armMotor.clearStickyFaults();
+    elevatorMotor.setPosition(0);
+    armMotor.setPosition(0);
+
+    controller = new PIDController(0.4, 0, 0.0125);
+
+    ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+    ElevatorTab.addNumber("Target Angle", () -> this.targetAngle);
+    // ElevatorTab.addNumber("PID output", () -> this.controller);
+    ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
+    ElevatorTab.add(controller);
+
+    targetHeight = 0;
+    currentHeight = 0;
+    targetAngle = 0;
+    currentAngle = 0;
+  }
+
+  public double getArmPosition() {
+    return armInchesToRotations(armMotor.getPosition().getValueAsDouble());
+  }
+
+  public double getElevatorPosition() {
+    return inchesToRotations(elevatorMotor.getPosition().getValueAsDouble());
+  }
+
+  public static double inchesToRotations(double height) {
+    return height / Elevator.ELEVATOR_GEAR_RATIO;
+  }
+
+  public static double rotationsToInches(double rotations) {
+    return rotations * Elevator.ELEVATOR_GEAR_RATIO;
+  }
+
+  public static double armInchesToRotations(double degrees) {
+    return degrees / Elevator.Arm.ARM_GEAR_RATIO;
+  }
+
+  public static double rotationsToArmInches(double rotations) {
+    return rotations * Elevator.Arm.ARM_GEAR_RATIO;
+  }
+
+  public void setTargetHeight(double targetHeight) {
+    this.targetHeight = MathUtil.clamp(targetHeight, Elevator.MAX_HEIGHT, 0);
+  }
+
+  public void setTargetAngle(double targetAngle) {
+    this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0);
+  }
+
+  public boolean nearTargetAngle() {
+    if (targetAngle - 0.5 <= getArmPosition() && getArmPosition() <= targetAngle + 0.5) {
+      return true;
     }
+    return false;
+  }
 
-    public void setTargetHeight(double targetHeight) {
-        this.targetHeight = MathUtil.clamp(targetHeight, Elevator.MAX_HEIGHT, 0);
-
-    }
-
-    public void setTargetAngle(double targetAngle) {
-        this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0);
+  public boolean nearTargetHeight() {
+    if (targetHeight - 0.5 <= getElevatorPosition()
+        && getElevatorPosition() <= targetHeight + 0.5) {
+      return true;
     }
-
-    public boolean nearTargetAngle() {
-        if (targetAngle - 0.5 <= getArmPosition() && getArmPosition() <= targetAngle + 0.5) {
-            return true;
-        }
-        return false;
-    }
-
-
-    public boolean nearTargetHeight() {
-        if (targetHeight - 0.5 <= getElevatorPosition() && getElevatorPosition() <= targetHeight + 0.5) {
-            return true;
-        }
-        return false;
-    }
-
-    public double calculateFeedforward(){
-        return 0; //FIXME
-    }
-
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
-        elevatorMotorPower = controller.calculate(getElevatorPosition(), targetHeight);
-        armMotorPower = controller.calculate(getArmPosition(), targetAngle);
-        elevatorMotor.setVoltage(MathUtil.clamp(elevatorMotorPower+Elevator.ELEVATOR_FEEDFORWARD, -10, 10));
-        armMotor.setVoltage(MathUtil.clamp(armMotorPower+calculateFeedforward(), -10, 10));
-    }
-
+    return false;
+  }
+
+  public double calculateFeedforward() {
+    return 0; // FIXME
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    elevatorMotorPower = controller.calculate(getElevatorPosition(), targetHeight);
+    armMotorPower = controller.calculate(getArmPosition(), targetAngle);
+    elevatorMotor.setVoltage(
+        MathUtil.clamp(elevatorMotorPower + Elevator.ELEVATOR_FEEDFORWARD, -10, 10));
+    armMotor.setVoltage(MathUtil.clamp(armMotorPower + calculateFeedforward(), -10, 10));
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index af35d82..214ed71 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -58,7 +58,6 @@ public IntakeSubsystem() {
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble());
       tab.addString("Current Mode", () -> intakeMode.toString());
-
     }
   }
 
@@ -66,8 +65,6 @@ public void setIntakeMode(IntakeMode intakeMode) {
     this.intakeMode = intakeMode;
   }
 
-
-
   private IntakeMode getIntakeMode() {
     return intakeMode;
   }
diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index 493de72..f727946 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -120,7 +120,7 @@ public void setTargetDegrees(double degrees) {
   }
 
   private static double rotationsToDegrees(double rotations) {
-    return rotations*Pivot.PIVOT_GEAR_RATIO;
+    return rotations * Pivot.PIVOT_GEAR_RATIO;
   }
 
   public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) {
@@ -156,11 +156,10 @@ public double getAngularError() {
     return targetDegrees - getCurrentAngle();
   }
 
-  public void setPower(double power){
+  public void setPower(double power) {
     this.power = power;
   }
 
-
   @Override
   public void periodic() {
 
@@ -178,6 +177,6 @@ public void periodic() {
 
     pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
 
-    pivotMotor.set(getFeedForward()+power);
+    pivotMotor.set(getFeedForward() + power);
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index d19f6c3..66ab7e9 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -28,7 +28,6 @@ public class ShooterSubsystem extends SubsystemBase {
   private DigitalInput shooterSensor;
   private DigitalInput serializerSensor;
 
-
   private ShooterMode shooterMode;
 
   // DEBUG
@@ -68,8 +67,10 @@ private ShooterMode(ShooterPowers shooterPowers) {
     }
   }
 
-  public record ShooterPowers(double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
-    public ShooterPowers(double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
+  public record ShooterPowers(
+      double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
+    public ShooterPowers(
+        double roller, double topToBottomRatio, double accelerator, double serializerSpeed) {
       this.roller = roller;
       this.topToBottomRatio = topToBottomRatio;
       this.accelerator = accelerator;
@@ -86,7 +87,6 @@ public ShooterSubsystem() {
     // shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
     // serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
 
-
     // rollerMotorTop.getConfigurator().apply(new TalonFXConfiguration());
     // rollerMotorBottom.getConfigurator().apply(new TalonFXConfiguration());
     // acceleratorMotor.getConfigurator().apply(new TalonFXConfiguration());
@@ -131,23 +131,26 @@ public ShooterSubsystem() {
           "Bottom roller amps", () -> rollerMotorBottom.getSupplyCurrent().getValueAsDouble());
       shooterTab.addString("mode", () -> shooterMode.toString());
 
-      ampRollerRatioEntry = shooterTab
-          .add("DEBUG Amp Top to Bottom Roller Ratio", 1)
-          .withWidget(BuiltInWidgets.kNumberSlider)
-          .withProperties(Map.of("min", 0, "max", 1))
-          .withSize(3, 1)
-          .getEntry();
-      shooterSpeedEntry = shooterTab
-          .add("DEBUG Shooter Velocity", .5)
-          .withWidget(BuiltInWidgets.kNumberSlider)
-          .withProperties(Map.of("min", 0, "max", 90))
-          .withSize(3, 1)
-          .getEntry();
-      useDebugControls = shooterTab
-          .add("Use Debug Controls", false)
-          .withWidget(BuiltInWidgets.kToggleSwitch)
-          .withSize(2, 1)
-          .getEntry();
+      ampRollerRatioEntry =
+          shooterTab
+              .add("DEBUG Amp Top to Bottom Roller Ratio", 1)
+              .withWidget(BuiltInWidgets.kNumberSlider)
+              .withProperties(Map.of("min", 0, "max", 1))
+              .withSize(3, 1)
+              .getEntry();
+      shooterSpeedEntry =
+          shooterTab
+              .add("DEBUG Shooter Velocity", .5)
+              .withWidget(BuiltInWidgets.kNumberSlider)
+              .withProperties(Map.of("min", 0, "max", 90))
+              .withSize(3, 1)
+              .getEntry();
+      useDebugControls =
+          shooterTab
+              .add("Use Debug Controls", false)
+              .withWidget(BuiltInWidgets.kToggleSwitch)
+              .withSize(2, 1)
+              .getEntry();
     }
   }
 
@@ -182,8 +185,7 @@ public void setShooterMode(ShooterMode shooterMode) {
   }
 
   public void setVariableVelocity(double velocity) {
-    if (shooterMode != ShooterMode.VARIABLE_VELOCITY)
-      shooterMode = ShooterMode.VARIABLE_VELOCITY;
+    if (shooterMode != ShooterMode.VARIABLE_VELOCITY) shooterMode = ShooterMode.VARIABLE_VELOCITY;
     this.variableVelocity = velocity;
   }
 

From 516fa1cc9998cfb8f745d2acdaca28f3bd0052ae Mon Sep 17 00:00:00 2001
From: audrey <audreypjohnson@gmail.com>
Date: Tue, 17 Sep 2024 19:21:32 -0700
Subject: [PATCH 19/29] Intake subsystem/command edits

---
 src/main/java/frc/robot/Constants.java        |  8 +++----
 src/main/java/frc/robot/RobotContainer.java   | 13 +++++------
 .../robot/commands/AdvancedIntakeCommand.java | 23 -------------------
 .../frc/robot/commands/IntakeCommand.java     | 10 +++++---
 .../robot/commands/TransferNoteCommand.java   |  2 +-
 .../frc/robot/subsystems/IntakeSubsystem.java | 16 ++++---------
 6 files changed, 21 insertions(+), 51 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/AdvancedIntakeCommand.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1a76881..d3b0e0a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -242,11 +242,9 @@ public static final class Ports {
     }
 
     public static final class Modes {
-      public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
-      public static final IntakePowers HOLD = new IntakePowers(0, 0d);
-      public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-      public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 0.7);
-      public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1);
+      public static final IntakePowers INTAKE = new IntakePowers(.95);
+      public static final IntakePowers HOLD = new IntakePowers(0);
+      public static final IntakePowers REVERSE = new IntakePowers(-.5);
     }
 
     public static final boolean IS_BEAMBREAK = true;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a96ef14..cbd3d9e 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -33,12 +33,11 @@
 import frc.robot.autonomous.HeadingAngle;
 import frc.robot.autonomous.HeadingTargetLock;
 import frc.robot.commands.AccelNoteCommand;
-import frc.robot.commands.AdvancedIntakeCommand;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.HeightCommand;
-import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.MaintainShooterCommand;
 import frc.robot.commands.OuttakeCommand;
 import frc.robot.commands.PivotAngleCommand;
@@ -131,7 +130,7 @@ public RobotContainer() {
 
     // reigster commands for pathplanner
     NamedCommands.registerCommand(
-        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
     NamedCommands.registerCommand(
         "ShooterRampUpCommand",
@@ -347,11 +346,11 @@ private void configureButtonBindings() {
     // INTAKE
     anthony
         .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     jacob
         .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     // SHOOT
     anthony
@@ -360,7 +359,7 @@ private void configureButtonBindings() {
             new AccelNoteCommand(shooterSubsystem)
                 .andThen(new ShootCommand(shooterSubsystem))
                 .andThen(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
     jacob
@@ -419,7 +418,7 @@ private void configureButtonBindings() {
                         ? -Setpoints.SOURCE_DEGREES
                         : Setpoints.SOURCE_DEGREES)
                 .alongWith(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
     anthony
diff --git a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java b/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
deleted file mode 100644
index b0bdeab..0000000
--- a/src/main/java/frc/robot/commands/AdvancedIntakeCommand.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.PivotSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
-public class AdvancedIntakeCommand extends SequentialCommandGroup {
-
-  public AdvancedIntakeCommand(
-      IntakeSubsystem intakeSubsystem,
-      ShooterSubsystem shooterSubsystem,
-      PivotSubsystem pivotSubsystem) {
-    addCommands(
-        new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem),
-        new ParallelCommandGroup(
-            (new StopIntakeCommand(intakeSubsystem)),
-            new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
-  }
-}
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index ee4d0d0..744005b 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 import frc.robot.subsystems.PivotSubsystem;
@@ -16,16 +17,19 @@ public class IntakeCommand extends Command {
   IntakeSubsystem intakeSubsystem;
   ShooterSubsystem shooterSubsystem;
   PivotSubsystem pivotSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
 
   /** Creates a new IntakeCommand. */
   public IntakeCommand(
       IntakeSubsystem intakeSubsystem,
       ShooterSubsystem shooterSubsystem,
-      PivotSubsystem pivotSubsystem) {
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
     this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
-    addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem);
     // Use addRequirements() here to declare subsystem dependencies.
     // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
   }
@@ -36,6 +40,7 @@ public void initialize() {
     pivotSubsystem.setTargetDegrees(20);
     intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
     shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
+    elevatorSubsystem.setTargetHeight(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -47,7 +52,6 @@ public void execute() {}
   public void end(boolean interrupted) {
     intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD);
     shooterSubsystem.setShooterMode(ShooterMode.IDLE);
-    shooterSubsystem.haltAccelerator();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index a4ec6e1..a12d7d9 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -18,7 +18,7 @@ public TransferNoteCommand(
     } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) {
       addCommands(
           new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
-              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
index 214ed71..059c142 100644
--- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -17,28 +17,22 @@ public class IntakeSubsystem extends SubsystemBase {
   private final TalonFX intakeMotor;
   private final ShuffleboardTab tab = Shuffleboard.getTab("Intake");
   private IntakeMode intakeMode;
-  private IntakeMode pastMode;
-  private double timeSincePenaltyHazard;
-  private boolean pastPenalty;
 
   public enum IntakeMode {
     INTAKE(Intake.Modes.INTAKE),
     HOLD(Intake.Modes.HOLD),
-    REVERSE(Intake.Modes.REVERSE),
-    SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER),
-    SHOOT_AMP(Intake.Modes.SHOOT_AMP);
+    REVERSE(Intake.Modes.REVERSE);
 
-    public final IntakePowers modePowers;
+    public final IntakePowers modePowers; // rename to modePower
 
     private IntakeMode(IntakePowers modePowers) {
       this.modePowers = modePowers;
     }
   }
 
-  public record IntakePowers(double intakeSpeed, double serializerSpeed) {
-    public IntakePowers(double intakeSpeed, double serializerSpeed) {
+  public record IntakePowers(double intakeSpeed) {
+    public IntakePowers(double intakeSpeed) {
       this.intakeSpeed = intakeSpeed;
-      this.serializerSpeed = serializerSpeed;
     }
   }
 
@@ -53,8 +47,6 @@ public IntakeSubsystem() {
 
     intakeMode = IntakeMode.HOLD;
 
-    timeSincePenaltyHazard = 7;
-
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble());
       tab.addString("Current Mode", () -> intakeMode.toString());

From 6a6f4e91f258003643a453bbf1263c1ab0b6471b Mon Sep 17 00:00:00 2001
From: audrey <audreypjohnson@gmail.com>
Date: Tue, 17 Sep 2024 19:48:59 -0700
Subject: [PATCH 20/29] shooter subsystem edits

---
 src/main/java/frc/robot/Constants.java        | 10 +--
 src/main/java/frc/robot/RobotContainer.java   | 22 +++--
 .../frc/robot/commands/AccelNoteCommand.java  |  2 +-
 .../commands/MaintainShooterCommand.java      | 23 -----
 .../java/frc/robot/commands/RGBCommand.java   |  2 +-
 .../robot/commands/TransferNoteCommand.java   |  4 +-
 .../robot/subsystems/ShooterSubsystem.java    | 89 +++++++++----------
 7 files changed, 62 insertions(+), 90 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/MaintainShooterCommand.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d3b0e0a..6d6b75a 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -260,7 +260,7 @@ public static final class Ports {
 
     public static final class Modes {
       public static final ShooterSubsystem.ShooterPowers INTAKE =
-          new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
+          new ShooterSubsystem.ShooterPowers(0, 1, 0, 0.5);
       public static final ShooterSubsystem.ShooterPowers IDLE =
           new ShooterSubsystem.ShooterPowers(0, 0, 0, 0);
       public static final ShooterSubsystem.ShooterPowers RAMP_AMP =
@@ -269,13 +269,9 @@ public static final class Modes {
           new ShooterSubsystem.ShooterPowers(76, 1, 0, 0);
       public static final ShooterSubsystem.ShooterPowers SHOOT_SPEAKER =
           new ShooterSubsystem.ShooterPowers(76, 1, .5, 0);
-      public static final ShooterSubsystem.ShooterPowers TARGET_LOCK =
-          new ShooterSubsystem.ShooterPowers(0, 1, 0, 0);
       public static final ShooterSubsystem.ShooterPowers SHOOT_AMP =
           new ShooterSubsystem.ShooterPowers(0, 0, 0, -1);
-      public static final ShooterSubsystem.ShooterPowers MAINTAIN_VELOCITY =
-          new ShooterSubsystem.ShooterPowers(40, 1, 0, 0);
-      public static final ShooterSubsystem.ShooterPowers SHUTTLE =
+      public static final ShooterSubsystem.ShooterPowers RAMP_SHUTTLE =
           new ShooterSubsystem.ShooterPowers(30, 1, 0, 0);
       public static final ShooterSubsystem.ShooterPowers SHOOT_SHUTTLE =
           new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
@@ -287,7 +283,7 @@ public static final class Modes {
           new ShooterSubsystem.ShooterPowers(30, 1, 0.5, 0);
       public static final ShooterSubsystem.ShooterPowers LOAD_SHOOTER =
           new ShooterSubsystem.ShooterPowers(0, 0, 0.5, 0.5);
-      public static final ShooterSubsystem.ShooterPowers SHOOTER_UNLOAD =
+      public static final ShooterSubsystem.ShooterPowers UNLOAD_SHOOTER =
           new ShooterSubsystem.ShooterPowers(0, 0, -0.5, -0.5);
     }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cbd3d9e..cf9647c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -33,12 +33,11 @@
 import frc.robot.autonomous.HeadingAngle;
 import frc.robot.autonomous.HeadingTargetLock;
 import frc.robot.commands.AccelNoteCommand;
-import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.HeightCommand;
-import frc.robot.commands.MaintainShooterCommand;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.OuttakeCommand;
 import frc.robot.commands.PivotAngleCommand;
 import frc.robot.commands.PivotManualCommand;
@@ -130,7 +129,8 @@ public RobotContainer() {
 
     // reigster commands for pathplanner
     NamedCommands.registerCommand(
-        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+        "IntakeCommand",
+        new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
     NamedCommands.registerCommand(
         "ShooterRampUpCommand",
@@ -148,8 +148,6 @@ public RobotContainer() {
         "Heading4Note2", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
     NamedCommands.registerCommand(
         "AngleDriveBase", new RotateAngleDriveCommand(drivebaseSubsystem, () -> 0, () -> 0, -17));
-    NamedCommands.registerCommand(
-        "MaintainShooterVelocity", new MaintainShooterCommand(shooterSubsystem));
     NamedCommands.registerCommand("HeadingLock", new HeadingTargetLock(drivebaseSubsystem));
     NamedCommands.registerCommand("LockForward", new HeadingAngle(drivebaseSubsystem, 0));
     NamedCommands.registerCommand(
@@ -346,11 +344,15 @@ private void configureButtonBindings() {
     // INTAKE
     anthony
         .leftBumper()
-        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+        .onTrue(
+            new IntakeCommand(
+                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     jacob
         .leftBumper()
-        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+        .onTrue(
+            new IntakeCommand(
+                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     // SHOOT
     anthony
@@ -359,7 +361,8 @@ private void configureButtonBindings() {
             new AccelNoteCommand(shooterSubsystem)
                 .andThen(new ShootCommand(shooterSubsystem))
                 .andThen(
-                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
     jacob
@@ -418,7 +421,8 @@ private void configureButtonBindings() {
                         ? -Setpoints.SOURCE_DEGREES
                         : Setpoints.SOURCE_DEGREES)
                 .alongWith(
-                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
     anthony
diff --git a/src/main/java/frc/robot/commands/AccelNoteCommand.java b/src/main/java/frc/robot/commands/AccelNoteCommand.java
index 265dc61..86b95c6 100644
--- a/src/main/java/frc/robot/commands/AccelNoteCommand.java
+++ b/src/main/java/frc/robot/commands/AccelNoteCommand.java
@@ -23,6 +23,6 @@ public void end(boolean interrupted) {
 
   @Override
   public boolean isFinished() {
-    return shooterSubsystem.isReadyToShoot();
+    return shooterSubsystem.isReadyToShootSpeaker();
   }
 }
diff --git a/src/main/java/frc/robot/commands/MaintainShooterCommand.java b/src/main/java/frc/robot/commands/MaintainShooterCommand.java
deleted file mode 100644
index c042b44..0000000
--- a/src/main/java/frc/robot/commands/MaintainShooterCommand.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-public class MaintainShooterCommand extends Command {
-  ShooterSubsystem shooterSubsystem;
-
-  public MaintainShooterCommand(ShooterSubsystem shooterSubsystem) {
-    this.shooterSubsystem = shooterSubsystem;
-  }
-
-  @Override
-  public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.MAINTAIN_VELOCITY);
-  }
-
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java
index 85ff0e5..26fe4bd 100644
--- a/src/main/java/frc/robot/commands/RGBCommand.java
+++ b/src/main/java/frc/robot/commands/RGBCommand.java
@@ -114,7 +114,7 @@ public void execute() {
   }
 
   private boolean isReadyToShootInSun() {
-    if (shooterSubsystem.isReadyToShoot()) {
+    if (shooterSubsystem.isReadyToShootSpeaker()) {
       sensorCounter += 1;
     }
     if (sensorCounter >= 38) { // waits 0.75 seconds to varify the note did not leave robot
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
index a12d7d9..f5a1a00 100644
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ b/src/main/java/frc/robot/commands/TransferNoteCommand.java
@@ -18,7 +18,9 @@ public TransferNoteCommand(
     } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) {
       addCommands(
           new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
-              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+              .andThen(
+                  new IntakeCommand(
+                      intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
index 66ab7e9..d70bec0 100644
--- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -20,10 +20,10 @@
 import java.util.Map;
 
 public class ShooterSubsystem extends SubsystemBase {
-  private final TalonFX rollerMotorBottom;
-  private final TalonFX rollerMotorTop;
-  private final TalonFX acceleratorMotor;
-  private final TalonFX serializerMotor;
+  private final TalonFX pivotShooterTopMotor; // pivotShooterTop
+  private final TalonFX pivotShooterBottomMotor; // pivotShooterBottom
+  private final TalonFX pivotAcceleratorMotor; // pivotAccelerator
+  private final TalonFX serializerMotor; // ampShooter
 
   private DigitalInput shooterSensor;
   private DigitalInput serializerSensor;
@@ -50,14 +50,13 @@ public enum ShooterMode {
     RAMP_SPEAKER(Shooter.Modes.RAMP_SPEAKER),
     SHOOT_SPEAKER(Shooter.Modes.SHOOT_SPEAKER),
     SHOOT_AMP(Shooter.Modes.SHOOT_AMP),
-    MAINTAIN_VELOCITY(Shooter.Modes.MAINTAIN_VELOCITY),
-    SHUTTLE(Shooter.Modes.SHUTTLE),
+    SHUTTLE(Shooter.Modes.RAMP_SHUTTLE),
     SHOOT_SHUTTLE(Shooter.Modes.SHOOT_SHUTTLE),
     ACCEL_SECURE(Shooter.Modes.ACCEL_SECURE),
     VARIABLE_VELOCITY(Shooter.Modes.VARIABLE_VELOCITY),
     SHOOT_VAR(Shooter.Modes.SHOOT_VAR),
     LOAD_SHOOTER(Shooter.Modes.LOAD_SHOOTER),
-    SHOOTER_UNLOAD(Shooter.Modes.SHOOTER_UNLOAD),
+    SHOOTER_UNLOAD(Shooter.Modes.UNLOAD_SHOOTER),
     RAMP_AMP(Shooter.Modes.RAMP_AMP);
 
     public final ShooterPowers shooterPowers;
@@ -79,34 +78,30 @@ public ShooterPowers(
   }
 
   public ShooterSubsystem() {
-    rollerMotorTop = new TalonFX(Shooter.Ports.TOP_SHOOTER_MOTOR_PORT);
-    rollerMotorBottom = new TalonFX(Shooter.Ports.BOTTOM_SHOOTER_MOTOR_PORT);
-    acceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT);
+    pivotShooterBottomMotor = new TalonFX(Shooter.Ports.TOP_SHOOTER_MOTOR_PORT);
+    pivotShooterTopMotor = new TalonFX(Shooter.Ports.BOTTOM_SHOOTER_MOTOR_PORT);
+    pivotAcceleratorMotor = new TalonFX(Shooter.Ports.ACCELERATOR_MOTOR_PORT);
     serializerMotor = new TalonFX(Intake.Ports.SERIALIZER_MOTOR_PORT);
 
-    // shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
-    // serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    shooterSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
+    serializerSensor = new DigitalInput(Shooter.Ports.BEAM_BREAK_SENSOR_PORT);
 
-    // rollerMotorTop.getConfigurator().apply(new TalonFXConfiguration());
-    // rollerMotorBottom.getConfigurator().apply(new TalonFXConfiguration());
-    // acceleratorMotor.getConfigurator().apply(new TalonFXConfiguration());
-
-    rollerMotorTop.clearStickyFaults();
-    acceleratorMotor.clearStickyFaults();
-    rollerMotorBottom.clearStickyFaults();
+    pivotShooterBottomMotor.clearStickyFaults();
+    pivotAcceleratorMotor.clearStickyFaults();
+    pivotShooterTopMotor.clearStickyFaults();
     serializerMotor.clearStickyFaults();
 
-    rollerMotorBottom.setControl(new Follower(rollerMotorTop.getDeviceID(), false));
+    pivotShooterTopMotor.setControl(new Follower(pivotShooterBottomMotor.getDeviceID(), false));
 
-    acceleratorMotor.setInverted(true);
-    rollerMotorBottom.setInverted(true);
-    rollerMotorTop.setInverted(true);
+    pivotAcceleratorMotor.setInverted(true);
+    pivotShooterTopMotor.setInverted(true);
+    pivotShooterBottomMotor.setInverted(true);
     serializerMotor.setInverted(true);
 
     serializerMotor.setNeutralMode(NeutralModeValue.Brake);
-    acceleratorMotor.setNeutralMode(NeutralModeValue.Brake);
-    rollerMotorTop.setNeutralMode(NeutralModeValue.Coast);
-    rollerMotorBottom.setNeutralMode(NeutralModeValue.Coast);
+    pivotAcceleratorMotor.setNeutralMode(NeutralModeValue.Brake);
+    pivotShooterBottomMotor.setNeutralMode(NeutralModeValue.Coast);
+    pivotShooterTopMotor.setNeutralMode(NeutralModeValue.Coast);
 
     shooterMode = ShooterMode.IDLE;
 
@@ -114,7 +109,8 @@ public ShooterSubsystem() {
       shooterTab.addBoolean("Sensor Input", this::isShooterBeamBreakSensorTriggered);
       shooterTab
           .addDouble(
-              "Top Roller Velocity (RPS)", () -> rollerMotorTop.getVelocity().getValueAsDouble())
+              "Top Roller Velocity (RPS)",
+              () -> pivotShooterBottomMotor.getVelocity().getValueAsDouble())
           .withWidget(BuiltInWidgets.kGraph)
           .withSize(2, 1);
       shooterTab.addDouble(
@@ -122,13 +118,13 @@ public ShooterSubsystem() {
       shooterTab
           .addDouble(
               "Bottom Roller Velocity (RPS)",
-              () -> rollerMotorBottom.getVelocity().getValueAsDouble())
+              () -> pivotShooterTopMotor.getVelocity().getValueAsDouble())
           .withWidget(BuiltInWidgets.kGraph)
           .withSize(2, 1);
       shooterTab.addDouble(
-          "Top roller amps", () -> rollerMotorTop.getSupplyCurrent().getValueAsDouble());
+          "Top roller amps", () -> pivotShooterBottomMotor.getSupplyCurrent().getValueAsDouble());
       shooterTab.addDouble(
-          "Bottom roller amps", () -> rollerMotorBottom.getSupplyCurrent().getValueAsDouble());
+          "Bottom roller amps", () -> pivotShooterTopMotor.getSupplyCurrent().getValueAsDouble());
       shooterTab.addString("mode", () -> shooterMode.toString());
 
       ampRollerRatioEntry =
@@ -159,27 +155,25 @@ public ShooterMode getMode() {
   }
 
   public boolean isShooterUpToSpeed() {
-    return rollerMotorBottom.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD
-        && rollerMotorTop.getVelocity().getValueAsDouble() >= Shooter.SHOOTER_VELOCITY_THRESHOLD;
+    return pivotShooterTopMotor.getVelocity().getValueAsDouble()
+            >= Shooter.SHOOTER_VELOCITY_THRESHOLD
+        && pivotShooterBottomMotor.getVelocity().getValueAsDouble()
+            >= Shooter.SHOOTER_VELOCITY_THRESHOLD;
   }
 
   public boolean isShooterBeamBreakSensorTriggered() {
-    return true;
+    return shooterSensor.get();
   }
 
   public boolean isSerializerBeamBreakSensorTriggered() {
     // if is triggered return true
-    return true;
+    return serializerSensor.get();
   }
 
-  public boolean isReadyToShoot() {
+  public boolean isReadyToShootSpeaker() {
     return isShooterBeamBreakSensorTriggered() && isShooterUpToSpeed();
   }
 
-  public void haltAccelerator() {
-    acceleratorMotor.set(0);
-  }
-
   public void setShooterMode(ShooterMode shooterMode) {
     this.shooterMode = shooterMode;
   }
@@ -211,23 +205,22 @@ public void periodic() {
       // In debug use all of the debug values for our mode
       d_topToBottomRatio = ampRollerRatioEntry.getDouble(1);
       d_ShooterSpeed = shooterSpeedEntry.getDouble(0);
-      rollerMotorBottom.setControl(velocityVoltageRequest.withVelocity(d_ShooterSpeed));
-      rollerMotorTop.setControl(
+      pivotShooterTopMotor.setControl(velocityVoltageRequest.withVelocity(d_ShooterSpeed));
+      pivotShooterBottomMotor.setControl(
           velocityVoltageRequest.withVelocity(d_ShooterSpeed * d_topToBottomRatio));
     } else if (shooterMode == ShooterMode.VARIABLE_VELOCITY
         || shooterMode == ShooterMode.SHOOT_VAR) {
-      rollerMotorBottom.setControl(velocityVoltageRequest.withVelocity(variableVelocity));
-      rollerMotorTop.setControl(velocityVoltageRequest.withVelocity(variableVelocity));
+      pivotShooterTopMotor.setControl(velocityVoltageRequest.withVelocity(variableVelocity));
+      pivotShooterBottomMotor.setControl(velocityVoltageRequest.withVelocity(variableVelocity));
     } else {
       // Otherwise just use our current mode for the values
-      rollerMotorBottom.setControl(
+      pivotShooterTopMotor.setControl(
           velocityVoltageRequest.withVelocity(shooterMode.shooterPowers.roller()));
-      rollerMotorTop.setControl(
+      pivotShooterBottomMotor.setControl(
           velocityVoltageRequest.withVelocity(
               shooterMode.shooterPowers.roller() * shooterMode.shooterPowers.topToBottomRatio()));
     }
-
-    acceleratorMotor.set(0);
-    serializerMotor.set(1);
+    pivotAcceleratorMotor.set(shooterMode.shooterPowers.accelerator());
+    serializerMotor.set(shooterMode.shooterPowers.serializerSpeed());
   }
 }

From f271cba610f81611b4c0050cb45910efd7ad36ba Mon Sep 17 00:00:00 2001
From: audrey <audreypjohnson@gmail.com>
Date: Tue, 17 Sep 2024 20:07:57 -0700
Subject: [PATCH 21/29] clearing unnecessary commands

---
 src/main/java/frc/robot/RobotContainer.java   | 29 ++------
 ...AngleCommand.java => ArmAngleCommand.java} |  4 +-
 .../frc/robot/commands/AutoAlignCommand.java  | 40 -----------
 ...ommand.java => ElevatorHeightCommand.java} |  4 +-
 .../frc/robot/commands/HashMapCommand.java    | 70 -------------------
 .../robot/commands/LoadShooterCommand.java    |  2 +
 .../robot/commands/SetRampModeCommand.java    | 40 -----------
 .../frc/robot/commands/ShootAmpCommand.java   | 40 -----------
 .../robot/commands/ShooterRampUpCommand.java  | 42 -----------
 .../robot/commands/SpeakerLockCommand.java    |  5 +-
 .../robot/commands/TransferNoteCommand.java   | 26 -------
 11 files changed, 15 insertions(+), 287 deletions(-)
 rename src/main/java/frc/robot/commands/{AngleCommand.java => ArmAngleCommand.java} (91%)
 delete mode 100644 src/main/java/frc/robot/commands/AutoAlignCommand.java
 rename src/main/java/frc/robot/commands/{HeightCommand.java => ElevatorHeightCommand.java} (90%)
 delete mode 100644 src/main/java/frc/robot/commands/HashMapCommand.java
 delete mode 100644 src/main/java/frc/robot/commands/SetRampModeCommand.java
 delete mode 100644 src/main/java/frc/robot/commands/ShootAmpCommand.java
 delete mode 100644 src/main/java/frc/robot/commands/ShooterRampUpCommand.java
 delete mode 100644 src/main/java/frc/robot/commands/TransferNoteCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cf9647c..f86c6d2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -35,9 +35,10 @@
 import frc.robot.commands.AccelNoteCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
+import frc.robot.commands.ElevatorHeightCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
-import frc.robot.commands.HeightCommand;
 import frc.robot.commands.IntakeCommand;
+import frc.robot.commands.LoadShooterCommand;
 import frc.robot.commands.OuttakeCommand;
 import frc.robot.commands.PivotAngleCommand;
 import frc.robot.commands.PivotManualCommand;
@@ -46,13 +47,10 @@
 import frc.robot.commands.RotateAngleDriveCommand;
 import frc.robot.commands.RotateVectorDriveCommand;
 import frc.robot.commands.RotateVelocityDriveCommand;
-import frc.robot.commands.SetRampModeCommand;
 import frc.robot.commands.ShootCommand;
-import frc.robot.commands.ShooterRampUpCommand;
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
 import frc.robot.commands.TargetLockCommand;
-import frc.robot.commands.TransferNoteCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -62,7 +60,6 @@
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.RGBSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
 import frc.robot.subsystems.VisionSubsystem;
 import frc.util.ControllerUtil;
 import frc.util.Layer;
@@ -132,10 +129,6 @@ public RobotContainer() {
         "IntakeCommand",
         new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
-    NamedCommands.registerCommand(
-        "ShooterRampUpCommand",
-        new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
-    NamedCommands.registerCommand("SetShooterToRamping", new SetRampModeCommand(shooterSubsystem));
     NamedCommands.registerCommand("AngleAtSpeaker", new PivotAngleCommand(pivotSubsystem, 55));
     NamedCommands.registerCommand("AngleAt1", new PivotAngleCommand(pivotSubsystem, 38));
     NamedCommands.registerCommand("AngleAt2", new PivotAngleCommand(pivotSubsystem, 40));
@@ -389,12 +382,6 @@ private void configureButtonBindings() {
     //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem,
     // ShooterMode.SHUTTLE)));
 
-    jacob
-        .povDown()
-        .onTrue(
-            new PivotAngleCommand(pivotSubsystem, 15)
-                .alongWith(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER)));
-
     // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
     // translationXSupplier,
     // translationYSupplier, Setpoints.SPEAKER));
@@ -425,13 +412,9 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony
-        .b()
-        .onTrue(
-            new TransferNoteCommand(
-                shooterSubsystem, intakeSubsystem, pivotSubsystem, elevatorSubsystem));
+    anthony.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
-    jacob.b().onTrue(new ShooterRampUpCommand(shooterSubsystem, ShooterMode.RAMP_SPEAKER));
+    jacob.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     jacob.a().onTrue(new ShootCommand(shooterSubsystem));
 
     // SPEAKER FROM SUBWOOFER
@@ -450,7 +433,9 @@ private void configureButtonBindings() {
                     translationXSupplier,
                     translationYSupplier,
                     DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
-                .alongWith(new HeightCommand(elevatorSubsystem, 20)));
+                .alongWith(
+                    new ElevatorHeightCommand(
+                        elevatorSubsystem, 20))); // height should be set to a constant
 
     DoubleSupplier rotation =
         exponential(
diff --git a/src/main/java/frc/robot/commands/AngleCommand.java b/src/main/java/frc/robot/commands/ArmAngleCommand.java
similarity index 91%
rename from src/main/java/frc/robot/commands/AngleCommand.java
rename to src/main/java/frc/robot/commands/ArmAngleCommand.java
index 66984cf..dbba70d 100644
--- a/src/main/java/frc/robot/commands/AngleCommand.java
+++ b/src/main/java/frc/robot/commands/ArmAngleCommand.java
@@ -10,7 +10,7 @@
 import frc.robot.subsystems.ElevatorSubsystem;
 
 /** An example command that uses an example subsystem. */
-public class AngleCommand extends Command {
+public class ArmAngleCommand extends Command {
 
   ElevatorSubsystem elevatorSubsystem;
 
@@ -21,7 +21,7 @@ public class AngleCommand extends Command {
    */
   double angle;
 
-  public AngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) {
+  public ArmAngleCommand(ElevatorSubsystem elevatorSubsystem, double angle) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.angle = angle;
     // Use addRequirements() here to declare subsystem dependencies.
diff --git a/src/main/java/frc/robot/commands/AutoAlignCommand.java b/src/main/java/frc/robot/commands/AutoAlignCommand.java
deleted file mode 100644
index 5f08948..0000000
--- a/src/main/java/frc/robot/commands/AutoAlignCommand.java
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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 com.pathplanner.lib.auto.AutoBuilder;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.DrivebaseSubsystem;
-
-public class AutoAlignCommand extends Command {
-  DrivebaseSubsystem drivebaseSubsystem;
-  /** Creates a new AutoAlignCommand. */
-  public AutoAlignCommand(
-      DrivebaseSubsystem drivebaseSubsystem, AutoBuilder autoBuilder, Pose2d targetPose) {
-    this.drivebaseSubsystem = drivebaseSubsystem;
-    addRequirements(drivebaseSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    drivebaseSubsystem.setDefenseMode();
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/HeightCommand.java b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java
similarity index 90%
rename from src/main/java/frc/robot/commands/HeightCommand.java
rename to src/main/java/frc/robot/commands/ElevatorHeightCommand.java
index 434ffe1..24ea50c 100644
--- a/src/main/java/frc/robot/commands/HeightCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java
@@ -10,7 +10,7 @@
 import frc.robot.subsystems.ElevatorSubsystem;
 
 /** An example command that uses an example subsystem. */
-public class HeightCommand extends Command {
+public class ElevatorHeightCommand extends Command {
 
   ElevatorSubsystem elevatorSubsystem;
 
@@ -21,7 +21,7 @@ public class HeightCommand extends Command {
    */
   double height;
 
-  public HeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
+  public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.height = height;
     // Use addRequirements() here to declare subsystem dependencies.
diff --git a/src/main/java/frc/robot/commands/HashMapCommand.java b/src/main/java/frc/robot/commands/HashMapCommand.java
deleted file mode 100644
index fa0b241..0000000
--- a/src/main/java/frc/robot/commands/HashMapCommand.java
+++ /dev/null
@@ -1,70 +0,0 @@
-// 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 edu.wpi.first.wpilibj2.command.CommandScheduler;
-import java.util.Map;
-import java.util.function.Supplier;
-import java.util.stream.Collectors;
-
-public class HashMapCommand<T> extends Command {
-  private final Map<T, Command> commandMap;
-  private final Supplier<T> keySupplier;
-  private Command selectedCommand;
-  private boolean runsWhenDisabled;
-
-  /**
-   * Creates a new ConditionalCommand.
-   *
-   * @param onTrue the command to run if the condition is true
-   * @param onFalse the command to run if the condition is false
-   * @param condition the condition to determine which command to run
-   */
-  public HashMapCommand(Map<T, Command> commandMap, Supplier<T> keySupplier) {
-    this.commandMap = commandMap;
-    this.keySupplier = keySupplier;
-
-    CommandScheduler.getInstance()
-        .registerComposedCommands(commandMap.values().toArray(new Command[0]));
-
-    m_requirements.addAll(
-        commandMap.values().stream()
-            .flatMap(command -> command.getRequirements().stream())
-            .collect(Collectors.toList()));
-
-    runsWhenDisabled = commandMap.values().stream().allMatch(Command::runsWhenDisabled);
-  }
-
-  @Override
-  public void initialize() {
-    var key = keySupplier.get();
-    selectedCommand = commandMap.get(key);
-    if (selectedCommand == null) {
-      throw new IllegalArgumentException("No command for key " + key);
-    }
-    selectedCommand.initialize();
-  }
-
-  @Override
-  public void execute() {
-    selectedCommand.execute();
-  }
-
-  @Override
-  public void end(boolean interrupted) {
-    selectedCommand.end(interrupted);
-  }
-
-  @Override
-  public boolean isFinished() {
-    return selectedCommand.isFinished();
-  }
-
-  @Override
-  public boolean runsWhenDisabled() {
-    return runsWhenDisabled;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index 3a8a8a4..95d3859 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -2,6 +2,8 @@
 // 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.
 
+/* EDIT THIS COMMAND the elevator needs time to realign with the pivot!!!! */
+
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
diff --git a/src/main/java/frc/robot/commands/SetRampModeCommand.java b/src/main/java/frc/robot/commands/SetRampModeCommand.java
deleted file mode 100644
index 4a89c2e..0000000
--- a/src/main/java/frc/robot/commands/SetRampModeCommand.java
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-public class SetRampModeCommand extends Command {
-  private ShooterSubsystem shooterSubsystem;
-  /** Creates a new SetRampModeCommand. */
-  public SetRampModeCommand(ShooterSubsystem shooterSubsystem) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    this.shooterSubsystem = shooterSubsystem;
-
-    addRequirements(shooterSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.RAMP_SPEAKER);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/ShootAmpCommand.java b/src/main/java/frc/robot/commands/ShootAmpCommand.java
deleted file mode 100644
index a178b05..0000000
--- a/src/main/java/frc/robot/commands/ShootAmpCommand.java
+++ /dev/null
@@ -1,40 +0,0 @@
-// 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.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-public class ShootAmpCommand extends Command {
-  private ShooterSubsystem shooterSubsystem;
-
-  /** Creates a new ShootAmp. */
-  public ShootAmpCommand(ShooterSubsystem shooterSubsystem) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    this.shooterSubsystem = shooterSubsystem;
-    addRequirements(shooterSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.SHOOT_AMP);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return !shooterSubsystem.isShooterBeamBreakSensorTriggered();
-  }
-}
diff --git a/src/main/java/frc/robot/commands/ShooterRampUpCommand.java b/src/main/java/frc/robot/commands/ShooterRampUpCommand.java
deleted file mode 100644
index 618a720..0000000
--- a/src/main/java/frc/robot/commands/ShooterRampUpCommand.java
+++ /dev/null
@@ -1,42 +0,0 @@
-// 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.subsystems.ShooterSubsystem;
-import frc.robot.subsystems.ShooterSubsystem.ShooterMode;
-
-public class ShooterRampUpCommand extends Command {
-  private ShooterSubsystem shooterSubsystem;
-  private ShooterMode mode;
-
-  /** Creates a new ShooterRampUpCommand. */
-  public ShooterRampUpCommand(ShooterSubsystem shooterSubsystem, ShooterMode mode) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    this.shooterSubsystem = shooterSubsystem;
-    this.mode = mode;
-    addRequirements(shooterSubsystem);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    shooterSubsystem.setShooterMode(mode);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return true;
-  }
-}
diff --git a/src/main/java/frc/robot/commands/SpeakerLockCommand.java b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
index dab0c0b..c0e54e5 100644
--- a/src/main/java/frc/robot/commands/SpeakerLockCommand.java
+++ b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
@@ -7,8 +7,7 @@
 public class SpeakerLockCommand extends ParallelCommandGroup {
   public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) {
     addCommands(
-        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0)
-        // new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)
-        );
+        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
+        new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/commands/TransferNoteCommand.java b/src/main/java/frc/robot/commands/TransferNoteCommand.java
deleted file mode 100644
index f5a1a00..0000000
--- a/src/main/java/frc/robot/commands/TransferNoteCommand.java
+++ /dev/null
@@ -1,26 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.subsystems.ElevatorSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.PivotSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-
-public class TransferNoteCommand extends SequentialCommandGroup {
-
-  public TransferNoteCommand(
-      ShooterSubsystem shooterSubsystem,
-      IntakeSubsystem intakeSubsystem,
-      PivotSubsystem pivotSubsystem,
-      ElevatorSubsystem elevatorSubsystem) {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) {
-      addCommands(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-    } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) {
-      addCommands(
-          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
-              .andThen(
-                  new IntakeCommand(
-                      intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
-    }
-  }
-}

From 2c95e5d57cf5a387311e71535f692fd26b782c5d Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 19 Sep 2024 11:07:15 -0700
Subject: [PATCH 22/29] Made the elevator and pivot go down when needed

---
 src/main/java/frc/robot/Constants.java        |  2 +
 src/main/java/frc/robot/RobotContainer.java   | 39 ++++++++++++++-----
 .../robot/commands/AmpPreparationCommand.java | 37 ++++++++++++++++++
 ...otAndElevatorTransferPositionsCommand.java | 20 ++++++++++
 4 files changed, 88 insertions(+), 10 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/AmpPreparationCommand.java
 create mode 100644 src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6d6b75a..4e06b1c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -578,6 +578,8 @@ public static final class Ports {
     public static final double MAX_HEIGHT = 50;
     /** FIXME */
     public static final double ELEVATOR_FEEDFORWARD = 0.01;
+    /**FIXME */
+    public static final double AMP_HEIGHT = 20;
 
     public static final class Arm {
       /** FIXME */
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f86c6d2..10436e5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -33,6 +33,7 @@
 import frc.robot.autonomous.HeadingAngle;
 import frc.robot.autonomous.HeadingTargetLock;
 import frc.robot.commands.AccelNoteCommand;
+import frc.robot.commands.AmpPreparationCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.ElevatorHeightCommand;
@@ -40,6 +41,7 @@
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
 import frc.robot.commands.OuttakeCommand;
+import frc.robot.commands.PivotAndElevatorTransferPositionsCommand;
 import frc.robot.commands.PivotAngleCommand;
 import frc.robot.commands.PivotManualCommand;
 import frc.robot.commands.PivotTargetLockCommand;
@@ -51,6 +53,7 @@
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
 import frc.robot.commands.TargetLockCommand;
+import frc.robot.commands.UnloadShooterCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -338,14 +341,18 @@ private void configureButtonBindings() {
     anthony
         .leftBumper()
         .onTrue(
-            new IntakeCommand(
-                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+            .andThen(
+                new IntakeCommand(
+                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     jacob
         .leftBumper()
         .onTrue(
-            new IntakeCommand(
-                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+            .andThen(
+                new IntakeCommand(
+                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT
     anthony
@@ -412,10 +419,22 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-
-    jacob.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
-    jacob.a().onTrue(new ShootCommand(shooterSubsystem));
+    anthony.b().onTrue(
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+
+    jacob.b().onTrue(
+        new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+    jacob.a().onTrue(
+        new RotateAngleDriveCommand(
+                drivebaseSubsystem,
+                translationXSupplier,
+                translationYSupplier,
+                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+            .alongWith(
+                new AmpPreparationCommand(
+                    pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     // SPEAKER FROM SUBWOOFER
     anthony
@@ -434,8 +453,8 @@ private void configureButtonBindings() {
                     translationYSupplier,
                     DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
                 .alongWith(
-                    new ElevatorHeightCommand(
-                        elevatorSubsystem, 20))); // height should be set to a constant
+                    new AmpPreparationCommand(
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem))); 
 
     DoubleSupplier rotation =
         exponential(
diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
new file mode 100644
index 0000000..321117a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -0,0 +1,37 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants.Elevator;
+import frc.robot.subsystems.DrivebaseSubsystem;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.PivotSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class AmpPreparationCommand extends SequentialCommandGroup {
+  /** Creates a new AmpPreparationCommand. */
+  public AmpPreparationCommand(
+    PivotSubsystem pivotSubsystem,
+    ElevatorSubsystem elevatorSubsystem,
+    ShooterSubsystem shooterSubsystem) {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
+      addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT));
+    }
+    else{
+      addCommands( 
+          new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+        .andThen(
+          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
+        .andThen(
+          new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
new file mode 100644
index 0000000..d7e7c2d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
@@ -0,0 +1,20 @@
+// 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 edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.PivotSubsystem;
+
+public class PivotAndElevatorTransferPositionsCommand extends ParallelCommandGroup {
+  /** Creates a new PivotAndElevatorTransferPositionsCommand. */
+  public PivotAndElevatorTransferPositionsCommand(
+    PivotSubsystem pivotSubsystem,
+    ElevatorSubsystem elevatorSubsystem) {
+      addCommands(new PivotAngleCommand(pivotSubsystem, 20),
+        new ElevatorHeightCommand(elevatorSubsystem, 0));
+  }
+}

From cfb73069ed33ac4a4a646930915f1ea16acf5ac1 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 19 Sep 2024 17:06:09 -0700
Subject: [PATCH 23/29] Checked pivot subsystem

---
 src/main/java/frc/robot/subsystems/PivotSubsystem.java | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index f727946..35cb677 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -99,7 +99,7 @@ public double getCurrentError() {
   }
 
   public double getCurrentAngle() {
-    return rotationsToDegrees(pivotMotor.getPosition().getValueAsDouble());
+    return rotationsToDegrees(pivotCANcoder.getAbsolutePosition().getValueAsDouble());
   }
 
   public double getTargetDegrees() {
@@ -120,7 +120,7 @@ public void setTargetDegrees(double degrees) {
   }
 
   private static double rotationsToDegrees(double rotations) {
-    return rotations * Pivot.PIVOT_GEAR_RATIO;
+    return rotations * 360;
   }
 
   public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) {
@@ -177,6 +177,6 @@ public void periodic() {
 
     pidVoltageOutput = MathUtil.clamp(pidOutput + getFeedForward(), -10, 10);
 
-    pivotMotor.set(getFeedForward() + power);
+    pivotMotor.set(getFeedForward() + pidVoltageOutput);
   }
 }

From 6018eeb081eb5d3b18648ed655683e5647dde420 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 19 Sep 2024 17:57:51 -0700
Subject: [PATCH 24/29] Pivot edits

---
 src/main/java/frc/robot/RobotContainer.java                 | 6 ++++--
 src/main/java/frc/robot/commands/AmpPreparationCommand.java | 6 ------
 src/main/java/frc/robot/commands/PivotManualCommand.java    | 2 +-
 3 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 10436e5..939a2b0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -415,8 +415,10 @@ private void configureButtonBindings() {
                         ? -Setpoints.SOURCE_DEGREES
                         : Setpoints.SOURCE_DEGREES)
                 .alongWith(
-                    new IntakeCommand(
-                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                    new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+                    .andThen(
+                        new IntakeCommand(
+                            intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))));
 
     // NOTE TO SHOOTER OR SERIALIZER
     anthony.b().onTrue(
diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
index 321117a..fc97b9b 100644
--- a/src/main/java/frc/robot/commands/AmpPreparationCommand.java
+++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -4,14 +4,8 @@
 
 package frc.robot.commands;
 
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.Elevator;
-import frc.robot.subsystems.DrivebaseSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
diff --git a/src/main/java/frc/robot/commands/PivotManualCommand.java b/src/main/java/frc/robot/commands/PivotManualCommand.java
index 0bc78d3..a38feb4 100644
--- a/src/main/java/frc/robot/commands/PivotManualCommand.java
+++ b/src/main/java/frc/robot/commands/PivotManualCommand.java
@@ -19,7 +19,7 @@ public PivotManualCommand(PivotSubsystem pivotSubsystem, DoubleSupplier joystick
 
   @Override
   public void execute() {
-    pivotSubsystem.setPower(joystickRate.getAsDouble());
+    pivotSubsystem.setPower(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
   }
 
   @Override

From 07be5d246a0d498bc75b4f45c32cf2e296786f53 Mon Sep 17 00:00:00 2001
From: audrey <audreypjohnson@gmail.com>
Date: Thu, 19 Sep 2024 19:05:38 -0700
Subject: [PATCH 25/29] improve naming, delete some unnecessary stuff, left
 comments for fixes

---
 src/main/java/frc/robot/Constants.java        |  2 +-
 src/main/java/frc/robot/RobotContainer.java   | 91 ++++++++-----------
 .../robot/commands/AmpPreparationCommand.java | 20 ++--
 .../frc/robot/commands/ArmAngleCommand.java   |  2 +-
 ...d.java => DrivebaseTargetLockCommand.java} |  4 +-
 .../robot/commands/ElevatorHeightCommand.java | 17 +---
 .../robot/commands/LoadShooterCommand.java    |  9 +-
 ...otAndElevatorTransferPositionsCommand.java | 12 ++-
 .../frc/robot/commands/PivotAngleCommand.java |  2 +-
 .../robot/commands/PivotManualCommand.java    |  2 +-
 .../java/frc/robot/commands/RGBCommand.java   |  4 +-
 .../robot/commands/SpeakerLockCommand.java    |  2 +-
 .../robot/subsystems/ElevatorSubsystem.java   | 26 ++++--
 .../frc/robot/subsystems/PivotSubsystem.java  | 38 +++-----
 14 files changed, 104 insertions(+), 127 deletions(-)
 rename src/main/java/frc/robot/commands/{TargetLockCommand.java => DrivebaseTargetLockCommand.java} (96%)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4e06b1c..947a2bb 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -578,7 +578,7 @@ public static final class Ports {
     public static final double MAX_HEIGHT = 50;
     /** FIXME */
     public static final double ELEVATOR_FEEDFORWARD = 0.01;
-    /**FIXME */
+    /** FIXME */
     public static final double AMP_HEIGHT = 20;
 
     public static final class Arm {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 939a2b0..5a26a02 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -36,7 +36,6 @@
 import frc.robot.commands.AmpPreparationCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
-import frc.robot.commands.ElevatorHeightCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
@@ -52,8 +51,7 @@
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
-import frc.robot.commands.TargetLockCommand;
-import frc.robot.commands.UnloadShooterCommand;
+import frc.robot.commands.DrivebaseTargetLockCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -149,7 +147,7 @@ public RobotContainer() {
     NamedCommands.registerCommand(
         "AutoPivotAngle", new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
     NamedCommands.registerCommand(
-        "AutoDrivebaseAngle", new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
+        "AutoDrivebaseAngle", new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0));
     NamedCommands.registerCommand("AccelNote", new AccelNoteCommand(shooterSubsystem));
     NamedCommands.registerCommand(
         "ZeroOrigin", new InstantCommand(() -> drivebaseSubsystem.zeroGyroscope()));
@@ -229,7 +227,7 @@ public RobotContainer() {
             translationXSupplier,
             translationYSupplier,
             // anthony.rightBumper(),
-            anthony.leftBumper()));
+            ()-> false));
 
     rgbSubsystem.setDefaultCommand(
         new RGBCommand(
@@ -262,7 +260,7 @@ public RobotContainer() {
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       driverView.addDouble("Shoot Var Velocity", () -> shooterSubsystem.variableVelocity);
       driverView.addString("ShooterMode", () -> shooterSubsystem.getMode().toString());
-      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getAngularError());
+      driverView.addDouble("Pivot Angle Error", () -> pivotSubsystem.getCurrentError());
       driverView.addDouble("Drivebase Angle Error", () -> drivebaseSubsystem.getAngularError());
     }
 
@@ -342,17 +340,17 @@ private void configureButtonBindings() {
         .leftBumper()
         .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-            .andThen(
-                new IntakeCommand(
-                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     jacob
         .leftBumper()
         .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-            .andThen(
-                new IntakeCommand(
-                    intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT
     anthony
@@ -365,6 +363,8 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
+    
+    // FIXME should be right trigger is move elevator up, left trigger is move elevator down
     jacob
         .rightTrigger()
         .onTrue(new AccelNoteCommand(shooterSubsystem).andThen(new ShootCommand(shooterSubsystem)));
@@ -374,30 +374,9 @@ private void configureButtonBindings() {
     jacob
         .y()
         .whileTrue(
-            new TargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
+            new DrivebaseTargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
                 .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
 
-    // jacob
-    //         .a()
-    //         .onTrue(
-    //                 new RotateAngleDriveCommand(
-    //                         drivebaseSubsystem,
-    //                         translationXSupplier,
-    //                         translationYSupplier,
-    //                         DriverStation.getAlliance().get().equals(Alliance.Red) ? -40 : 40)
-    //                         .alongWith(new PivotAngleCommand(pivotSubsystem, 60))
-    //                         .alongWith(new ShooterRampUpCommand(shooterSubsystem,
-    // ShooterMode.SHUTTLE)));
-
-    // anthony.y().whileTrue(new TargetLockCommand(drivebaseSubsystem,
-    // translationXSupplier,
-    // translationYSupplier, Setpoints.SPEAKER));
-
-    // DoubleSupplier variableVelocityRate = () -> modifyAxis(-jacob.getRightY());
-
-    // new Trigger(() -> Math.abs(variableVelocityRate.getAsDouble()) > 0.07)
-    // .onTrue(new VariableShooterCommand(shooterSubsystem, variableVelocityRate));
-
     DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
 
     new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
@@ -421,22 +400,30 @@ private void configureButtonBindings() {
                             intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony.b().onTrue(
+    anthony
+        .b()
+        .onTrue(
             new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+                .andThen(
+                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
-    jacob.b().onTrue(
-        new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
-    jacob.a().onTrue(
-        new RotateAngleDriveCommand(
-                drivebaseSubsystem,
-                translationXSupplier,
-                translationYSupplier,
-                DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
-            .alongWith(
-                new AmpPreparationCommand(
-                    pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
+    jacob
+        .b()
+        .onTrue(
+            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
+                .andThen(
+                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+    jacob
+        .a()
+        .onTrue(
+            new RotateAngleDriveCommand(
+                    drivebaseSubsystem,
+                    translationXSupplier,
+                    translationYSupplier,
+                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
+                .alongWith(
+                    new AmpPreparationCommand(
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     // SPEAKER FROM SUBWOOFER
     anthony
@@ -453,10 +440,10 @@ private void configureButtonBindings() {
                     drivebaseSubsystem,
                     translationXSupplier,
                     translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50)
+                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
-                        pivotSubsystem, elevatorSubsystem, shooterSubsystem))); 
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
 
     DoubleSupplier rotation =
         exponential(
@@ -475,7 +462,7 @@ private void configureButtonBindings() {
                 translationXSupplier,
                 translationYSupplier,
                 rotationVelocity,
-                anthony.rightBumper()));
+                () -> false));
 
     new Trigger(
             () ->
@@ -488,7 +475,7 @@ private void configureButtonBindings() {
                 translationYSupplier,
                 anthony::getRightY,
                 anthony::getRightX,
-                anthony.rightBumper()));
+                () -> false));
   }
 
   /**
diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
index fc97b9b..9f8626b 100644
--- a/src/main/java/frc/robot/commands/AmpPreparationCommand.java
+++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -13,19 +13,17 @@
 public class AmpPreparationCommand extends SequentialCommandGroup {
   /** Creates a new AmpPreparationCommand. */
   public AmpPreparationCommand(
-    PivotSubsystem pivotSubsystem,
-    ElevatorSubsystem elevatorSubsystem,
-    ShooterSubsystem shooterSubsystem) {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()){
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      ShooterSubsystem shooterSubsystem) {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) {
       addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT));
-    }
-    else{
-      addCommands( 
+    } else {
+      addCommands(
           new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-        .andThen(
-          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
-        .andThen(
-          new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
+              .andThen(
+                  new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
+              .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/commands/ArmAngleCommand.java b/src/main/java/frc/robot/commands/ArmAngleCommand.java
index dbba70d..66c18ed 100644
--- a/src/main/java/frc/robot/commands/ArmAngleCommand.java
+++ b/src/main/java/frc/robot/commands/ArmAngleCommand.java
@@ -46,6 +46,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.nearTargetAngle();
+    return elevatorSubsystem.atTargetAngle();
   }
 }
diff --git a/src/main/java/frc/robot/commands/TargetLockCommand.java b/src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java
similarity index 96%
rename from src/main/java/frc/robot/commands/TargetLockCommand.java
rename to src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java
index 16b5e47..e3796d8 100644
--- a/src/main/java/frc/robot/commands/TargetLockCommand.java
+++ b/src/main/java/frc/robot/commands/DrivebaseTargetLockCommand.java
@@ -17,7 +17,7 @@
  * This command takes a drive angle and a target x,y coordinate, and snaps the robot to face the
  * target. This is useful to lock the robot to fixed target with a button.
  */
-public class TargetLockCommand extends Command {
+public class DrivebaseTargetLockCommand extends Command {
   private final DrivebaseSubsystem drivebaseSubsystem;
 
   private final DoubleSupplier translationXSupplier;
@@ -28,7 +28,7 @@ public class TargetLockCommand extends Command {
   private double targetAngle;
 
   /** Creates a new TargetLockCommand. */
-  public TargetLockCommand(
+  public DrivebaseTargetLockCommand(
       DrivebaseSubsystem drivebaseSubsystem,
       DoubleSupplier translationXSupplier,
       DoubleSupplier translationYSupplier) {
diff --git a/src/main/java/frc/robot/commands/ElevatorHeightCommand.java b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java
index 24ea50c..b61bb8b 100644
--- a/src/main/java/frc/robot/commands/ElevatorHeightCommand.java
+++ b/src/main/java/frc/robot/commands/ElevatorHeightCommand.java
@@ -13,12 +13,6 @@
 public class ElevatorHeightCommand extends Command {
 
   ElevatorSubsystem elevatorSubsystem;
-
-  /**
-   * Creates a new ExampleCommand.
-   *
-   * @param subsystem The subsystem used by this command.
-   */
   double height;
 
   public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
@@ -30,14 +24,13 @@ public ElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height)
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    elevatorSubsystem.setTargetHeight(height);
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-
-    elevatorSubsystem.setTargetHeight(height);
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
@@ -46,6 +39,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.nearTargetHeight();
+    return elevatorSubsystem.atTargetHeight();
   }
 }
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index 95d3859..a02b248 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -19,6 +19,8 @@ public class LoadShooterCommand extends Command {
   ElevatorSubsystem elevatorSubsystem;
   PivotSubsystem pivotSubsystem;
 
+  /** Use PivotAndElevatorTransferCommand before running */
+
   public LoadShooterCommand(
       ShooterSubsystem shooterSubsystem,
       PivotSubsystem pivotSubsystem,
@@ -32,14 +34,17 @@ public LoadShooterCommand(
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
     elevatorSubsystem.setTargetHeight(0);
     pivotSubsystem.setTargetDegrees(20);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) {
+      shooterSubsystem.setShooterMode(ShooterMode.LOAD_SHOOTER);
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
index d7e7c2d..853f013 100644
--- a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
+++ b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
@@ -4,7 +4,6 @@
 
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
@@ -12,9 +11,12 @@
 public class PivotAndElevatorTransferPositionsCommand extends ParallelCommandGroup {
   /** Creates a new PivotAndElevatorTransferPositionsCommand. */
   public PivotAndElevatorTransferPositionsCommand(
-    PivotSubsystem pivotSubsystem,
-    ElevatorSubsystem elevatorSubsystem) {
-      addCommands(new PivotAngleCommand(pivotSubsystem, 20),
-        new ElevatorHeightCommand(elevatorSubsystem, 0));
+      PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
+    addCommands(
+        new PivotAngleCommand(pivotSubsystem, 20), new ElevatorHeightCommand(elevatorSubsystem, 0));
   }
 }
+
+  /* combine load and unload commands with this one - 
+  it should move everything into the correct position 
+  and either load or unload based on input from a button */
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/PivotAngleCommand.java b/src/main/java/frc/robot/commands/PivotAngleCommand.java
index ba0a31e..60f47d9 100644
--- a/src/main/java/frc/robot/commands/PivotAngleCommand.java
+++ b/src/main/java/frc/robot/commands/PivotAngleCommand.java
@@ -36,6 +36,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return pivotSubsystem.isAtTargetDegrees();
+    return pivotSubsystem.atTargetDegrees();
   }
 }
diff --git a/src/main/java/frc/robot/commands/PivotManualCommand.java b/src/main/java/frc/robot/commands/PivotManualCommand.java
index a38feb4..67b3690 100644
--- a/src/main/java/frc/robot/commands/PivotManualCommand.java
+++ b/src/main/java/frc/robot/commands/PivotManualCommand.java
@@ -19,7 +19,7 @@ public PivotManualCommand(PivotSubsystem pivotSubsystem, DoubleSupplier joystick
 
   @Override
   public void execute() {
-    pivotSubsystem.setPower(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
+    pivotSubsystem.setTargetDegrees(pivotSubsystem.getTargetDegrees() + joystickRate.getAsDouble());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/commands/RGBCommand.java b/src/main/java/frc/robot/commands/RGBCommand.java
index 26fe4bd..7af4188 100644
--- a/src/main/java/frc/robot/commands/RGBCommand.java
+++ b/src/main/java/frc/robot/commands/RGBCommand.java
@@ -92,7 +92,7 @@ public void execute() {
 
     // ready to shoot = red
     if (isReadyToShootInSun()
-        && pivotSubsystem.isAtTargetDegrees()
+        && pivotSubsystem.atTargetDegrees()
         && Math.abs(drivebaseSubsystem.getAngularError()) < 2
         && readyToShootMsg.isEmpty()
         && visionSubsystem.getCanSeeSpeakerTags()) {
@@ -107,7 +107,7 @@ public void execute() {
         // input
         shooterSubsystem.getMode().equals(ShooterMode.SHOOT_SHUTTLE)
         || shooterSubsystem.getMode().equals(ShooterMode.SHOOT_VAR)
-        || pivotSubsystem.isAtTargetDegrees()) {
+        || pivotSubsystem.atTargetDegrees()) {
       readyToShootMsg.ifPresent(RGBMessage::expire);
       readyToShootMsg = Optional.empty();
     }
diff --git a/src/main/java/frc/robot/commands/SpeakerLockCommand.java b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
index c0e54e5..63155db 100644
--- a/src/main/java/frc/robot/commands/SpeakerLockCommand.java
+++ b/src/main/java/frc/robot/commands/SpeakerLockCommand.java
@@ -7,7 +7,7 @@
 public class SpeakerLockCommand extends ParallelCommandGroup {
   public SpeakerLockCommand(DrivebaseSubsystem drivebaseSubsystem, PivotSubsystem pivotSubsystem) {
     addCommands(
-        new TargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
+        new DrivebaseTargetLockCommand(drivebaseSubsystem, () -> 0, () -> 0),
         new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
index 23f8f2e..11be8cb 100644
--- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java
@@ -16,13 +16,15 @@ public class ElevatorSubsystem extends SubsystemBase {
 
   private TalonFX armMotor;
   private TalonFX elevatorMotor;
-  private PIDController controller;
+  private PIDController elevatorController;
+  private PIDController armController;
   private double targetHeight;
   private double targetAngle;
   private double elevatorMotorPower;
   private double armMotorPower;
   private double currentHeight;
   private double currentAngle;
+
   private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
 
   /** Creates a new ElevatorSubsystem. */
@@ -36,13 +38,15 @@ public ElevatorSubsystem() {
     elevatorMotor.setPosition(0);
     armMotor.setPosition(0);
 
-    controller = new PIDController(0.4, 0, 0.0125);
+    // FIXME placeholder values
+    elevatorController = new PIDController(0.4, 0, 0.0125);
+    armController = new PIDController(0.2, 0, 0);
 
     ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
     ElevatorTab.addNumber("Target Angle", () -> this.targetAngle);
-    // ElevatorTab.addNumber("PID output", () -> this.controller);
     ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
-    ElevatorTab.add(controller);
+    ElevatorTab.add(elevatorController);
+    ElevatorTab.add(armController);
 
     targetHeight = 0;
     currentHeight = 0;
@@ -82,14 +86,15 @@ public void setTargetAngle(double targetAngle) {
     this.targetAngle = MathUtil.clamp(targetAngle, Elevator.Arm.MAX_ANGLE, 0);
   }
 
-  public boolean nearTargetAngle() {
+  public boolean atTargetAngle() {
+    // replace with epsilon equals
     if (targetAngle - 0.5 <= getArmPosition() && getArmPosition() <= targetAngle + 0.5) {
       return true;
     }
     return false;
   }
 
-  public boolean nearTargetHeight() {
+  public boolean atTargetHeight() {
     if (targetHeight - 0.5 <= getElevatorPosition()
         && getElevatorPosition() <= targetHeight + 0.5) {
       return true;
@@ -97,17 +102,18 @@ && getElevatorPosition() <= targetHeight + 0.5) {
     return false;
   }
 
-  public double calculateFeedforward() {
+  public double calculateArmFeedforward() {
     return 0; // FIXME
+    // using a constant force spring
   }
 
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    elevatorMotorPower = controller.calculate(getElevatorPosition(), targetHeight);
-    armMotorPower = controller.calculate(getArmPosition(), targetAngle);
+    elevatorMotorPower = elevatorController.calculate(getElevatorPosition(), targetHeight);
+    armMotorPower = armController.calculate(getArmPosition(), targetAngle);
     elevatorMotor.setVoltage(
         MathUtil.clamp(elevatorMotorPower + Elevator.ELEVATOR_FEEDFORWARD, -10, 10));
-    armMotor.setVoltage(MathUtil.clamp(armMotorPower + calculateFeedforward(), -10, 10));
+    armMotor.setVoltage(MathUtil.clamp(armMotorPower + calculateArmFeedforward(), -10, 10));
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/PivotSubsystem.java b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
index 35cb677..5733376 100644
--- a/src/main/java/frc/robot/subsystems/PivotSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/PivotSubsystem.java
@@ -36,11 +36,9 @@ public class PivotSubsystem extends SubsystemBase {
   private double pidVoltageOutput;
   private double calculatedTargetDegrees;
 
-  private boolean inRange;
   private Pose2d pose;
-  private double distance;
+  private double distanceFromSpeaker;
   private GenericEntry debugTarget;
-  private double power;
 
   private double pastDebugTarget = 0;
   private boolean listenToDebug = true;
@@ -71,15 +69,14 @@ public PivotSubsystem() {
       pivotTab.addNumber(
           "Current Motor Position", () -> pivotMotor.getPosition().getValueAsDouble());
       pivotTab.addNumber("Current Pivot Angle", this::getCurrentAngle);
-      pivotTab.addBoolean("Is at target", this::isAtTargetDegrees);
+      pivotTab.addBoolean("Is at target", this::atTargetDegrees);
       pivotTab.addNumber("Motor Error", this::getCurrentError);
       pivotTab.addNumber("PID Error", pidController::getPositionError);
       pivotTab.addNumber("Target Degrees", this::getTargetDegrees);
       pivotTab.addNumber("Applied Voltage", () -> pivotMotor.getMotorVoltage().getValueAsDouble());
       pivotTab.addDouble("PID Voltage Output", () -> pidVoltageOutput);
       pivotTab.addDouble("Calculated Target Angle", () -> calculatedTargetDegrees);
-      pivotTab.addDouble("Power", () -> power);
-      pivotTab.addDouble("Distance", () -> distance);
+      pivotTab.addDouble("Distance", () -> distanceFromSpeaker);
       pivotTab.add(pidController);
       debugTarget =
           pivotTab
@@ -106,14 +103,10 @@ public double getTargetDegrees() {
     return targetDegrees;
   }
 
-  public boolean isAtTargetDegrees() {
+  public boolean atTargetDegrees() {
     return Util.epsilonEquals(getCurrentAngle(), targetDegrees, Pivot.EPSILON);
   }
 
-  public boolean isReadyToShoot() {
-    return isAtTargetDegrees() && inRange;
-  }
-
   public void setTargetDegrees(double degrees) {
     this.targetDegrees = MathUtil.clamp(degrees, Setpoints.MINIMUM_ANGLE, Setpoints.MAXIMUM_ANGLE);
     listenToDebug = false;
@@ -138,31 +131,24 @@ public void calculatePivotTargetDegrees(Pose2d pose, double xV, double yV) {
       speakerX = Pivot.BLUE_SPEAKER_POSE.getX();
       speakerY = Pivot.BLUE_SPEAKER_POSE.getY();
     }
-    distance =
+    distanceFromSpeaker =
         (Math.sqrt(Math.pow((x - speakerX), 2) + Math.pow((y - speakerY), 2)))
             - Pivot.CENTER_OF_ROBOT_TO_BUMPER;
     targetDegrees =
-        -0.006073 * Math.pow(distance, 6)
-            + 0.167509 * Math.pow(distance, 5)
-            - 1.803043 * Math.pow(distance, 4)
-            + 9.309355 * Math.pow(distance, 3)
-            - 21.517994 * Math.pow(distance, 2)
-            + 6.8762 * distance
+        -0.006073 * Math.pow(distanceFromSpeaker, 6)
+            + 0.167509 * Math.pow(distanceFromSpeaker, 5)
+            - 1.803043 * Math.pow(distanceFromSpeaker, 4)
+            + 9.309355 * Math.pow(distanceFromSpeaker, 3)
+            - 21.517994 * Math.pow(distanceFromSpeaker, 2)
+            + 6.8762 * distanceFromSpeaker
             + 64.78029
             - 2.7;
   }
 
-  public double getAngularError() {
-    return targetDegrees - getCurrentAngle();
-  }
-
-  public void setPower(double power) {
-    this.power = power;
-  }
-
   @Override
   public void periodic() {
 
+    /* debugging stuff */
     double currentTarget = debugTarget.getDouble(23.5);
 
     if (currentTarget != pastDebugTarget) {

From fb07786458c5d6d0710f390065e3b2a116ab6dfc Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 19 Sep 2024 20:02:46 -0700
Subject: [PATCH 26/29] Transferring note in two commands

---
 src/main/java/frc/robot/RobotContainer.java   | 31 ++++++-------------
 .../robot/commands/AmpPreparationCommand.java | 12 ++++---
 .../frc/robot/commands/IntakeCommand.java     | 10 +++---
 ...otAndElevatorTransferPositionsCommand.java | 22 -------------
 .../robot/commands/UnloadShooterCommand.java  |  8 +++--
 5 files changed, 28 insertions(+), 55 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5a26a02..eb3ea61 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -40,7 +40,6 @@
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
 import frc.robot.commands.OuttakeCommand;
-import frc.robot.commands.PivotAndElevatorTransferPositionsCommand;
 import frc.robot.commands.PivotAngleCommand;
 import frc.robot.commands.PivotManualCommand;
 import frc.robot.commands.PivotTargetLockCommand;
@@ -339,18 +338,14 @@ private void configureButtonBindings() {
     anthony
         .leftBumper()
         .onTrue(
-            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-                .andThen(
-                    new IntakeCommand(
-                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+            new IntakeCommand(
+                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     jacob
         .leftBumper()
         .onTrue(
-            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-                .andThen(
-                    new IntakeCommand(
-                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+            new IntakeCommand(
+                intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     // SHOOT
     anthony
@@ -394,25 +389,19 @@ private void configureButtonBindings() {
                         ? -Setpoints.SOURCE_DEGREES
                         : Setpoints.SOURCE_DEGREES)
                 .alongWith(
-                    new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-                    .andThen(
-                        new IntakeCommand(
-                            intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))));
+                    new IntakeCommand(
+                        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
     anthony
         .b()
         .onTrue(
-            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-                .andThen(
-                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+            new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     jacob
         .b()
         .onTrue(
-            new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-                .andThen(
-                    new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
+            new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     jacob
         .a()
         .onTrue(
@@ -423,7 +412,7 @@ private void configureButtonBindings() {
                     DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
-                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem)));
 
     // SPEAKER FROM SUBWOOFER
     anthony
@@ -443,7 +432,7 @@ private void configureButtonBindings() {
                     DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
-                        pivotSubsystem, elevatorSubsystem, shooterSubsystem)));
+                        pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem)));
 
     DoubleSupplier rotation =
         exponential(
diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
index 9f8626b..dd55d69 100644
--- a/src/main/java/frc/robot/commands/AmpPreparationCommand.java
+++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.Elevator;
 import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 
@@ -15,15 +16,16 @@ public class AmpPreparationCommand extends SequentialCommandGroup {
   public AmpPreparationCommand(
       PivotSubsystem pivotSubsystem,
       ElevatorSubsystem elevatorSubsystem,
-      ShooterSubsystem shooterSubsystem) {
+      ShooterSubsystem shooterSubsystem,
+      IntakeSubsystem intakeSubsystem) {
     if (shooterSubsystem.isSerializerBeamBreakSensorTriggered()) {
       addCommands(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT));
     } else {
       addCommands(
-          new PivotAndElevatorTransferPositionsCommand(pivotSubsystem, elevatorSubsystem)
-              .andThen(
-                  new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem))
-              .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
+          new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
+      .andThen(new IntakeCommand(
+        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))
+      .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index 744005b..ca99496 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -37,15 +37,17 @@ public IntakeCommand(
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pivotSubsystem.setTargetDegrees(20);
-    intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
-    shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
     elevatorSubsystem.setTargetHeight(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if(elevatorSubsystem.atTargetHeight()){
+      intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
+      shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
+    }
+  }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java b/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
deleted file mode 100644
index 853f013..0000000
--- a/src/main/java/frc/robot/commands/PivotAndElevatorTransferPositionsCommand.java
+++ /dev/null
@@ -1,22 +0,0 @@
-// 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.ParallelCommandGroup;
-import frc.robot.subsystems.ElevatorSubsystem;
-import frc.robot.subsystems.PivotSubsystem;
-
-public class PivotAndElevatorTransferPositionsCommand extends ParallelCommandGroup {
-  /** Creates a new PivotAndElevatorTransferPositionsCommand. */
-  public PivotAndElevatorTransferPositionsCommand(
-      PivotSubsystem pivotSubsystem, ElevatorSubsystem elevatorSubsystem) {
-    addCommands(
-        new PivotAngleCommand(pivotSubsystem, 20), new ElevatorHeightCommand(elevatorSubsystem, 0));
-  }
-}
-
-  /* combine load and unload commands with this one - 
-  it should move everything into the correct position 
-  and either load or unload based on input from a button */
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
index e8c18ec..9c95f2f 100644
--- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -32,16 +32,18 @@ public UnloadShooterCommand(
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered();
-    shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
     pivotSubsystem.setTargetDegrees(20);
     elevatorSubsystem.setTargetHeight(0);
+    pass = shooterSubsystem.isSerializerBeamBreakSensorTriggered();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) {
+    if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) {
+      shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
+    }
+      if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) {
       pass = true;
     }
   }

From 7cfc734f793d3e9c4751b64db983dcff712a48e3 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 19 Sep 2024 20:30:20 -0700
Subject: [PATCH 27/29] Elevator manual

---
 src/main/java/frc/robot/RobotContainer.java   | 10 ++++
 .../robot/commands/ManualElevatorCommand.java | 46 +++++++++++++++++++
 2 files changed, 56 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/ManualElevatorCommand.java

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index eb3ea61..ef2c097 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -39,6 +39,7 @@
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
+import frc.robot.commands.ManualElevatorCommand;
 import frc.robot.commands.OuttakeCommand;
 import frc.robot.commands.PivotAngleCommand;
 import frc.robot.commands.PivotManualCommand;
@@ -376,6 +377,15 @@ private void configureButtonBindings() {
 
     new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
         .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
+    
+    DoubleSupplier elevatorDownSupplier = () -> modifyAxis(-jacob.getLeftTriggerAxis());
+    DoubleSupplier elevatorUpSupplier = () -> modifyAxis(jacob.getLeftTriggerAxis());
+
+    new Trigger(() -> elevatorDownSupplier.getAsDouble()<-0.07)
+        .onTrue(new ManualElevatorCommand(elevatorDownSupplier, elevatorSubsystem));
+
+    new Trigger(() -> elevatorUpSupplier.getAsDouble()>0.07)
+        .onTrue(new ManualElevatorCommand(elevatorUpSupplier, elevatorSubsystem));
 
     // SOURCE
     anthony
diff --git a/src/main/java/frc/robot/commands/ManualElevatorCommand.java b/src/main/java/frc/robot/commands/ManualElevatorCommand.java
new file mode 100644
index 0000000..c8dbe67
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ManualElevatorCommand.java
@@ -0,0 +1,46 @@
+// 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 java.util.function.DoubleSupplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
+
+public class ManualElevatorCommand extends Command {
+  /** Creates a new ManualElevatorCommand. */
+  DoubleSupplier directionSupplier;
+  ElevatorSubsystem elevatorSubsystem;
+  public ManualElevatorCommand(
+    DoubleSupplier directionSupplier, ElevatorSubsystem elevatorSubsystem) {
+    this.directionSupplier = directionSupplier;
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // 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() {
+    elevatorSubsystem.setTargetHeight(
+      elevatorSubsystem.getElevatorPosition()
+      +directionSupplier.getAsDouble());
+  
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

From e2262c927c40b62d098b0e5a866483bd3b5cda3a Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Fri, 20 Sep 2024 11:48:37 -0700
Subject: [PATCH 28/29] Added manual pivot commands and placeholer trap

---
 src/main/java/frc/robot/RobotContainer.java | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ef2c097..87cdeca 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -443,6 +443,27 @@ private void configureButtonBindings() {
                 .alongWith(
                     new AmpPreparationCommand(
                         pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem)));
+    
+    //PIVOT SETPOINTS
+    anthony
+        .povUp().onTrue(
+            new PivotAngleCommand(pivotSubsystem, 30));
+
+    anthony
+        .povLeft().onTrue(
+            new PivotAngleCommand(pivotSubsystem, 60));
+
+    anthony
+        .povRight().onTrue(
+            new PivotAngleCommand(pivotSubsystem, 75));
+
+    anthony
+        .povDown().onTrue(
+            new PivotAngleCommand(pivotSubsystem, 55));
+    
+    //TRAP CLIMB STUFF
+    // jacob.
+    //     povCenter().onTrue();
 
     DoubleSupplier rotation =
         exponential(

From 14564b98a73f024bdb05d8004a01121cc724d071 Mon Sep 17 00:00:00 2001
From: Heggiehog <nizcat5@gmail.com>
Date: Thu, 26 Sep 2024 17:34:10 -0700
Subject: [PATCH 29/29] Spotless apply

---
 src/main/java/frc/robot/RobotContainer.java   | 57 ++++++++-----------
 .../robot/commands/AmpPreparationCommand.java |  7 ++-
 .../frc/robot/commands/IntakeCommand.java     |  2 +-
 .../robot/commands/LoadShooterCommand.java    |  1 -
 .../robot/commands/ManualElevatorCommand.java | 11 ++--
 .../robot/commands/UnloadShooterCommand.java  |  2 +-
 6 files changed, 35 insertions(+), 45 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 87cdeca..951ecd9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -36,6 +36,7 @@
 import frc.robot.commands.AmpPreparationCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
+import frc.robot.commands.DrivebaseTargetLockCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.LoadShooterCommand;
@@ -51,7 +52,6 @@
 import frc.robot.commands.ShootCommand;
 import frc.robot.commands.StopIntakeCommand;
 import frc.robot.commands.StopShooterCommand;
-import frc.robot.commands.DrivebaseTargetLockCommand;
 import frc.robot.commands.VibrateHIDCommand;
 import frc.robot.subsystems.CANWatchdogSubsystem;
 import frc.robot.subsystems.DrivebaseSubsystem;
@@ -227,7 +227,7 @@ public RobotContainer() {
             translationXSupplier,
             translationYSupplier,
             // anthony.rightBumper(),
-            ()-> false));
+            () -> false));
 
     rgbSubsystem.setDefaultCommand(
         new RGBCommand(
@@ -359,7 +359,7 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
-    
+
     // FIXME should be right trigger is move elevator up, left trigger is move elevator down
     jacob
         .rightTrigger()
@@ -370,21 +370,22 @@ private void configureButtonBindings() {
     jacob
         .y()
         .whileTrue(
-            new DrivebaseTargetLockCommand(drivebaseSubsystem, translationXSupplier, translationYSupplier)
+            new DrivebaseTargetLockCommand(
+                    drivebaseSubsystem, translationXSupplier, translationYSupplier)
                 .alongWith(new PivotTargetLockCommand(pivotSubsystem, drivebaseSubsystem)));
 
     DoubleSupplier pivotManualRate = () -> modifyAxis(-jacob.getLeftY());
 
     new Trigger(() -> pivotManualRate.getAsDouble() > 0.07)
         .onTrue(new PivotManualCommand(pivotSubsystem, pivotManualRate));
-    
+
     DoubleSupplier elevatorDownSupplier = () -> modifyAxis(-jacob.getLeftTriggerAxis());
     DoubleSupplier elevatorUpSupplier = () -> modifyAxis(jacob.getLeftTriggerAxis());
 
-    new Trigger(() -> elevatorDownSupplier.getAsDouble()<-0.07)
+    new Trigger(() -> elevatorDownSupplier.getAsDouble() < -0.07)
         .onTrue(new ManualElevatorCommand(elevatorDownSupplier, elevatorSubsystem));
 
-    new Trigger(() -> elevatorUpSupplier.getAsDouble()>0.07)
+    new Trigger(() -> elevatorUpSupplier.getAsDouble() > 0.07)
         .onTrue(new ManualElevatorCommand(elevatorUpSupplier, elevatorSubsystem));
 
     // SOURCE
@@ -403,15 +404,9 @@ private void configureButtonBindings() {
                         intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
-    anthony
-        .b()
-        .onTrue(
-            new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+    anthony.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
-    jacob
-        .b()
-        .onTrue(
-            new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
+    jacob.b().onTrue(new LoadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     jacob
         .a()
         .onTrue(
@@ -419,7 +414,9 @@ private void configureButtonBindings() {
                     drivebaseSubsystem,
                     translationXSupplier,
                     translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
+                    DriverStation.getAlliance().get().equals(Alliance.Red)
+                        ? -50
+                        : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
                         pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem)));
@@ -439,29 +436,23 @@ private void configureButtonBindings() {
                     drivebaseSubsystem,
                     translationXSupplier,
                     translationYSupplier,
-                    DriverStation.getAlliance().get().equals(Alliance.Red) ? -50 : 50) // FIXME confirm angles
+                    DriverStation.getAlliance().get().equals(Alliance.Red)
+                        ? -50
+                        : 50) // FIXME confirm angles
                 .alongWith(
                     new AmpPreparationCommand(
                         pivotSubsystem, elevatorSubsystem, shooterSubsystem, intakeSubsystem)));
-    
-    //PIVOT SETPOINTS
-    anthony
-        .povUp().onTrue(
-            new PivotAngleCommand(pivotSubsystem, 30));
 
-    anthony
-        .povLeft().onTrue(
-            new PivotAngleCommand(pivotSubsystem, 60));
+    // PIVOT SETPOINTS
+    anthony.povUp().onTrue(new PivotAngleCommand(pivotSubsystem, 30));
 
-    anthony
-        .povRight().onTrue(
-            new PivotAngleCommand(pivotSubsystem, 75));
+    anthony.povLeft().onTrue(new PivotAngleCommand(pivotSubsystem, 60));
 
-    anthony
-        .povDown().onTrue(
-            new PivotAngleCommand(pivotSubsystem, 55));
-    
-    //TRAP CLIMB STUFF
+    anthony.povRight().onTrue(new PivotAngleCommand(pivotSubsystem, 75));
+
+    anthony.povDown().onTrue(new PivotAngleCommand(pivotSubsystem, 55));
+
+    // TRAP CLIMB STUFF
     // jacob.
     //     povCenter().onTrue();
 
diff --git a/src/main/java/frc/robot/commands/AmpPreparationCommand.java b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
index dd55d69..6fda043 100644
--- a/src/main/java/frc/robot/commands/AmpPreparationCommand.java
+++ b/src/main/java/frc/robot/commands/AmpPreparationCommand.java
@@ -23,9 +23,10 @@ public AmpPreparationCommand(
     } else {
       addCommands(
           new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
-      .andThen(new IntakeCommand(
-        intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))
-      .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
+              .andThen(
+                  new IntakeCommand(
+                      intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem))
+              .andThen(new ElevatorHeightCommand(elevatorSubsystem, Elevator.AMP_HEIGHT)));
     }
   }
 }
diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java
index ca99496..3176e01 100644
--- a/src/main/java/frc/robot/commands/IntakeCommand.java
+++ b/src/main/java/frc/robot/commands/IntakeCommand.java
@@ -43,7 +43,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(elevatorSubsystem.atTargetHeight()){
+    if (elevatorSubsystem.atTargetHeight()) {
       intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
       shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
     }
diff --git a/src/main/java/frc/robot/commands/LoadShooterCommand.java b/src/main/java/frc/robot/commands/LoadShooterCommand.java
index a02b248..23b73d8 100644
--- a/src/main/java/frc/robot/commands/LoadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/LoadShooterCommand.java
@@ -20,7 +20,6 @@ public class LoadShooterCommand extends Command {
   PivotSubsystem pivotSubsystem;
 
   /** Use PivotAndElevatorTransferCommand before running */
-
   public LoadShooterCommand(
       ShooterSubsystem shooterSubsystem,
       PivotSubsystem pivotSubsystem,
diff --git a/src/main/java/frc/robot/commands/ManualElevatorCommand.java b/src/main/java/frc/robot/commands/ManualElevatorCommand.java
index c8dbe67..8f61e17 100644
--- a/src/main/java/frc/robot/commands/ManualElevatorCommand.java
+++ b/src/main/java/frc/robot/commands/ManualElevatorCommand.java
@@ -4,17 +4,18 @@
 
 package frc.robot.commands;
 
-import java.util.function.DoubleSupplier;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ElevatorSubsystem;
+import java.util.function.DoubleSupplier;
 
 public class ManualElevatorCommand extends Command {
   /** Creates a new ManualElevatorCommand. */
   DoubleSupplier directionSupplier;
+
   ElevatorSubsystem elevatorSubsystem;
+
   public ManualElevatorCommand(
-    DoubleSupplier directionSupplier, ElevatorSubsystem elevatorSubsystem) {
+      DoubleSupplier directionSupplier, ElevatorSubsystem elevatorSubsystem) {
     this.directionSupplier = directionSupplier;
     this.elevatorSubsystem = elevatorSubsystem;
     addRequirements(elevatorSubsystem);
@@ -29,9 +30,7 @@ public void initialize() {}
   @Override
   public void execute() {
     elevatorSubsystem.setTargetHeight(
-      elevatorSubsystem.getElevatorPosition()
-      +directionSupplier.getAsDouble());
-  
+        elevatorSubsystem.getElevatorPosition() + directionSupplier.getAsDouble());
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/UnloadShooterCommand.java b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
index 9c95f2f..92a11ed 100644
--- a/src/main/java/frc/robot/commands/UnloadShooterCommand.java
+++ b/src/main/java/frc/robot/commands/UnloadShooterCommand.java
@@ -43,7 +43,7 @@ public void execute() {
     if (elevatorSubsystem.atTargetHeight() && pivotSubsystem.atTargetDegrees()) {
       shooterSubsystem.setShooterMode(ShooterMode.SHOOTER_UNLOAD);
     }
-      if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) {
+    if (shooterSubsystem.isSerializerBeamBreakSensorTriggered() && pass == false) {
       pass = true;
     }
   }