diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13e454ce..97c2f324 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -268,7 +268,7 @@ 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. + /* Amp Align -- Pressing and holding the button will cause the robot to automatically pathfind 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) @@ -279,6 +279,13 @@ private void configureBindings() { ).andThen(new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))) ); + /* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its + * climb hooks will end up directly above the center of the nearest chain. */ + driveController.getStageAlignButton().onTrue( + AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()) + .onlyWhile(driveController.getStageAlignButton()) + ); + /* Note align -- deprecated, new version in the works*/ driveController.getNoteAlign().onTrue( new NoteAlignCommand(swerveSubsystem, noteDetector, driveController) diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java index a6f5efea..f5899343 100644 --- a/src/main/java/frc/robot/controllers/BaseDriveController.java +++ b/src/main/java/frc/robot/controllers/BaseDriveController.java @@ -88,4 +88,7 @@ public abstract class BaseDriveController { * Gets the button to aim the shooter at the speaker. */ public abstract JoystickButton getShooterAimButton(); + + /** Returns the button to auto-align to the nearest stage face. */ + public abstract JoystickButton getStageAlignButton(); } diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java index 24bb713c..a8fe8b92 100644 --- a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java +++ b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java @@ -15,6 +15,7 @@ public class DualJoystickDriveController extends BaseDriveController { private final JoystickButton leftMiddleButton = new JoystickButton(leftJoystick, 2); private final JoystickButton leftTopLeftButton = new JoystickButton(leftJoystick, 3); private final JoystickButton leftTopRightButton = new JoystickButton(leftJoystick, 4); + private final JoystickButton leftBottomLeftButton = new JoystickButton(leftJoystick, 5); private final Joystick rightJoystick = new Joystick(1); private final JoystickButton rightTrigger = new JoystickButton(rightJoystick, 1); @@ -100,5 +101,10 @@ public Boolean getSwerveAimMode() { @Override public JoystickButton getShooterAimButton() { return leftTopRightButton; - } + } + + @Override + public JoystickButton getStageAlignButton() { + return leftBottomLeftButton; + } } diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java index db991758..a76e737e 100644 --- a/src/main/java/frc/robot/controllers/XboxDriveController.java +++ b/src/main/java/frc/robot/controllers/XboxDriveController.java @@ -21,6 +21,9 @@ public class XboxDriveController extends BaseDriveController { private final JoystickButton driveLStickButton = new JoystickButton( driveController, XboxController.Button.kLeftStick.value ); + private final JoystickButton driveRStickButton = new JoystickButton( + driveController, XboxController.Button.kRightStick.value + ); @Override public double getForwardPower() { @@ -77,7 +80,13 @@ public Boolean getSwerveAimMode() { return driveLStickButton.getAsBoolean(); } - public JoystickButton getShooterAimButton(){ + @Override + public JoystickButton getShooterAimButton() { return driveLStickButton; } + + @Override + public JoystickButton getStageAlignButton() { + return driveRStickButton; + } }