diff --git a/src/main/java/frc/robot/OperatorState.java b/src/main/java/frc/robot/OperatorState.java index dc361406..628ab911 100755 --- a/src/main/java/frc/robot/OperatorState.java +++ b/src/main/java/frc/robot/OperatorState.java @@ -43,6 +43,10 @@ public State decrement() { public static Trigger isManualMode = new Trigger(() -> manualModeEnabled()); public static Trigger isAmpMode = new Trigger(() -> getCurrentState() == State.kAmp); public static Trigger isSpeakerMode = new Trigger(() -> getCurrentState() == State.kSpeaker); + public static Trigger isShootWhileMoveMode = + new Trigger(() -> getCurrentState() == State.kShootWhileMove); + public static Trigger isPostMode = new Trigger(() -> getCurrentState() == State.kPost); + public static Trigger isClimbMode = new Trigger(() -> getCurrentState() == State.kClimb); /** Get whether or not operator should be able to control wrist and elevator manually. */ public static boolean manualModeEnabled() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 293481d8..dbb833cb 100755 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; @@ -246,39 +245,27 @@ private void configureButtonBindings() { OperatorState.decrement(); }).ignoringDisable(true)); // run action based on current state as incremented through operator states list - operator.a().whileTrue(new SelectCommand(Map.of( - // - OperatorState.State.kSpeaker, - elevatorWrist.speakerPreset() - .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop)), - // - OperatorState.State.kAmp, - Commands.either(elevatorWrist.ampPosition(), Commands.none(), this.intake.noteInIndexer) - .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop)), - // - OperatorState.State.kShootWhileMove, - new ShootWhileMoving(s_Swerve, driver, () -> s_Swerve.getPose(), + operator.a().and(OperatorState.isSpeakerMode).whileTrue(elevatorWrist.speakerPreset()); + operator.a().and(OperatorState.isAmpMode).whileTrue(Commands + .either(elevatorWrist.ampPosition(), Commands.none(), this.intake.noteInIndexer)); + operator.a().and(OperatorState.isShootWhileMoveMode).and(s_Swerve.seeAprilTag) + .whileTrue(new ShootWhileMoving(s_Swerve, driver, () -> s_Swerve.getPose(), () -> FieldConstants.allianceFlip(FieldConstants.Speaker.centerSpeakerOpening) .getTranslation()) .alongWith(elevatorWrist.followPosition( () -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, () -> elevatorWrist.getAngleFromDistance(s_Swerve.getPose()) - .plus(Rotation2d.fromDegrees(0.0)))), - // - OperatorState.State.kPost, - new TurnToAngle(s_Swerve, Rotation2d.fromDegrees(25)).alongWith(elevatorWrist + .plus(Rotation2d.fromDegrees(0.0))))); + operator.a().and(OperatorState.isPostMode) + .whileTrue(new TurnToAngle(s_Swerve, Rotation2d.fromDegrees(25)).alongWith(elevatorWrist .followPosition(() -> Constants.ElevatorWristConstants.SetPoints.HOME_HEIGHT, - () -> Constants.ElevatorWristConstants.SetPoints.PODIUM_ANGLE)), - // - OperatorState.State.kClimb, - Commands + () -> Constants.ElevatorWristConstants.SetPoints.PODIUM_ANGLE))); + operator.a().and(OperatorState.isClimbMode) + .whileTrue(Commands .sequence(elevatorWrist.ampPosition(), Commands.runOnce(() -> OperatorState.enableManualMode())) .alongWith(new TeleopSwerve(s_Swerve, driver, Constants.Swerve.isFieldRelative, - Constants.Swerve.isOpenLoop))), - OperatorState::getCurrentState)); + Constants.Swerve.isOpenLoop))); // Toggle manual mode operator.start().onTrue(Commands.runOnce(() -> { diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index cf8f1c2c..11c65ad2 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.FieldConstants; import frc.lib.util.photon.PhotonCameraWrapper; import frc.lib.util.swerve.SwerveModule; @@ -49,6 +50,9 @@ public class Swerve extends SubsystemBase { .withProperties(Map.of("Color when true", "green", "Color when false", "red")) .withPosition(11, 0).withSize(2, 2).getEntry(); + public Trigger seeAprilTag = + new Trigger(() -> Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); + /** * Swerve Subsystem */ @@ -307,8 +311,7 @@ public void periodic() { Robot.profiler.push("field"); field.setRobotPose(getPose()); Robot.profiler.swap("apriltag"); - aprilTagTarget - .setBoolean(Arrays.asList(cameraSeesTarget).stream().anyMatch(val -> val == true)); + aprilTagTarget.setBoolean(seeAprilTag.getAsBoolean()); Robot.profiler.swap("dist-to-speaker"); SmartDashboard.putNumber("Distance to Speaker",