From 6e6fb2b8d9645f6d454acc6cc29bd489177b2b02 Mon Sep 17 00:00:00 2001 From: PatribotsProgramming Date: Thu, 10 Oct 2024 20:30:42 -0700 Subject: [PATCH] add pivot lock and amp sounds super duper again --- src/main/java/frc/robot/RobotContainer.java | 10 +++---- .../robot/commands/managers/PieceControl.java | 26 ++++++++++++++++--- .../robot/commands/managers/ShooterCmds.java | 12 +++++++++ .../frc/robot/subsystems/shooter/Pivot.java | 20 ++++++++++++++ 4 files changed, 58 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7eec7dd..e20473df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -439,7 +439,7 @@ private void configureCommonDriverBindings(PatriBoxController controller) { .onTrue(swerve.resetHDCCommand()) .toggleOnTrue( Commands.sequence( - elevator.toBottomCommand(), + pieceControl.elevatorToBottomSafe().unless(elevator::atDesiredPosition), limelight3g.setLEDState(() -> true), new ActiveConditionalCommand( // This runs SWD on heading control @@ -603,9 +603,7 @@ private void configureDriverBindings(PatriBoxController controller) { private void configureOperatorBindings(PatriBoxController controller) { controller.povUp() - .onTrue( - shooterCmds.stowPivot() - .andThen(pieceControl.elevatorToPlacement(false))); + .onTrue(pieceControl.elevatorToPlacement(false)); controller.povLeft() .onTrue(pieceControl.elevatorToPlacement(true)); @@ -614,9 +612,7 @@ private void configureOperatorBindings(PatriBoxController controller) { .onTrue(ampper.toggleSpeed()); controller.povDown() - .onTrue( - shooterCmds.stowPivot() - .andThen(pieceControl.elevatorToBottom())); + .onTrue(pieceControl.elevatorToBottomSafe()); controller.leftBumper() .whileTrue(pieceControl.intakeUntilNote()) diff --git a/src/main/java/frc/robot/commands/managers/PieceControl.java b/src/main/java/frc/robot/commands/managers/PieceControl.java index 84c91a75..44d88db2 100644 --- a/src/main/java/frc/robot/commands/managers/PieceControl.java +++ b/src/main/java/frc/robot/commands/managers/PieceControl.java @@ -278,7 +278,7 @@ public Command ejectNote() { public Command stopEjecting() { return Commands.parallel( - elevator.toBottomCommand(), + elevatorToBottomSafe(), stopAllMotors() ); } @@ -375,8 +375,8 @@ public Command elevatorToPlacement(boolean povLeftPosition) { Commands.sequence( stopIntakeAndIndexer(), Commands.either( - setElevatorPosition(() -> ElevatorConstants.NOTE_FIX_POS), - setElevatorPosition(() -> ElevatorConstants.TRAP_PLACE_POS), + elevatorToNoteFixSafe(), + elevatorToTopSafe(), () -> povLeftPosition), placeWhenReady().onlyIf(this::shouldPlaceWhenReady)); } @@ -525,4 +525,24 @@ public void restartDoubleAmpTimer() { public boolean doubleAmpTimerReady() { return doubleAmpTimer.get() > 0.35; } + + public Command setElevatorPositionSafe(DoubleSupplier position) { + return Commands.sequence( + shooterCmds.stowPivot(), + shooterCmds.lockPivot(), + elevator.setPositionCommand(position), + shooterCmds.unlockPivot()); + } + + public Command elevatorToTopSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.ELEVATOR_TOP_LIMIT); + } + + public Command elevatorToBottomSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.BOTTOM_POS); + } + + public Command elevatorToNoteFixSafe() { + return setElevatorPositionSafe(() -> ElevatorConstants.NOTE_FIX_POS); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/managers/ShooterCmds.java b/src/main/java/frc/robot/commands/managers/ShooterCmds.java index f4c61be4..e39a6d36 100644 --- a/src/main/java/frc/robot/commands/managers/ShooterCmds.java +++ b/src/main/java/frc/robot/commands/managers/ShooterCmds.java @@ -201,5 +201,17 @@ public Command stowPivot() { public Command raisePivot() { return pivot.setAngleCommand(ShooterConstants.PIVOT_RAISE_ANGLE_DEGREES); } + + public Command togglePivotLock() { + return pivot.togglePivotLockCommand(); + } + + public Command lockPivot() { + return pivot.setPivotLockCommand(true); + } + + public Command unlockPivot() { + return pivot.setPivotLockCommand(false); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/Pivot.java b/src/main/java/frc/robot/subsystems/shooter/Pivot.java index 6c348425..81bd8f76 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Pivot.java +++ b/src/main/java/frc/robot/subsystems/shooter/Pivot.java @@ -26,6 +26,9 @@ public class Pivot extends SubsystemBase implements PivotIO { @AutoLogOutput (key = "Subsystems/Pivot/AtDesiredPassAngle") private boolean atDesiredPassAngle = false; + + @AutoLogOutput (key = "Subsystems/Pivot/PivotLock") + private boolean pivotLock = false; public Pivot() { motor = new Neo( @@ -72,6 +75,11 @@ public void periodic() { * @param double The angle to set the shooter to */ public void setAngle(double angle) { + + if (pivotLock) { + return; + } + angle = MathUtil.clamp( angle, ShooterConstants.PIVOT_LOWER_LIMIT_DEGREES, @@ -121,6 +129,18 @@ public boolean getAtDesiredPassAngle() { return atDesiredPassAngle; } + public void setPivotLock(boolean lock) { + pivotLock = lock; + } + + public Command setPivotLockCommand(boolean lock) { + return runOnce(() -> setPivotLock(lock)); + } + + public Command togglePivotLockCommand() { + return runOnce(() -> setPivotLock(!pivotLock)); + } + @Override public void updateInputs(PivotIOInputs inputs) { inputs.targetPositionDegrees = this.getTargetAngle();