leds su

RegisJesuitRobotics · Mar 6, 2024 · 979b5f3 · 979b5f3
1 parent 673c931
commit 979b5f3
Show file tree

Hide file tree

Showing 5 changed files with 59 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -316,6 +316,7 @@ public static class LEDConstants {
     public static final int PWM_PORT = 0;
     public static final int FRONT_LEFT_SIZE = 12;
     public static final int FRONT_RIGHT_SIZE = 12;
+    public static final int STATUS_DEDICATED_SIZE = 2;
     public static final int BACK_LEFT_SIZE = 14;
     public static final int BACK_RIGHT_SIZE = 14;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -36,6 +36,7 @@
 import frc.robot.utils.led.SlidePattern;
 import frc.robot.utils.led.SolidPattern;
 import java.util.List;
+import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.function.DoubleSupplier;
 
 /**
@@ -75,6 +76,8 @@ public class RobotContainer {
   private final ListenableSendableChooser<Command> driveCommandChooser =
       new ListenableSendableChooser<>();
 
+  private final AtomicBoolean signalHumanPlayer = new AtomicBoolean(false);
+
   public RobotContainer() {
     configureDriverBindings();
     configureOperatorBindings();
@@ -87,16 +90,44 @@ public RobotContainer() {
   }
 
   private void configureLEDs() {
+    // Default state for homing lights
+    ledSubsystem.setStatusLight(0, Color.kRed);
+    ledSubsystem.setStatusLight(1, Color.kRed);
+
+    // Set homing lights to state
+    new Trigger(slapdownSuperstructure.getSlapdownRotationSubsystem()::isHomed).onFalse(
+        Commands.runOnce(() -> {
+          ledSubsystem.setStatusLight(0, Color.kRed);
+        }).ignoringDisable(true).withName("SlapdownLEDStatusFalse")
+    ).onTrue(
+        Commands.runOnce(() -> {
+          ledSubsystem.setStatusLight(0, Color.kGreen);
+        }).ignoringDisable(true).withName("SlapdownLEDStatusTrue")
+    );
+    new Trigger(elevatorSubsystem::isHomed).onFalse(
+        Commands.runOnce(() -> {
+          ledSubsystem.setStatusLight(1, Color.kRed);
+        }).ignoringDisable(true).withName("ElevatorLEDStatusFalse")
+    ).onTrue(
+        Commands.runOnce(() -> {
+          ledSubsystem.setStatusLight(1, Color.kGreen);
+        }).ignoringDisable(true).withName("ElevatorLEDStatusTrue")
+    );
+
     List<LEDState> ledStates =
         List.of(
+            new LEDState(
+                signalHumanPlayer::get,
+                new AlternatePattern(0.5, Color.kOrange, Color.kBlack)),
             // Red blink if we have any faults
             new LEDState(
                 () -> Alert.getDefaultGroup().hasAnyErrors(),
                 new AlternatePattern(2.0, Color.kRed, Color.kBlack)),
+            // Green if we can go under the stage
             new LEDState(
                 () ->
                     DriverStation.isTeleopEnabled()
-                        && elevatorSubsystem.atBottomLimit()
+                        && elevatorSubsystem.atBottom()
                         && wristSubsystem.atBottom(),
                 new SolidPattern(Color.kGreen)),
             // Default disabled pattern
@@ -129,9 +160,14 @@ private void configureDriverBindings() {
         .whileTrue(
             Commands.parallel(
                 IntakingCommands.intakeUntilDetected(
-                    intakeSubsystem, slapdownSuperstructure, transportSubsystem)));
-    driverController.rightTrigger().onFalse(slapdownSuperstructure.setUpCommand());
+                    intakeSubsystem, slapdownSuperstructure, transportSubsystem))).onFalse(
+            slapdownSuperstructure.setUpCommand()
+        );
     driverController.circle().whileTrue(new LockModulesCommand(driveSubsystem).repeatedly());
+
+    driverController.a().onTrue(Commands.runOnce(() -> signalHumanPlayer.set(true))).onFalse(
+        Commands.sequence(Commands.waitSeconds(1.5), Commands.runOnce(() -> signalHumanPlayer.set(false)))
+    );
   }
 
   private void configureOperatorBindings() {
@@ -140,7 +176,7 @@ private void configureOperatorBindings() {
         .onTrue(
             Commands.parallel(
                 ScoringCommands.shootSetpointAmpCommand(shooterSubsystem),
-                transportSubsystem.setVoltageCommand(12)));
+                transportSubsystem.setVoltageCommand(10)));
     operatorController
         .triangle()
         .onTrue(

diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -195,6 +195,10 @@ public boolean atGoal() {
     return controller.atGoal();
   }
 
+  public boolean atBottom() {
+    return controller.getGoal().position == ELEVATOR_MIN_HEIGHT && atGoal();
+  }
+
   public Command setFollowerCommand(double voltage) {
     return this.run(() -> elevatorMotorFollower.setVoltage(voltage));
   }

diff --git a/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/led/LEDSubsystem.java
@@ -17,6 +17,7 @@ public class LEDSubsystem extends SubsystemBase {
   private final ParentAddressableLEDBuffer ledBuffer = new ParentAddressableLEDBuffer(TOTAL_SIZE);
   private final RaiderAddressableLEDBuffer[] splitBuffers;
   private final Pattern[] patterns;
+  private final RaiderAddressableLEDBuffer statusDedicatedBuffer;
   private double patternStartTime = 0.0;
 
   public LEDSubsystem() {
@@ -29,21 +30,24 @@ public LEDSubsystem() {
     }
     i += FRONT_LEFT_SIZE;
 
-    splitBuffers[1] = ledBuffer.split(i, i + BACK_LEFT_SIZE);
-    splitBuffers[1] = splitBuffers[1].reversed();
+    int effectiveBackLeftSize = BACK_LEFT_SIZE - STATUS_DEDICATED_SIZE;
+    // Pre-concatenate a buffer for the status dedicated LEDs so it appears the saem as the rest
+    splitBuffers[1] = ledBuffer.split(i, i + effectiveBackLeftSize).reversed().preConcatenate(new FakeLEDBuffer(STATUS_DEDICATED_SIZE));
     if (BACK_LEFT_SIZE < MAX_SIZE) {
       splitBuffers[1] = splitBuffers[1].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_LEFT_SIZE));
     }
-    i += BACK_LEFT_SIZE;
+    i += effectiveBackLeftSize;
+
+    statusDedicatedBuffer = ledBuffer.split(i, i + STATUS_DEDICATED_SIZE);
+    i += STATUS_DEDICATED_SIZE;
 
     splitBuffers[2] = ledBuffer.split(i, i + BACK_RIGHT_SIZE);
     if (BACK_RIGHT_SIZE < MAX_SIZE) {
       splitBuffers[2] = splitBuffers[2].concatenate(new FakeLEDBuffer(MAX_SIZE - BACK_RIGHT_SIZE));
     }
     i += BACK_RIGHT_SIZE;
 
-    splitBuffers[3] = ledBuffer.split(i, i + FRONT_RIGHT_SIZE);
-    splitBuffers[3] = splitBuffers[3].reversed();
+    splitBuffers[3] = ledBuffer.split(i, i + FRONT_RIGHT_SIZE).reversed();
     if (FRONT_RIGHT_SIZE < MAX_SIZE) {
       splitBuffers[3] = splitBuffers[3].concatenate(new FakeLEDBuffer(MAX_SIZE - FRONT_RIGHT_SIZE));
     }
@@ -59,6 +63,10 @@ public LEDSubsystem() {
     led.start();
   }
 
+  public void setStatusLight(int i, Color color) {
+    statusDedicatedBuffer.setLED(i, color);
+  }
+
   public void setAllPattern(Pattern pattern, boolean forceRestart) {
     setPatterns(forceRestart, pattern, pattern, pattern, pattern);
   }

diff --git a/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java b/src/main/java/frc/robot/subsystems/slapdown/SlapdownRotationSubsystem.java
@@ -146,7 +146,7 @@ private boolean atLimit() {
     return !rotationLimitSwitch.get();
   }
 
-  private boolean isHomed() {
+  public boolean isHomed() {
     return isHomed;
   }