diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e5bbb6..a38d0fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,14 +62,8 @@ public class RobotContainer { m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); m_driverController.povUp().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE2)); - // m_driverController.x().onTrue(intakeSubsystem.killSpeedCommand()); - // m_driverController.a().onTrue(intakeSubsystem.startSpeedCommand()); - - - - m_driverController.b().whileTrue(turretSubsystem.setShooter(1500)); - m_driverController.x().whileTrue(turretSubsystem.setShooter(2000)); - m_driverController.y().whileTrue(turretSubsystem.setShooter(3000)); + m_driverController.rightBumper().whileTrue(intakeSubsystem.intake()); + m_driverController.y().onTrue(intakeSubsystem.deploy()); m_driverController.a().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); } diff --git a/src/main/java/frc/robot/commands/ManualIntake.java b/src/main/java/frc/robot/commands/ManualIntake.java index 39c9aa4..95096a8 100644 --- a/src/main/java/frc/robot/commands/ManualIntake.java +++ b/src/main/java/frc/robot/commands/ManualIntake.java @@ -36,7 +36,7 @@ public class ManualIntake extends Command { public void execute() { if(m_XboxController.getYButton()==true) { - m_subsystem.extend(); + m_subsystem.deploy(); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index a200006..04122df 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -17,6 +17,7 @@ import static org.fairportrobotics.frc.posty.assertions.Assertions.*; import static org.fairportrobotics.frc.robolib.motors.Utils.*; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; public class IntakeSubsystem extends TestableSubsystem { /** Creates a new ExampleSubsystem. */ @@ -35,9 +36,24 @@ public class IntakeSubsystem extends TestableSubsystem { deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); } - public void extend() + public Command deploy() { - deployMotor.set(1); + return Commands.sequence( + Commands.deadline( + Commands.waitSeconds(2), + this.runEnd( + () -> {deployMotor.set(-0.5);}, + () -> {deployMotor.stopMotor();} + ) + ), + Commands.deadline( + Commands.waitSeconds(2), + this.runEnd( + () -> { deployMotor.set(0.5); }, + () -> { deployMotor.stopMotor(); } + ) + ) + ); } public void setSpeed(double speed) @@ -45,19 +61,14 @@ public class IntakeSubsystem extends TestableSubsystem { intakeMotor.set(speed); } - public Command startSpeedCommand() + public Command intake() { - return this.runOnce(() -> setSpeed(-.5)); + return this.runEnd(() -> setSpeed(-.5), () -> { intakeMotor.stopMotor();}); } - public Command killSpeedCommand() + public Command reverseIntake() { - return this.runOnce(() -> setSpeed(0)); - } - - public Command revSpeedCommand() - { - return this.runOnce(() -> setSpeed(.5)); + return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();}); } @PostTest(enabled = true)