Skip to content

Commit

Permalink
add pivot lock and amp sounds super duper again
Browse files Browse the repository at this point in the history
  • Loading branch information
PatribotsProgramming committed Oct 11, 2024
1 parent c0febf7 commit 6e6fb2b
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 10 deletions.
10 changes: 3 additions & 7 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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));
Expand All @@ -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())
Expand Down
26 changes: 23 additions & 3 deletions src/main/java/frc/robot/commands/managers/PieceControl.java
Original file line number Diff line number Diff line change
Expand Up @@ -278,7 +278,7 @@ public Command ejectNote() {

public Command stopEjecting() {
return Commands.parallel(
elevator.toBottomCommand(),
elevatorToBottomSafe(),
stopAllMotors()
);
}
Expand Down Expand Up @@ -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));
}
Expand Down Expand Up @@ -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);
}
}
12 changes: 12 additions & 0 deletions src/main/java/frc/robot/commands/managers/ShooterCmds.java
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

}
20 changes: 20 additions & 0 deletions src/main/java/frc/robot/subsystems/shooter/Pivot.java
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down

0 comments on commit 6e6fb2b

Please sign in to comment.