Added LEDInstructions for certain robot tasks to distinguish.

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -37,14 +37,11 @@
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.robot.subsystems.leds.LEDController;
-import com.stuypulse.robot.subsystems.leds.instructions.LEDPulseColor;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.subsystems.vision.AprilTagVision;
 import com.stuypulse.robot.subsystems.vision.NoteVision;
-import com.stuypulse.robot.util.SLColor;
-import com.stuypulse.robot.util.ShooterSpeeds;
 import com.stuypulse.robot.util.PathUtil.AutonConfig;
 
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -123,8 +120,8 @@ private void configureDriverBindings() {
         // intaking
         driver.getRightTriggerButton()
             .whileTrue(new IntakeAcquire()
-                    .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE))
-                .andThen(new BuzzController(driver)
+                    .deadlineWith(new LEDSet(LEDInstructions.INTAKE))
+                .andThen(new BuzzController(driver) 
                     .alongWith(new LEDSet(LEDInstructions.PICKUP)
                         .withTimeout(3.0))));
 
@@ -137,7 +134,7 @@ private void configureDriverBindings() {
         driver.getRightBumper()
             .onTrue(new ShooterPodiumShot())
             .whileTrue(new SwerveDriveToShoot()
-                    .deadlineWith(new LEDSet(LEDInstructions.ASSIST_FLASH))
+                    .deadlineWith(new LEDSet(LEDInstructions.SPEAKER_ALIGN))
                 .andThen(new ShooterWaitForTarget()
                     .withTimeout(1.5))
                 .andThen(new ConveyorShoot()))
@@ -165,7 +162,8 @@ private void configureDriverBindings() {
         driver.getLeftMenuButton()
             .whileTrue(ConveyorToAmp.withCheckLift()
                 .andThen(AmperToHeight.untilDone(Lift.AMP_SCORE_HEIGHT))
-                .andThen(new AmperScore()))
+                .andThen(new AmperScore()
+                    .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE))))
             .onFalse(new AmperStop())
             .onFalse(new AmperToHeight(Lift.MIN_HEIGHT));
 
@@ -174,51 +172,63 @@ private void configureDriverBindings() {
             .onTrue(new AmperScoreTrap())
             .onFalse(new AmperStop());
 
+        // lift to trap
         driver.getDPadRight()
             .onTrue(new ConditionalCommand(new ConveyorToAmp(), new DoNothingCommand(), () -> Intake.getInstance().hasNote())
                 .andThen(new AmperToHeight(Lift.TRAP_SCORE_HEIGHT)));
 
+        // reset heading
         driver.getDPadUp()
             .onTrue(new SwerveDriveResetHeading());
 
+        // trap outtake
         driver.getDPadDown()
             .onTrue(new AmperScoreSpeed(-Score.TRAP_SPEED))
             .onFalse(new AmperStop());
 
+        // automatic swerve drive
         driver.getTopButton()
             // on command start
             .onTrue(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                .deadlineWith(new LEDSet(LEDInstructions.GREEN)))
+                .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
 
             .onTrue(new SwerveDriveAutomatic(driver)
                 // after command end
                 .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                    .deadlineWith(new LEDSet(LEDInstructions.GREEN)))
+                    .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE)))
 
                 .andThen(new WaitCommand(Driver.Drive.BUZZ_DURATION))
 
                 .andThen(new BuzzController(driver, Assist.BUZZ_INTENSITY)
-                    .deadlineWith(new LEDSet(LEDInstructions.GREEN))));
+                    .deadlineWith(new LEDSet(LEDInstructions.AUTO_SWERVE))));
 
+        // climb
         driver.getRightButton()
-            .whileTrue(new SwerveDriveToClimb());
+            .whileTrue(new SwerveDriveToClimb()
+                .deadlineWith(new LEDSet(LEDInstructions.TRAP_ALIGN)));
 
+        // drive to chain
         driver.getBottomButton()
