replace select command

Frc5572 · Sep 27, 2024 · 8d93ccd · 8d93ccd
1 parent cb2ecfb
commit 8d93ccd
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 27 deletions.
diff --git 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
@@ -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<OperatorState.State>(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
@@ -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",