add led indicators for drivers (#72)

* unfinished * Automated commit at 10/21/2024 19:01:33 * Set vision working supplier for LEDs * Hopefully change nothing but format --------- Co-authored-by: minhnguyenbhs <[email protected]> Co-authored-by: avidraccoon <[email protected]>
team401 · Oct 26, 2024 · 3ed491f · 3ed491f
1 parent de170bb
commit 3ed491f
Show file tree

Hide file tree

Showing 5 changed files with 163 additions and 1 deletion.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -21,6 +21,7 @@
 import frc.robot.constants.PhoenixDriveConstants.AlignTarget;
 import frc.robot.constants.ScoringConstants;
 import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.LED;
 import frc.robot.subsystems.drive.PhoenixDrive;
 import frc.robot.subsystems.drive.PhoenixDrive.SysIdRoutineType;
 import frc.robot.subsystems.intake.IntakeIO;
@@ -53,6 +54,7 @@ public class RobotContainer {
 
     ScoringSubsystem scoringSubsystem;
     IntakeSubsystem intakeSubsystem;
+    LED leds;
 
     CommandJoystick leftJoystick = new CommandJoystick(0);
     CommandJoystick rightJoystick = new CommandJoystick(1);
@@ -115,6 +117,12 @@ private void configureSubsystems() {
         if (FeatureFlags.runIntake) {
             initIntake();
         }
+        if (FeatureFlags.runScoring
+                && FeatureFlags.runIntake
+                && FeatureFlags.runVision
+                && FeatureFlags.runLEDS) {
+            initLEDs();
+        }
     }
 
     private void initDrive() {
@@ -206,6 +214,10 @@ private void initScoring() {
         }
     }
 
+    private void initLEDs() {
+        leds = new LED(scoringSubsystem, intakeSubsystem);
+    }
+
     private void configureSuppliers() {
         if (FeatureFlags.runScoring) {
             Supplier<Pose2d> poseSupplier;
@@ -247,6 +259,14 @@ private void configureSuppliers() {
                 tagVision.setCameraConsumer((m) -> {});
             }
         }
+
+        if (FeatureFlags.runLEDS) {
+            if (FeatureFlags.runVision) {
+                leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected());
+            } else {
+                leds.setVisionWorkingSupplier(() -> false);
+            }
+        }
     }
 
     private void configureBindings() {

diff --git a/src/main/java/frc/robot/constants/FeatureFlags.java b/src/main/java/frc/robot/constants/FeatureFlags.java
@@ -8,4 +8,5 @@ public final class FeatureFlags {
     public static final boolean runLocalizer = false;
     public static final boolean runIntake = true;
     public static final boolean runScoring = true;
+    public static final boolean runLEDS = true;
 }
diff --git a/src/main/java/frc/robot/constants/LEDConstants.java b/src/main/java/frc/robot/constants/LEDConstants.java
@@ -0,0 +1,8 @@
+package frc.robot.constants;
+
+public class LEDConstants {
+
+    public static final int ledPort = 0;
+
+    public static final int ledLength = 30;
+}
diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java
@@ -34,7 +34,7 @@ public class ScoringConstants {
     public static final double aimerCurrentLimit = 60;
 
     public static final int aimerEncoderId = 13;
-    ;
+
     public static final double aimerEncoderOffset = -0.087402; // Armencoder is zeroed
 
     public static final double aimerEncoderToMechanismRatio = 1.0;

diff --git a/src/main/java/frc/robot/subsystems/LED.java b/src/main/java/frc/robot/subsystems/LED.java
@@ -0,0 +1,133 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.LEDConstants;
+import frc.robot.constants.ModeConstants;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.scoring.ScoringSubsystem;
+import java.util.function.Supplier;
+
+public class LED extends SubsystemBase {
+
+    private final AddressableLED led;
+    private final AddressableLEDBuffer ledBuffer;
+
+    private Timer timer;
+
+    private boolean enabled = true;
+
+    private final int ledcount = LEDConstants.ledLength;
+    private final int ledBrightness = 100;
+
+    private int offset = 0;
+    private final int rainbowSpeed = 4;
+    private final int rainbowScale = 2;
+
+    private ScoringSubsystem scoringSubsystem;
+    private IntakeSubsystem intakeSubsystem;
+
+    private Supplier<Boolean> visionWorkingSupplier = () -> true;
+
+    public LED(ScoringSubsystem scoringSubsystem, IntakeSubsystem intakeSubsystem) {
+        led = new AddressableLED(LEDConstants.ledPort);
+        ledBuffer = new AddressableLEDBuffer(ledcount);
+        led.setLength(ledBuffer.getLength());
+        timer = new Timer();
+        this.scoringSubsystem = scoringSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+
+        led.setData(ledBuffer);
+        led.start();
+    }
+
+    public void setEnabled(boolean enabled) {
+        this.enabled = enabled;
+    }
+
+    @Override
+    public void periodic() {
+
+        clear();
+
+        if (!enabled) {
+            // LEDs left cleared if not enabled
+        } else if (DriverStation.isDisabled()) {
+            if (ModeConstants.currentMode == ModeConstants.Mode.REAL) {
+                rainbow();
+
+                if (!visionWorkingSupplier.get()) {
+                    for (int i = 0; i < ledcount - 10; i++) {
+                        ledBuffer.setRGB(i, 0, 0, 255 / 3);
+                    }
+                }
+            }
+
+        } else {
+            if ((scoringSubsystem != null && scoringSubsystem.hasNote())
+                    || (intakeSubsystem != null && intakeSubsystem.hasNote())) {
+                for (int i = 0; i < ledcount; i++) {
+                    ledBuffer.setRGB(i, 245 / 3, 117 / 3, 66 / 3);
+                }
+            }
+
+            if (DriverStation.getMatchTime() < 20 && DriverStation.getMatchTime() > 17) {
+                for (int i = 0; i < ledcount; i++) {
+                    ledBuffer.setRGB(i, 255 / 3, 0, 0);
+                }
+            }
+
+            if (!visionWorkingSupplier.get()) {
+                for (int i = 0; i < ledcount - 10; i++) {
+                    ledBuffer.setRGB(i, 0, 0, 255 / 3);
+                }
+            }
+        }
+
+        if (ModeConstants.currentMode == ModeConstants.Mode.REAL) {
+            led.setData(ledBuffer);
+        }
+    }
+
+    public void setVisionWorkingSupplier(Supplier<Boolean> visionWorkingSupplier) {
+        this.visionWorkingSupplier = visionWorkingSupplier;
+    }
+
+    private void clear() {
+        for (int i = 0; i < ledcount; i++) {
+            ledBuffer.setRGB(i, 0, 0, 0);
+        }
+    }
+
+    private void setSolidColorSection(int lower, int upper, int[] rgbCode) {
+        for (int i = lower; i <= upper; i++) {
+            ledBuffer.setRGB(i, rgbCode[0], rgbCode[1], rgbCode[2]);
+        }
+    }
+
+    private void setFlashingColorSection(int lower, int upper, int[] rgbCode1, int[] rgbCode2) {
+        if ((Math.floor(timer.get()) / 5.0) % 2 == 0) {
+            for (int i = lower; i <= upper; i++) {
+                ledBuffer.setRGB(i, rgbCode1[0], rgbCode1[1], rgbCode1[2]);
+            }
+        } else {
+            for (int i = lower; i <= upper; i++) {
+                ledBuffer.setRGB(i, rgbCode2[0], rgbCode2[1], rgbCode2[2]);
+            }
+        }
+    }
+
+    private void rainbow() {
+        for (int i = 0; i < ledcount; i++) {
+
+            int hue = (offset + i * rainbowScale) % 180;
+            ledBuffer.setHSV(i, hue, 255, ledBrightness);
+        }
+
+        offset += rainbowSpeed;
+        offset %= 180;
+    }
+}