Add Leds

Author: leo-les

src/main/java/frc/team3128/RobotContainer.java

@@ -62,8 +62,6 @@ public class RobotContainer {
     private Hopper hopper;
     private Intake intake;
     private Shooter shooter;
-
-
     private Leds leds;
 
     // private NAR_ButtonBoard judgePad;
@@ -91,6 +89,7 @@ public RobotContainer() {
         hopper = Hopper.getInstance();
         intake = Intake.getInstance();
         shooter = Shooter.getInstance();
+        leds = Leds.getInstance();
 
         //uncomment line below to enable driving
         CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true));
@@ -151,7 +150,10 @@ private void configureButtonBindings() {
         //Stops shooting when all notes are gone
         new Trigger(()-> shooter.noteInRollers()).negate()
         .and(()->hopper.hasObjectPresent()).negate()
-        .onTrue(shooter.setShooting(false));
+        .onTrue(sequence(
+            shooter.setShooting(false),
+            runOnce(() -> leds.setLedColor(Colors.BLUE))
+        ));
 
         //Queues note to hopper
         new Trigger(()-> intake.getMeasurement() > 90)
@@ -169,6 +171,7 @@ private void configureButtonBindings() {
         .and(()->hopper.hasObjectPresent())
         .and(() -> !shooter.getShooting())
         .onTrue(sequence(
+            runOnce(() -> leds.blinkLEDColor(Colors.RED, Colors.GREEN, .25)),
             shooter.runKickMotor(KICK_POWER),
             hopper.runManipulator(HOPPER_INTAKE_POWER)
         ))
@@ -178,7 +181,10 @@ private void configureButtonBindings() {
             shooter.runKickMotor(0)
         ));
 
-        // new Trigger(() -> shouldEjectNote()).onTrue(ejectNote());
+        // new Trigger(() -> shouldEjectNote()).onTrue(sequence(
+        //     runOnce(() -> leds.setLedColor(Colors.PURPLE)),
+        //     ejectNote()
+        //     ));
 
     }
 
@@ -189,6 +195,7 @@ private boolean shouldEjectNote(){
         if(shooter.noteInRollers() && hopper.hasObjectPresent() && !ejectTimerStarted){
             ejectTimerStarted = true;
             ejecTimer.start();
+            runOnce(() -> leds.blinkLEDColor(Colors.RED, Colors.ORANGE, .25));
         }
 
         else if(shooter.noteInRollers() && hopper.hasObjectPresent() && ejectTimerStarted){

src/main/java/frc/team3128/subsystems/Leds.java

@@ -7,8 +7,12 @@
 import com.ctre.phoenix.led.FireAnimation;
 import com.ctre.phoenix.led.RainbowAnimation;
 import com.ctre.phoenix.led.SingleFadeAnimation;
+import com.ctre.phoenix.led.StrobeAnimation;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+
+import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds;
 import static frc.team3128.Constants.LedConstants.*;
 
 import frc.team3128.Constants.LedConstants.Colors;
@@ -79,6 +83,14 @@ public void setLedColor(Colors color) {
         }
     }
 
+    public void blinkLEDColor(Colors blinkColor, Colors endColor, double duration) {
+        resetAnimationSlot(2);
+        m_candle.animate(new StrobeAnimation(blinkColor.r, blinkColor.g, blinkColor.b, WHITE_VALUE, 1,STARTING_ID + PIVOT_FRONT,STARTING_ID));
+        waitSeconds(duration);
+        setLedColor(endColor);
+        
+    }
+
     public void resetAnimationSlot(int slots) {
         m_candle.setLEDs(0,0,0, WHITE_VALUE, STARTING_ID, PIVOT_COUNT);
         for (int i = 0; i < slots; i++) {