Manual overrides (#76)

HighlanderRobotics · Apr 8, 2024 · 6b43ea3 · 6b43ea3
1 parent 7b9bea0
commit 6b43ea3
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -70,7 +70,13 @@ public static enum RobotMode {
 
   public static enum Target {
     AMP,
-    SPEAKER
+    SPEAKER,
+    FEED,
+    SUBWOOFER;
+
+    public boolean isSpeakerAlike() {
+      return this == Target.SPEAKER || this == Target.FEED || this == Target.SUBWOOFER;
+    }
   }
 
   public static final RobotMode mode = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM;
@@ -187,7 +193,7 @@ public void robotInit() {
     elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0));
     feeder.setDefaultCommand(
         Commands.repeatingSequence(
-            feeder.indexCmd().until(() -> currentTarget != Target.SPEAKER),
+            feeder.indexCmd().until(() -> !currentTarget.isSpeakerAlike()),
             Commands.sequence(
                     feeder
                         .runVelocityCmd(-FeederSubsystem.INDEXING_VELOCITY)
@@ -208,12 +214,11 @@ public void robotInit() {
                         .until(() -> feeder.getFirstBeambreak()),
                     carriage.runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE).withTimeout(0.5),
                     carriage.runVoltageCmd(-0.5).until(() -> !feeder.getFirstBeambreak()))
-                .until(() -> currentTarget != Target.SPEAKER)));
+                .until(() -> !currentTarget.isSpeakerAlike())));
     intake.setDefaultCommand(intake.runVoltageCmd(0.0, 0.0));
     shooter.setDefaultCommand(shooter.runFlywheelsCmd(() -> 0.0, () -> 0.0));
     leds.setDefaultCommand(
-        leds.defaultStateDisplayCmd(
-            () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER));
+        leds.defaultStateDisplayCmd(() -> DriverStation.isEnabled(), () -> currentTarget));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
@@ -258,17 +263,37 @@ public void robotInit() {
         .whileTrue(staticAutoAim());
     controller
         .rightTrigger()
-        .and(() -> currentTarget == Target.SPEAKER)
-        .and(operator.b())
+        .and(() -> currentTarget == Target.FEED)
         .whileTrue(
-            shooter.runStateCmd(
-                AutoAim.FEED_SHOT.getRotation(),
-                AutoAim.FEED_SHOT.getLeftRPS(),
-                AutoAim.FEED_SHOT.getRightRPS()))
+            Commands.parallel(
+                Commands.waitUntil(() -> shooter.isAtGoal())
+                    .andThen(controller.rumbleCmd(1.0, 1.0).withTimeout(0.25))
+                    .asProxy(),
+                shooter.runStateCmd(
+                    AutoAim.FEED_SHOT.getRotation(),
+                    AutoAim.FEED_SHOT.getLeftRPS(),
+                    AutoAim.FEED_SHOT.getRightRPS())))
         .onFalse(
             Commands.parallel(
                     shooter.run(() -> {}), feeder.runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY))
                 .withTimeout(0.5));
+    controller
+        .rightTrigger()
+        .and(() -> currentTarget == Target.SUBWOOFER)
+        .whileTrue(
+            Commands.parallel(
+                Commands.waitUntil(() -> shooter.isAtGoal())
+                    .andThen(controller.rumbleCmd(1.0, 1.0).withTimeout(0.25))
+                    .asProxy(),
+                shooter.runStateCmd(
+                    AutoAim.FENDER_SHOT.getRotation(),
+                    AutoAim.FENDER_SHOT.getLeftRPS(),
+                    AutoAim.FENDER_SHOT.getRightRPS())))
+        .onFalse(
+            Commands.parallel(
+                    shooter.run(() -> {}), feeder.runVelocityCmd(FeederSubsystem.INDEXING_VELOCITY))
+                .withTimeout(0.5));
+
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.SPEAKER)
@@ -292,7 +317,7 @@ public void robotInit() {
     controller.leftBumper().whileTrue(swerve.stopWithXCmd());
     controller
         .rightBumper()
-        .and(() -> currentTarget == Target.SPEAKER)
+        .and(() -> currentTarget.isSpeakerAlike())
         .and(controller.rightTrigger().negate())
         .whileTrue(
             speakerHeadingSnap(
@@ -359,6 +384,8 @@ public void robotInit() {
                                     .getDegrees())
                             < 10.0)
                 .andThen(Commands.runOnce(() -> currentTarget = Target.AMP)));
+    operator.b().onTrue(Commands.runOnce(() -> currentTarget = Target.FEED));
+    operator.x().onTrue(Commands.runOnce(() -> currentTarget = Target.SUBWOOFER));
 
     operator
         .a()

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -12,6 +12,8 @@
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot.Target;
+import java.util.Map;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
@@ -88,14 +90,23 @@ public Command setRunAlongCmd(
         });
   }
 
-  public Command defaultStateDisplayCmd(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) {
+  public Command defaultStateDisplayCmd(BooleanSupplier enabled, Supplier<Target> target) {
     return Commands.either(
-            Commands.either(
-                this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0)
-                    .until(() -> !targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()),
-                this.setBlinkingCmd(new Color("#ff7777"), new Color(), 10.0)
-                    .until(() -> targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()),
-                targetIsSpeaker),
+            Commands.select(
+                Map.of(
+                    Target.SPEAKER,
+                    this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0)
+                        .until(() -> target.get() != Target.SPEAKER || !enabled.getAsBoolean()),
+                    Target.AMP,
+                    this.setBlinkingCmd(new Color("#ff7777"), new Color(), 10.0)
+                        .until(() -> target.get() != Target.AMP || !enabled.getAsBoolean()),
+                    Target.FEED,
+                    this.setBlinkingCmd(new Color("#0000ff"), new Color(), 10.0)
+                        .until(() -> target.get() != Target.FEED || !enabled.getAsBoolean()),
+                    Target.SUBWOOFER,
+                    this.setBlinkingCmd(new Color("#ff0000"), new Color(), 10.0)
+                        .until(() -> target.get() != Target.SUBWOOFER || !enabled.getAsBoolean())),
+                target),
             this.setRunAlongCmd(
                     // Set color to be purple with a moving dash corresponding to alliance color
                     () -> {