From 0a863b033ddc5d11c72ded421b72fd7af1692d84 Mon Sep 17 00:00:00 2001
From: MysticalApple <timon.cappellapimor@gmail.com>
Date: Wed, 3 Apr 2024 01:19:53 -0700
Subject: [PATCH] replaced amp-align binding with auto-amp

---
 src/main/java/frc/robot/RobotContainer.java   | 35 +++++++++++++------
 .../controllers/BaseDriveController.java      |  4 +--
 .../DualJoystickDriveController.java          |  2 +-
 .../controllers/XboxDriveController.java      |  2 +-
 4 files changed, 29 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 1432faf2..71ae9088 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -38,6 +38,7 @@
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc.robot.Constants.SwerveConstants;
+import frc.robot.commands.auton.AutoAmpSequence;
 import frc.robot.commands.auton.AutonBuilder;
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.elevator.ElevatorToAmpCommand;
@@ -269,17 +270,31 @@ private void configureBindings() {
         /* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
         // drive
 
-        /* Amp Align -- Pressing and holding the button will cause the robot to automatically path find to the amp.
-         * Releasing the button will stop the robot (and the path finding). */
-        driveController.getAmpAlign().onTrue(new InstantCommand(
-            () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
-            ).andThen(new ParallelRaceGroup(
-                AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
-                new ConditionalWaitCommand(
-                    () -> !driveController.getAmpAlign().getAsBoolean()))
-                    ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
+        /* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the
+         * amp and deposit its note. Releasing the button will stop the robot (and the path finding). */
+        // TODO: Bring back LED integration.
+        driveController.getAutoAmp().onTrue(
+            new AutoAmpSequence(fmsSubsystem,
+                                swerveSubsystem, 
+                                elevatorSubsystem,
+                                intakePivotSubsystem,
+                                intakeRollerSubsystem)
+
+                .onlyWhile(driveController.getAutoAmp())
         );
 
+        // DEPRECATED AMP ALIGN
+        // driveController.getAmpAlign().onTrue(new InstantCommand(
+        //     () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
+        //     ).andThen(new ParallelRaceGroup(
+        //         AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
+        //         new ConditionalWaitCommand(
+        //             () -> !driveController.getAmpAlign().getAsBoolean()))
+        //      ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)))
+        // );
+
+
+
         /* Note align -- deprecated, new version in the works*/
         driveController.getNoteAlign().onTrue(
             new NoteAlignCommand(swerveSubsystem, noteDetector, driveController)
@@ -419,7 +434,7 @@ private void configureBindings() {
                     // if elevator is up
                     new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
                         () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot
-                        
+
                     // if elevator is down
                     new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
                         .until(() -> mechController.getLeftTriggerAxis() > .05 
diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java
index a6f5efea..b3b4c3d6 100644
--- a/src/main/java/frc/robot/controllers/BaseDriveController.java
+++ b/src/main/java/frc/robot/controllers/BaseDriveController.java
@@ -57,11 +57,11 @@ public abstract class BaseDriveController {
     public abstract Boolean getRelativeMode();
 
     /**
-     * Gets the button bound to auto-aligning to the amp.
+     * Gets the button bound to auto-align and -deposit into the amp.
      *
      * @return The JoystickButton to ampAlign.
      */
-    public abstract JoystickButton getAmpAlign();
+    public abstract JoystickButton getAutoAmp();
 
     /**
      * Gets the button bound to run the noteAlign sequence.
diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
index 24bb713c..2fc362da 100644
--- a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
+++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
@@ -78,7 +78,7 @@ public Boolean getRelativeMode() {
     }
 
     @Override
-    public JoystickButton getAmpAlign() {
+    public JoystickButton getAutoAmp() {
         return leftTopLeftButton;
     }
 
diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java
index db991758..f10d2179 100644
--- a/src/main/java/frc/robot/controllers/XboxDriveController.java
+++ b/src/main/java/frc/robot/controllers/XboxDriveController.java
@@ -58,7 +58,7 @@ public Boolean getRelativeMode() {
     }
 
     @Override
-    public JoystickButton getAmpAlign() {
+    public JoystickButton getAutoAmp() {
         return xButton;
     }