diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2cd8285..3745b75 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,14 +103,14 @@ public Command createTeleopInitSequence() { m_arm.createStowCommand(), m_arm.createFlapDeployCommand(), m_LEDs.createEnabledCommand( - m_intake.eitherSensorSupplier(), m_arm.stateChecker(ArmState.DEPLOYED))); + m_intake.hasNote, m_arm.stateChecker(ArmState.DEPLOYED))); } public Command createAutonomousInitSequence() { return new SequentialCommandGroup( m_arm.createFlapDeployCommand(), m_LEDs.createEnabledCommand( - m_intake.eitherSensorSupplier(), m_arm.stateChecker(ArmState.DEPLOYED))); + m_intake.hasNote, m_arm.stateChecker(ArmState.DEPLOYED))); } public Command createDisabledInitSequence() { @@ -263,15 +263,13 @@ private void configureDriverButtonBindings() { new JoystickButton(m_driver,OIConstants.kZorroAIn) .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver))); - - Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn); // Reverse intake to outake or reject intaking Note - D_Button.and(armDeployed.negate()) + D_Button.and(m_arm.deployed.negate()) .whileTrue(m_intake.createOuttakeToFloorCommand()); // Shoot Note into Amp - D_Button.and(armDeployed) + D_Button.and(m_arm.deployed) .whileTrue(m_intake.createOuttakeToAmpCommand()); } // spotless:on @@ -281,9 +279,6 @@ private void configureOperatorButtonBindings() { JoystickButton rightBumper = new JoystickButton(m_operator, Button.kRightBumper.value); JoystickButton leftBumper = new JoystickButton(m_operator, Button.kLeftBumper.value); - Trigger hasNote = new Trigger(m_intake.eitherSensorSupplier()); - Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED)); - // CLIMBER // Calibrate upper limit of climber actuators new JoystickButton(m_operator, Button.kStart.value).onTrue(new CalibrateCommand(m_climber) @@ -307,25 +302,25 @@ private void configureOperatorButtonBindings() { // Control position of Note in intake Trigger leftStick = new Trigger(() -> Math.abs(m_operator.getLeftY()) > 0.2); //While arm is down - leftStick.and(armDeployed.negate()).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); + leftStick.and(m_arm.deployed.negate()).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmDown)); //While arm is up - leftStick.and(armDeployed).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); + leftStick.and(m_arm.deployed).whileTrue(m_intake.createJoystickControlCommand(m_operator, IntakeConstants.kRepositionSpeedArmUp)); // Intake Note from floor - rightBumper.and(hasNote.negate()) + rightBumper.and(m_intake.hasNote.negate()) .whileTrue(m_arm.createStowCommand() .andThen(m_intake.createIntakeCommand())); - hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) + m_intake.hasNote.and(m_intake.stateChecker(IntakeState.INTAKING)).and(() -> RobotState.isTeleop()) .onTrue(m_arm.createCarryCommand() .andThen(m_intake.createAdvanceAfterIntakingCommand())); // Reverse intake to outake or reject intaking Note - leftBumper.and(armDeployed.negate()) + leftBumper.and(m_arm.deployed.negate()) .whileTrue(m_intake.createOuttakeToFloorCommand()); // Shoot Note into Amp - leftBumper.and(armDeployed) + leftBumper.and(m_arm.deployed) .whileTrue(m_intake.createOuttakeToAmpCommand()); // Shift Note further into Intake @@ -366,7 +361,7 @@ private void configureOperatorButtonBindings() { public Command createAutoIntakeCommandSequence() { return new SequentialCommandGroup( m_arm.createStowCommand(), - m_intake.createIntakeCommand().until(m_intake.eitherSensorSupplier()), + m_intake.createIntakeCommand().until(m_intake.hasNote), m_arm.createCarryCommand(), m_intake.createAdvanceAfterIntakingCommand()); } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index b8e44bd..67f3dbc 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.ArmConstants; import frc.robot.Constants.ArmConstants.ArmState; import java.util.function.BooleanSupplier; @@ -39,6 +40,8 @@ public void periodic() { if (m_armState != null) SmartDashboard.putString("Arm State", m_armState.name()); } + public final Trigger deployed = new Trigger(this.stateChecker(ArmState.DEPLOYED)); + public BooleanSupplier stateChecker(ArmState state) { return () -> { if (this.m_armState != null) return this.m_armState.equals(state); diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index a4948ff..2287f37 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.IntakeConstants; import frc.robot.Constants.IntakeConstants.IntakeState; import java.util.function.BooleanSupplier; @@ -43,7 +44,7 @@ public class Intake extends SubsystemBase { private final EventLoop m_loop = new EventLoop(); private final BooleanEvent m_secondSensorTriggered = - new BooleanEvent(m_loop, secondSensorSupplier()); + new BooleanEvent(m_loop, m_noteSensorRetroReflective::get).negate().rising(); public Intake() { @@ -94,7 +95,7 @@ private void configurePositionController(double targetPosition) { // spotless:off private void advanceAfterIntaking(double targetPosition) { - m_secondSensorTriggered.rising().ifHigh( + m_secondSensorTriggered.ifHigh( () -> { m_positionController.reset( m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); @@ -138,22 +139,13 @@ public Command createAdvanceAfterIntakingCommand() { // end interrupted -> {}, // isFinished - this.atGoalSupplier(), + m_positionController::atGoal, // requirements this); } - public BooleanSupplier eitherSensorSupplier() { - return () -> (!m_noteSensorRetroReflective.get() || !m_noteSensorBeamBreak.get()); - } - - public BooleanSupplier secondSensorSupplier() { - return () -> !m_noteSensorRetroReflective.get(); - } - - public BooleanSupplier atGoalSupplier() { - return () -> m_positionController.atGoal(); - } + public final Trigger hasNote = + new Trigger(() -> (!m_noteSensorRetroReflective.get() || !m_noteSensorBeamBreak.get())); private void setVelocity(double targetVelocity) { m_velocityController.setReference(targetVelocity, ControlType.kVelocity);