add lights off button and reset team colors button

diff --git a/src/main/java/frc/robot/subsystems/LED/LED2.java b/src/main/java/frc/robot/subsystems/LED/LED2.java
index 415268b95a233a1e68371e2c78ee1f15ac9fa046..17f6c23980cddfc52a78d0c80929d91e68443280 100644 (file)
--- a/src/main/java/frc/robot/subsystems/LED/LED2.java
+++ b/src/main/java/frc/robot/subsystems/LED/LED2.java
@@ -66,8 +66,21 @@ public class LED2 extends SubsystemBase {
 
                System.out.println("CANdle features: " + featureConf + ", LED config: " + ledConf);
 
+               SmartDashboard.putData("LED Off", new InstantCommand(() -> lightsOff()));
                SmartDashboard.putData("LED Strobe", new InstantCommand(() -> setStrobe()));
                SmartDashboard.putData("LED Static", new InstantCommand(() -> setStatic()));
+               SmartDashboard.putData("LED Color/Team Reset", new InstantCommand(() -> {
+                       var allianceeee = DriverStation.getAlliance();
+                       if (allianceeee.isEmpty()) {
+                               color = Color.kWhite;
+                       } else if (allianceeee.get() == Alliance.Red) {
+                               color = Color.kRed;
+                       } else if (allianceeee.get() == Alliance.Blue) {
+                               color = Color.kBlue;
+                       } else {
+                               color = Color.kWhite;
+                       }
+               }));
        }
 
        private boolean flippy = true;
@@ -83,14 +96,19 @@ public class LED2 extends SubsystemBase {
                }
        }
 
-       private void setStrobe() {
+       public void setStrobe() {
                candle.setControl(new StrobeAnimation(8, 8 + stripLength).withFrameRate(FLASH_RATE).withColor(new RGBWColor(color)));
        }
 
-       private void setStatic() {
+       public void setStatic() {
                candle.setControl(new SolidColor(8, 8 + stripLength).withColor(new RGBWColor(color)));
        }
 
+       public void lightsOff() {
+               candle.setControl(new SolidColor(8 , 8 + stripLength).withColor(new RGBWColor(0, 0, 0, 0)));
+       }
+       
+
        private boolean underSecsToFlip(double secs) {
                Optional<Double> timeToActive = HubActive.timeToActive();
                Optional<Double> timeToInactive = HubActive.timeToInactive();