-            .whileTrue(new SwerveDriveDriveToChain());
+            .whileTrue(new SwerveDriveDriveToChain()
+                .deadlineWith(new LEDSet(LEDInstructions.TRAP_ALIGN)));
     }
 
     private void configureOperatorBindings() {
+        // climber
         new Trigger(() -> operator.getLeftStick().magnitude() > Settings.Operator.DEADBAND.get())
             .whileTrue(new ClimberDrive(operator));
 
+        // climber LED
         new Trigger(() -> operator.getLeftY() > Settings.Operator.DEADBAND.get())
             .onTrue(new LEDSet(LEDInstructions.CLIMB_UP)
                 .withTimeout(1.0));
 
+        // move note to amp
         new Trigger(() -> operator.getLeftY() > 0.25)
             .onTrue(new ConveyorToAmp()
                 .andThen(new ShooterStop()));
 
+
         new Trigger(() -> operator.getRightStick().magnitude() > Settings.Operator.DEADBAND.get())
             .whileTrue(new AmperLiftDrive(operator));
 
@@ -227,7 +237,7 @@ private void configureOperatorBindings() {
             .onFalse(new IntakeStop());
         operator.getRightTriggerButton()
             .whileTrue(new IntakeAcquire()
-                .deadlineWith(new LEDSet(LEDInstructions.DARK_BLUE))
+                .deadlineWith(new LEDSet(LEDInstructions.INTAKE))
                 .andThen(new BuzzController(driver)
                     .alongWith(new LEDSet(LEDInstructions.PICKUP)
                         .withTimeout(3.0))));
@@ -267,7 +277,7 @@ private void configureOperatorBindings() {
 
         // human player attention button
         operator.getRightMenuButton()
-            .whileTrue(new LEDSet(LEDInstructions.PULSE_PURPLE));
+            .whileTrue(new LEDSet(LEDInstructions.ATTENTION));
 
         operator.getLeftMenuButton()
             .onTrue(new AmperToConveyor())

src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java

@@ -62,7 +62,7 @@ public AmpScoreRoutine() {
                 new ConveyorToAmp(),
                 new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get()))
                     .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE)
-                    .deadlineWith(new LEDSet(LEDInstructions.GREEN))
+                    .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN))
             ),
 
             new ParallelCommandGroup(
@@ -74,7 +74,7 @@ public AmpScoreRoutine() {
                         AMP_WALL_SCORE_Y_TOLERANCE,
                         AMP_WALL_SCORE_ANGLE_TOLERANCE)
                     .withTimeout(SCORE_ALIGN_TIMEOUT)
-                    .deadlineWith(new LEDSet(LEDInstructions.GREEN))
+                    .deadlineWith(new LEDSet(LEDInstructions.AMP_SCORE))
             ),
 
             AmperScore.untilDone(),

src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java

@@ -12,6 +12,7 @@
 import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.robot.subsystems.leds.LEDController;
 import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction;
+import com.stuypulse.stuylib.util.StopWatch;
 
 import edu.wpi.first.wpilibj2.command.Command;
 
@@ -27,7 +28,6 @@
 public class LEDDefaultMode extends Command {
 
     private final LEDController leds;
-
     private final Intake intake;
     private final Amper amper;
 
@@ -44,9 +44,10 @@ private LEDInstruction getInstruction() {
                 && amper.isAtTargetHeight(0.15))
             return LEDInstructions.GREEN;
 
-        if (intake.hasNote())
-            return LEDInstructions.RED;
-
+        if (intake.hasNote()) {
+            return LEDInstructions.CONTAINS_NOTE;
+        }
+
         return LEDInstructions.DEFAULT;
     }
 

src/main/java/com/stuypulse/robot/constants/LEDInstructions.java

