diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c1e16f7..3738558 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -326,6 +326,9 @@ private void configureBindings() { // When the shooter isnt shooting, get it ready to shoot. shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose)); + + //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it. + gamepad_.a().whileTrue(intake_.ejectSequence()); } private void configureDriveBindings() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f2d74d3..8f639b1 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -52,6 +52,7 @@ public final class IntakeConstants { public static final AngularVelocity rollerMaxVelocity= DegreesPerSecond.of(360); //Temporary speed to be changed as needed public static final Voltage rollerCollectVoltage= Volts.of(6); //Temporary voltage + public static final Voltage rollerEjectVoltage= Volts.of(-6); //Temporary voltage public static final Angle pivotTolerance = Degrees.of(5); //Tolerance to compare current angle to target diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index d7f18df..2b3099c 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -69,6 +69,10 @@ private void stopIntaking() { io.stopRoller(); } + private void eject(){ + io.setRollerVoltage(IntakeConstants.rollerEjectVoltage); + } + /** * Stows the intake and hopper. */ @@ -150,6 +154,16 @@ public Command intakeSequence() { ).finallyDo(interrupted -> waiting()); } + public Command runEjectCmd() { + return startEnd(this::eject, this::stopIntaking); + } + + public Command ejectSequence() { + return runEjectCmd().beforeStarting( + deployCmd().unless(this::isIntakeDeployed) + ).finallyDo(interrupted -> waiting()); + } + //////////////////////////// /// Sys ID Routine creation/ /// ////////////////////////