diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13e454ce..1432faf2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -48,6 +48,7 @@ import frc.robot.commands.intake.roller.IntakeRollerAmpIntakeCommand; import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand; +import frc.robot.commands.sequences.PrepareAmpSequence; import frc.robot.commands.shooter.flywheel.ShooterFlywheelShuttleCommand; import frc.robot.commands.swerve.AlignCommand; import frc.robot.commands.swerve.AutoIntakeSequence; @@ -418,20 +419,12 @@ 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 SequentialCommandGroup( - new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).unless(() -> !intakeRollerSubsystem.getRockwellSensorValue()).andThen(// extend pivot - new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75).unless(intakeRollerSubsystem::getAmpSensor) // run rollers to front sensor - .until(() -> intakeRollerSubsystem.getFrontSensorReached()), - new IntakePivotSetPositionCommand(intakePivotSubsystem, 0.2), - new ElevatorToAmpCommand(elevatorSubsystem) - ) - - // raise elevator - // angle intake for scoring - ).until(() -> mechController.getLeftTriggerAxis() > .05 - || mechController.getRightTriggerAxis() > .05 - ), + new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem) + .until(() -> mechController.getLeftTriggerAxis() > .05 + || mechController.getRightTriggerAxis() > .05), + // check if the elevator is currently targeting one of the upper positions to choose what to do () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP || elevatorSubsystem.getTargetState() == ElevatorState.TRAP)); diff --git a/src/main/java/frc/robot/commands/sequences/PrepareAmpSequence.java b/src/main/java/frc/robot/commands/sequences/PrepareAmpSequence.java new file mode 100644 index 00000000..d2c594d2 --- /dev/null +++ b/src/main/java/frc/robot/commands/sequences/PrepareAmpSequence.java @@ -0,0 +1,36 @@ +package frc.robot.commands.sequences; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.commands.elevator.ElevatorToAmpCommand; +import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand; +import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand; +import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.intake.IntakePivotSubsystem; +import frc.robot.subsystems.intake.IntakeRollerSubsystem; + +/** + * Deploys the intake, rolls the note to its amping position, then raises the elevator to amp height while putting the + * intake at amping angle. Given that a note is already in the robot, this sequence prepares the mechanisms to deposit + * it into the amp. + */ +public class PrepareAmpSequence extends SequentialCommandGroup { + + /** Constructs a new {@link PrepareAmpSequence}. */ + public PrepareAmpSequence(ElevatorSubsystem elevatorSubsystem, + IntakePivotSubsystem intakePivotSubsystem, + IntakeRollerSubsystem intakeRollerSubsystem) { + + this.addCommands( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) // extend pivot + .unless(() -> !intakeRollerSubsystem.getRockwellSensorValue()), + new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor + .unless(intakeRollerSubsystem::getAmpSensor) + .until(() -> intakeRollerSubsystem.getFrontSensorReached()), + Commands.parallel( + new IntakePivotSetPositionCommand(intakePivotSubsystem, 0.2), // angle intake for scoring + new ElevatorToAmpCommand(elevatorSubsystem) // raise elevator + ) // These commands can run at the same time to save time. + ); + } +} \ No newline at end of file