@@ -46,9 +46,9 @@ public interface LEDInstructions {
     LEDInstruction WHITE = new LEDSingleColor(new SLColor(255, 255, 255));
     LEDInstruction YELLOW = new LEDSingleColor(new SLColor(255, 255, 0));
 
-    LEDInstruction BANGLADESH = new LEDSection(new SLColor[] {SLColor.RED, SLColor.BLACK, SLColor.DARK_GREEN});
+    LEDInstruction BANGLADESH = new LEDSection(new SLColor[] {SLColor.RED, SLColor.BLACK, SLColor.DARK_GREEN}, false);
 
-    LEDInstruction CLIMB_UP = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN});
+
 
     /******************/
     /*** NON-STATIC ***/
@@ -58,18 +58,37 @@ public interface LEDInstructions {
 
     LEDInstruction PULSE_RED = new LEDPulseColor(SLColor.RED);
     LEDInstruction PULSE_RED_BLUE = new LEDPulseColor(SLColor.RED, SLColor.BLUE);
-    LEDInstruction PULSE_PURPLE = new LEDPulseColor(SLColor.PURPLE, .25);
     LEDInstruction RICHIE = new RichieMode(SLColor.RED);
 
-    LEDInstruction PICKUP = new LEDPulseColor(SLColor.WHITE, 0.5);
+
 
     /********************************************/
     /*** LED CONSTANTS TO BE USED IN COMMANDS ***/
     /********************************************/
 
     LEDInstruction DEFAULT = LEDInstructions.OFF;
 
-    LEDInstruction ASSIST_FLASH = LEDInstructions.GREEN;
+    LEDInstruction INTAKE = new LEDPulseColor(SLColor.RED);
+
+    LEDInstruction PICKUP = new LEDPulseColor(SLColor.RED);
+
+    LEDInstruction SPEAKER_ALIGN = new LEDPulseColor(SLColor.GREEN);
+
+    LEDInstruction AMP_ALIGN = new LEDPulseColor(SLColor.GREEN);
+
+    LEDInstruction AMP_SCORE = LEDInstructions.PURPLE;
+
+    LEDInstruction AUTO_SWERVE =  new LEDPulseColor(SLColor.GREEN);
+
+    LEDInstruction ATTENTION = new LEDPulseColor(SLColor.YELLOW);
+
+    LEDInstruction CLIMB_UP = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN}, false);
+
+    LEDInstruction TRAP_ALIGN = new LEDSection(new SLColor[] {SLColor.PURPLE, SLColor.GREEN}, true);
+
+    LEDInstruction FERRY = new LEDPulseColor(SLColor.ORANGE); // No Ferry command yet
+
+	LEDInstruction CONTAINS_NOTE = new LEDSingleColor(SLColor.RED);
 
     // TO FUTURE USERS, DONT PUT LEDAlign and LEDAutonChooser (any disabled LEDInstructions) inside
     // the LEDInstructions interface

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -154,7 +154,7 @@ public interface Swerve {
         public interface Assist {
             SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change
 
-            double BUZZ_INTENSITY = 0.8;
+            double BUZZ_INTENSITY = 1;
 
             SmartNumber kP = new SmartNumber("SwerveAssist/kP", 2.0);
             SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);
@@ -422,7 +422,7 @@ public interface Rotation {
 
     public interface LED {
         int LED_LENGTH = 15;
-        SmartNumber BLINK_TIME = new SmartNumber("LED/LED Blink Time", .5);
+        SmartNumber BLINK_TIME = new SmartNumber("LED/LED Blink Time", .15);
 
         SmartNumber TRANSLATION_SPREAD = new SmartNumber("LED/LED Translation Spread (m)", 0.5);
         SmartNumber ROTATION_SPREAD = new SmartNumber("LED/LED Rotation Spread (deg)", 15);

src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDAutonChooser.java

@@ -88,7 +88,7 @@ public static AutonLEDColors fromName(String name) {
     }
 
     public LEDAutonChooser() {
-        super(AutonLEDColors.fromName(RobotContainer.getAutonomousCommandNameStatic()).ledColors);
+        super(AutonLEDColors.fromName(RobotContainer.getAutonomousCommandNameStatic()).ledColors, true);
     }
 
     @Override

src/main/java/com/stuypulse/robot/subsystems/leds/instructions/LEDSection.java

@@ -6,44 +6,88 @@
 
 package com.stuypulse.robot.subsystems.leds.instructions;
 
+import java.util.Arrays;
+
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.util.SLColor;
+import com.stuypulse.stuylib.util.StopWatch;
 
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 
 public class LEDSection implements LEDInstruction {
 
-    public final SLColor[] sections;
-    public final int[] separatorIndexes;
+    private final SLColor[] sections;
+    private final SLColor[] altSections;
+    private final boolean isBlinking;
+    private final int[] separatorIndexes;
+    private final StopWatch stopWatch;
 
-    public LEDSection(SLColor[] sections, int[] separatorIndexes) {
-        if (sections.length - 1 != separatorIndexes.length) {
-            throw new IllegalArgumentException("Mismatching section and seperating indexes");
+    public LEDSection(SLColor[] sections, SLColor[] altSections, int[] separatorIndexes, boolean isBlinking) {
+        if (sections.length != separatorIndexes.length) {
+            throw new IllegalArgumentException("Mismatching section and seperating indexes: \n Section Length (" + sections.length +") \n SeparatorIndexes ("+ separatorIndexes.length +")" );
         }
 
         this.sections = sections;
+        this.altSections = altSections;
         this.separatorIndexes = separatorIndexes;
+        this.isBlinking = isBlinking;
+        this.stopWatch = new StopWatch();
     }
 
-    public LEDSection(SLColor[] sections) {
-        this.sections = sections;
+    public LEDSection(SLColor[] sections, SLColor[] altSections){
+        this(sections, altSections, genSeparators(sections), true);
+    }
 
+    public LEDSection(SLColor[] sections, int[] separatorIndexes) {
+        this(sections, genEmptyArray(sections.length), separatorIndexes, false);
+    }
+
+    public LEDSection(SLColor[] sections, boolean isBlinking) {
+        this(sections, genEmptyArray(sections.length), genSeparators(sections), isBlinking);
+    }
+
+    @Override
+    public void setLED(AddressableLEDBuffer ledBuffer) {
+        if (isBlinking){
+            double time = stopWatch.getTime();
+            double pulseTime = Settings.LED.BLINK_TIME.get();
+
+            if (time < pulseTime) {
+                setColorSections(ledBuffer, this.sections, this.separatorIndexes);
+            } else if (time < pulseTime * 2) {
+                setColorSections(ledBuffer, this.altSections, this.separatorIndexes);
+            } else {
+                stopWatch.reset();
+            }
+        } else {
+            setColorSections(ledBuffer, this.sections, this.separatorIndexes);
+        }
+    }
+
+    private static SLColor[] genEmptyArray(int length) {
+        SLColor[] array = new SLColor[length];
+        Arrays.fill(array, SLColor.BLACK);
+        return array;
+    }
+
+    private static int[] genSeparators(SLColor[] sections){
         int totalLEDLength = Settings.LED.LED_LENGTH;
         int sectionLength = totalLEDLength / sections.length;
         int extraLEDS = totalLEDLength % sections.length;
 
-        this.separatorIndexes = new int[sections.length];
+        int[] separators = new int[sections.length];
 
         // extra LEDs get distributed to the first few sections
         for (int i = 0; i < sections.length; i++) {
             int offset = Math.min(extraLEDS, i);
             int extraLEDForCurrentSection = (i < extraLEDS ? 1 : 0);
-            this.separatorIndexes[i] = (i + 1) * sectionLength + offset + extraLEDForCurrentSection - 1;
+            separators[i] = (i + 1) * sectionLength + offset + extraLEDForCurrentSection - 1;
         }
+
+        return separators;
     }
 
-    @Override
-    public void setLED(AddressableLEDBuffer ledBuffer) {
+    private void setColorSections(AddressableLEDBuffer ledBuffer, SLColor[] sections, int[] separatorIndexes){
         int sectionIndex = 0;
 
         for (int ledIndex = 0; ledIndex < ledBuffer.getLength(); ledIndex++) {