From 3ed491ffdcd9d804778999e98628716da11da34d Mon Sep 17 00:00:00 2001 From: aidnem <99768676+aidnem@users.noreply.github.com> Date: Sat, 26 Oct 2024 08:33:03 -0400 Subject: [PATCH] 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 Co-authored-by: avidraccoon --- src/main/java/frc/robot/RobotContainer.java | 20 +++ .../frc/robot/constants/FeatureFlags.java | 1 + .../frc/robot/constants/LEDConstants.java | 8 ++ .../frc/robot/constants/ScoringConstants.java | 2 +- src/main/java/frc/robot/subsystems/LED.java | 133 ++++++++++++++++++ 5 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/constants/LEDConstants.java create mode 100644 src/main/java/frc/robot/subsystems/LED.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2f1eca..9a8b2cd 100644 --- 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 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 index 3e14c0c..6058022 100644 --- 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 new file mode 100644 index 0000000..309b8af --- /dev/null +++ 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 index cdc4fda..bad11d6 100644 --- 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 new file mode 100644 index 0000000..5d2d89b --- /dev/null +++ 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 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 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; + } +}