From c4b89ec126d247a4631081fb9c062cf9fb1be7d7 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Sun, 4 Feb 2024 21:38:53 -0800
Subject: [PATCH 01/14] feat: LED Framework

---
 .../org/team1540/robot2024/Constants.java     |  3 +-
 .../java/org/team1540/robot2024/Robot.java    | 10 +++
 .../team1540/robot2024/RobotContainer.java    |  2 +
 .../robot2024/subsystems/led/LedPattern.java  | 34 ++++++++++
 .../subsystems/led/LedPatternRainbow.java     | 23 +++++++
 .../robot2024/subsystems/led/Leds.java        | 50 +++++++++++++++
 .../led/ZonedAddressableLEDBuffer.java        | 64 +++++++++++++++++++
 7 files changed, 185 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index 4d632b8c..0af642f6 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -20,7 +20,8 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
-
+    public static final int LED_STRIP_PORT_PWM = 0;
+    public static final int LED_STRIP_LENGTH= 80;
     public enum Mode {
         /**
          * Running on a real robot.
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 0872c1b6..25a4e0fb 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -1,6 +1,7 @@
 package org.team1540.robot2024;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -9,6 +10,9 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team1540.robot2024.subsystems.led.LedPattern;
+import org.team1540.robot2024.subsystems.led.LedPatternRainbow;
+import org.team1540.robot2024.subsystems.led.Leds;
 import org.team1540.robot2024.util.MechanismVisualiser;
 
 /**
@@ -101,6 +105,8 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1, LedPattern.solid(Color.kBlueViolet));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2, LedPattern.solid(Color.kPaleVioletRed));
     }
 
     /**
@@ -115,6 +121,7 @@ public void disabledPeriodic() {
      */
     @Override
     public void autonomousInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,LedPattern.alternating(Color.kBlueViolet, Color.kCrimson));
         autonomousCommand = robotContainer.getAutonomousCommand();
         // schedule the autonomous command (example)
         if (autonomousCommand != null) {
@@ -138,6 +145,7 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1, LedPattern.alternating(Color.kBlueViolet, Color.kGreen));
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
@@ -155,6 +163,8 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternRainbow(1));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternRainbow(1));
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 8c9d79d7..79158466 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -27,6 +27,7 @@
 import org.team1540.robot2024.subsystems.indexer.IndexerIO;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
+import org.team1540.robot2024.subsystems.led.Leds;
 import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.tramp.TrampIO;
@@ -56,6 +57,7 @@ public class RobotContainer {
     public final Elevator elevator;
     public final Indexer indexer;
     public final AprilTagVision aprilTagVision;
+    public final Leds leds = new Leds();
 
     // Controller
     public final CommandXboxController driver = new CommandXboxController(0);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
new file mode 100644
index 00000000..8ee9c340
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
@@ -0,0 +1,34 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+
+import java.util.function.Consumer;
+
+public class LedPattern {
+    private final Consumer<ZonedAddressableLEDBuffer> applier;
+    public final boolean isDynamic;
+    LedPattern(Consumer<ZonedAddressableLEDBuffer> applier, boolean isDynamic) {
+        this.applier = applier;
+        this.isDynamic = isDynamic;
+    }
+
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        this.applier.accept(buffer);
+    }
+
+    public static LedPattern solid(Color color) {
+        return new LedPattern((buffer) -> {
+            for (int i = 0; i<buffer.getLength();i++) {
+                buffer.setLED(i, color);
+            }
+        }, false);
+    }
+    public static LedPattern alternating(Color a, Color b) {
+        return new LedPattern((buffer) -> {
+            for (int i = 0; i<buffer.getLength();i++) {
+                buffer.setLED(i, i % 2 == 0 ? a : b);
+            }
+        }, false);
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
new file mode 100644
index 00000000..6024aec4
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -0,0 +1,23 @@
+package org.team1540.robot2024.subsystems.led;
+
+import static java.lang.Math.round;
+
+public class LedPatternRainbow extends LedPattern {
+    private final int speed;
+    int initialHue = 0;
+    public LedPatternRainbow(int speed) {
+        super(null, true);
+        this.speed = speed;
+    }
+
+    @Override
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        for (int i = 0; i < buffer.getLength(); i++) {
+            int hue = (initialHue + (i * 180 / buffer.getLength())) % 180;
+            buffer.setHSV(i, hue, 255, 128);
+        }
+        initialHue += speed;
+        initialHue %= 180;
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
new file mode 100644
index 00000000..497dbfbc
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -0,0 +1,50 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.team1540.robot2024.Constants;
+
+
+import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;
+
+public class Leds extends SubsystemBase {
+
+    private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.LED_STRIP_LENGTH);
+    private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
+    private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
+    private final LedPattern[] patterns = new LedPattern[ZONE_COUNT];
+
+    public Leds() {
+        strip.setLength(ledBuffer.getLength());
+        strip.setData(ledBuffer);
+        strip.start();
+
+        buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 0, 40, false);
+        buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
+    }
+
+    @Override
+    public void periodic() {
+        for (int i = 0; i < ZONE_COUNT;i++) {
+            if (patterns[i].isDynamic) {
+                patterns[i].apply(buffers[i]);
+            }
+        }
+        strip.setData(ledBuffer);
+    }
+
+    public void setPattern(Zone zone, LedPattern pattern) {
+        patterns[zone.ordinal()] = pattern;
+        if (!pattern.isDynamic) {
+            pattern.apply(buffers[zone.ordinal()]);
+        }
+    }
+
+
+    private static final int ZONE_COUNT=Zone.values().length;
+    public enum Zone {
+        ZONE1,
+        ZONE2;
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
new file mode 100644
index 00000000..179c272a
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
@@ -0,0 +1,64 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+class ZonedAddressableLEDBuffer {
+    private final AddressableLEDBuffer buffer;
+    public final boolean isInverted;
+    public final int start;
+    public final int length;
+
+    public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {
+        if (start > end) {
+            throw new IllegalArgumentException("start must be less than end");
+        }
+        this.buffer = buffer;
+        this.isInverted = isInverted;
+        this.start = start;
+        this.length = end - start;
+    }
+
+    public void setRGB(int index, int r, int g, int b) {
+        buffer.setRGB(this.getAbsoluteIndex(index), r, g, b);
+    }
+
+    public void setHSV(int index, int h, int s, int v) {
+        buffer.setHSV(this.getAbsoluteIndex(index), h, s, v);
+    }
+
+    public void setLED(int index, Color color) {
+        System.out.println(this.getAbsoluteIndex(index));
+        buffer.setLED(this.getAbsoluteIndex(index), color);
+    }
+
+    public void setLED(int index, Color8Bit color) {
+        buffer.setLED(this.getAbsoluteIndex(index), color);
+    }
+
+    public Color getLED(int index) {
+        return buffer.getLED(this.getAbsoluteIndex(index));
+    }
+
+    public Color8Bit getLED8Bit(int index) {
+        return buffer.getLED8Bit(this.getAbsoluteIndex(index));
+    }
+
+    public int getLength() {
+        return this.length;
+    }
+
+    private int getAbsoluteIndex(int index) {
+        if (index >= length) {
+            DriverStation.reportWarning("led index out of bounds", false);
+            return 0;
+        }
+        if (this.isInverted) {
+            return this.start + length -1 - index;
+        } else {
+            return this.start + index;
+        }
+    }
+}

From da7b227d77f9037287b3e09c610975f7605e2891 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Sun, 4 Feb 2024 21:50:49 -0800
Subject: [PATCH 02/14] refactor: clean up modifiers, loops

---
 .../robot2024/subsystems/led/LedPattern.java  | 28 +++++++++----------
 .../subsystems/led/LedPatternRainbow.java     |  7 ++---
 2 files changed, 16 insertions(+), 19 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
index 8ee9c340..5da35421 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
@@ -1,34 +1,32 @@
 package org.team1540.robot2024.subsystems.led;
 
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
 
-import java.util.function.Consumer;
+import java.util.function.BiConsumer;
 
 public class LedPattern {
-    private final Consumer<ZonedAddressableLEDBuffer> applier;
+    private final BiConsumer<ZonedAddressableLEDBuffer, Integer> applier;
     public final boolean isDynamic;
-    LedPattern(Consumer<ZonedAddressableLEDBuffer> applier, boolean isDynamic) {
+    private LedPattern(BiConsumer<ZonedAddressableLEDBuffer, Integer> applier, boolean isDynamic) {
         this.applier = applier;
         this.isDynamic = isDynamic;
     }
+    protected LedPattern(boolean isDynamic) {
+        this.applier = null;
+        this.isDynamic = isDynamic;
+    }
 
     void apply(ZonedAddressableLEDBuffer buffer) {
-        this.applier.accept(buffer);
+        assert this.applier != null;
+        for (int i = 0; i < buffer.getLength(); i++) {
+            this.applier.accept(buffer, i);
+        }
     }
 
     public static LedPattern solid(Color color) {
-        return new LedPattern((buffer) -> {
-            for (int i = 0; i<buffer.getLength();i++) {
-                buffer.setLED(i, color);
-            }
-        }, false);
+        return new LedPattern((buffer, i) -> buffer.setLED(i, color), false);
     }
     public static LedPattern alternating(Color a, Color b) {
-        return new LedPattern((buffer) -> {
-            for (int i = 0; i<buffer.getLength();i++) {
-                buffer.setLED(i, i % 2 == 0 ? a : b);
-            }
-        }, false);
+        return new LedPattern((buffer, i) -> buffer.setLED(i, i % 2 == 0 ? a : b), false);
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
index 6024aec4..38e2639b 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -1,12 +1,11 @@
 package org.team1540.robot2024.subsystems.led;
 
-import static java.lang.Math.round;
-
 public class LedPatternRainbow extends LedPattern {
     private final int speed;
-    int initialHue = 0;
+    private int initialHue = 0;
+
     public LedPatternRainbow(int speed) {
-        super(null, true);
+        super(true);
         this.speed = speed;
     }
 

From 4243c0b14bc82185b3bb9f98519d91fbd0157bf6 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Mon, 5 Feb 2024 08:38:12 -0800
Subject: [PATCH 03/14] feat: add wave pattern

---
 .../java/org/team1540/robot2024/Robot.java    |  5 +--
 .../subsystems/led/LedPatternWave.java        | 34 +++++++++++++++++++
 .../led/ZonedAddressableLEDBuffer.java        |  4 +--
 3 files changed, 39 insertions(+), 4 deletions(-)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 25a4e0fb..0d6e38fc 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -12,6 +12,7 @@
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import org.team1540.robot2024.subsystems.led.LedPattern;
 import org.team1540.robot2024.subsystems.led.LedPatternRainbow;
+import org.team1540.robot2024.subsystems.led.LedPatternWave;
 import org.team1540.robot2024.subsystems.led.Leds;
 import org.team1540.robot2024.util.MechanismVisualiser;
 
@@ -163,8 +164,8 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternRainbow(1));
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternRainbow(1));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternWave());
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternWave());
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
new file mode 100644
index 00000000..c5a3e6fc
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
@@ -0,0 +1,34 @@
+package org.team1540.robot2024.subsystems.led;
+
+public class LedPatternWave extends LedPattern {
+    private static final double degradation = 0.7;
+    private static final int delay = 5;
+    private int location = 0;
+    private int ticker = 0;
+
+    public LedPatternWave() {
+        super(true);
+    }
+
+    @Override
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        for (int i = 0; i < buffer.getLength(); i++) {
+            int distance = Math.abs(location-i);
+            if (distance > buffer.getLength()/2) {
+                if (location > i) {
+                    distance = buffer.getLength()-location + i;
+                } else {
+                    distance = buffer.getLength() -i + location;
+                }
+            }
+            buffer.setHSV(i, 100, 255, (int) Math.max(255.0-distance*distance*degradation,0));
+        }
+        ticker++;
+        if (ticker % delay == 0) {
+            location++;
+        }
+        location %= buffer.getLength();
+    }
+
+
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
index 179c272a..99c39bc8 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
@@ -6,10 +6,10 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
 class ZonedAddressableLEDBuffer {
-    private final AddressableLEDBuffer buffer;
     public final boolean isInverted;
     public final int start;
     public final int length;
+    private final AddressableLEDBuffer buffer;
 
     public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {
         if (start > end) {
@@ -56,7 +56,7 @@ private int getAbsoluteIndex(int index) {
             return 0;
         }
         if (this.isInverted) {
-            return this.start + length -1 - index;
+            return this.start + length - 1 - index;
         } else {
             return this.start + index;
         }

From a4bb106d5a407948d1e869dfabafbb3020847f1b Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Mon, 5 Feb 2024 11:04:59 -0800
Subject: [PATCH 04/14] refactor: change led pattern structure

---
 .../java/org/team1540/robot2024/Robot.java    | 13 ++++-----
 .../robot2024/subsystems/led/LedPattern.java  | 28 ++++---------------
 .../subsystems/led/LedPatternRainbow.java     |  7 ++++-
 .../robot2024/subsystems/led/Leds.java        |  4 +--
 .../subsystems/led/SimpleLedPattern.java      | 27 ++++++++++++++++++
 5 files changed, 46 insertions(+), 33 deletions(-)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 0d6e38fc..7acbcc26 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -10,10 +10,7 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
-import org.team1540.robot2024.subsystems.led.LedPattern;
-import org.team1540.robot2024.subsystems.led.LedPatternRainbow;
-import org.team1540.robot2024.subsystems.led.LedPatternWave;
-import org.team1540.robot2024.subsystems.led.Leds;
+import org.team1540.robot2024.subsystems.led.*;
 import org.team1540.robot2024.util.MechanismVisualiser;
 
 /**
@@ -106,8 +103,8 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1, LedPattern.solid(Color.kBlueViolet));
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2, LedPattern.solid(Color.kPaleVioletRed));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.solid(Color.kBlueViolet));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2, SimpleLedPattern.solid(Color.kPaleVioletRed));
     }
 
     /**
@@ -122,7 +119,7 @@ public void disabledPeriodic() {
      */
     @Override
     public void autonomousInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,LedPattern.alternating(Color.kBlueViolet, Color.kCrimson));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,SimpleLedPattern.alternating(Color.kBlueViolet, Color.kCrimson));
         autonomousCommand = robotContainer.getAutonomousCommand();
         // schedule the autonomous command (example)
         if (autonomousCommand != null) {
@@ -146,7 +143,7 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1, LedPattern.alternating(Color.kBlueViolet, Color.kGreen));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
index 5da35421..7c179ff4 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
@@ -1,32 +1,16 @@
 package org.team1540.robot2024.subsystems.led;
 
-import edu.wpi.first.wpilibj.util.Color;
+public abstract class LedPattern {
+    private final boolean isDynamic;
 
-import java.util.function.BiConsumer;
-
-public class LedPattern {
-    private final BiConsumer<ZonedAddressableLEDBuffer, Integer> applier;
-    public final boolean isDynamic;
-    private LedPattern(BiConsumer<ZonedAddressableLEDBuffer, Integer> applier, boolean isDynamic) {
-        this.applier = applier;
-        this.isDynamic = isDynamic;
-    }
     protected LedPattern(boolean isDynamic) {
-        this.applier = null;
         this.isDynamic = isDynamic;
     }
 
-    void apply(ZonedAddressableLEDBuffer buffer) {
-        assert this.applier != null;
-        for (int i = 0; i < buffer.getLength(); i++) {
-            this.applier.accept(buffer, i);
-        }
+    boolean isDynamic() {
+        return isDynamic;
     }
 
-    public static LedPattern solid(Color color) {
-        return new LedPattern((buffer, i) -> buffer.setLED(i, color), false);
-    }
-    public static LedPattern alternating(Color a, Color b) {
-        return new LedPattern((buffer, i) -> buffer.setLED(i, i % 2 == 0 ? a : b), false);
-    }
+    abstract void apply(ZonedAddressableLEDBuffer buffer);
+
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
index 38e2639b..484c1f80 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -10,7 +10,12 @@ public LedPatternRainbow(int speed) {
     }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public boolean isDynamic() {
+        return true;
+    }
+
+    @Override
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             int hue = (initialHue + (i * 180 / buffer.getLength())) % 180;
             buffer.setHSV(i, hue, 255, 128);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
index 497dbfbc..8db0fef3 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -27,7 +27,7 @@ public Leds() {
     @Override
     public void periodic() {
         for (int i = 0; i < ZONE_COUNT;i++) {
-            if (patterns[i].isDynamic) {
+            if (patterns[i].isDynamic()) {
                 patterns[i].apply(buffers[i]);
             }
         }
@@ -36,7 +36,7 @@ public void periodic() {
 
     public void setPattern(Zone zone, LedPattern pattern) {
         patterns[zone.ordinal()] = pattern;
-        if (!pattern.isDynamic) {
+        if (!pattern.isDynamic()) {
             pattern.apply(buffers[zone.ordinal()]);
         }
     }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
new file mode 100644
index 00000000..b66ce211
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
@@ -0,0 +1,27 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.util.Color;
+
+import java.util.function.BiConsumer;
+
+public class SimpleLedPattern extends LedPattern{
+    private final BiConsumer<ZonedAddressableLEDBuffer, Integer> applier;
+    private SimpleLedPattern(BiConsumer<ZonedAddressableLEDBuffer, Integer> applier) {
+        super(false);
+        this.applier = applier;
+    }
+
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        for (int i = 0; i < buffer.getLength(); i++) {
+            this.applier.accept(buffer, i);
+        }
+    }
+
+    public static LedPattern solid(Color color) {
+        return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, color));
+    }
+    public static LedPattern alternating(Color... colors) {
+        final int colorCount = colors.length;
+        return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, colors[i%colorCount]));
+    }
+}

From c5945c4abc1db59d7e7c4d4a2f6d8b20d34641bc Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Mon, 5 Feb 2024 11:09:48 -0800
Subject: [PATCH 05/14] feat: add alternating group sizes

---
 .../team1540/robot2024/subsystems/led/SimpleLedPattern.java   | 4 ++++
 .../robot2024/subsystems/led/ZonedAddressableLEDBuffer.java   | 1 -
 2 files changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
index b66ce211..3863a7e9 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
@@ -24,4 +24,8 @@ public static LedPattern alternating(Color... colors) {
         final int colorCount = colors.length;
         return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, colors[i%colorCount]));
     }
+    public static LedPattern alternating(int groupSize, Color... colors) {
+        final int colorCount = colors.length;
+        return new SimpleLedPattern((buffer, i) -> buffer.setLED(i, colors[(i/groupSize)%colorCount]));
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
index 99c39bc8..692d78a7 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
@@ -30,7 +30,6 @@ public void setHSV(int index, int h, int s, int v) {
     }
 
     public void setLED(int index, Color color) {
-        System.out.println(this.getAbsoluteIndex(index));
         buffer.setLED(this.getAbsoluteIndex(index), color);
     }
 

From af4aa655b930ebcdb2a4f21d6ad5ffb0ce903de3 Mon Sep 17 00:00:00 2001
From: "David :D" <KongD@catlin.edu>
Date: Mon, 5 Feb 2024 18:50:14 -0800
Subject: [PATCH 06/14] feat: Fire and breathing LED patterns :D

---
 .../org/team1540/robot2024/Constants.java     |  4 +-
 .../java/org/team1540/robot2024/Robot.java    | 11 ++-
 .../team1540/robot2024/RobotContainer.java    | 74 +++++++++---------
 .../subsystems/led/LedPatternBreathing.java   | 33 ++++++++
 .../subsystems/led/LedPatternFlame.java       | 76 +++++++++++++++++++
 .../subsystems/led/LedPatternRainbow.java     |  2 +-
 .../robot2024/subsystems/led/Leds.java        |  2 +-
 .../subsystems/tramp/TrampIOSparkMax.java     |  2 +-
 8 files changed, 158 insertions(+), 46 deletions(-)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index cef5d80d..d0d75e42 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -20,7 +20,7 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
-    public static final int LED_STRIP_PORT_PWM = 0;
+    public static final int LED_STRIP_PORT_PWM = 1;
     public static final int LED_STRIP_LENGTH= 80;
     public enum Mode {
         /**
@@ -78,7 +78,7 @@ public static class Indexer {
         public static final double INTAKE_GEAR_RATIO = 1.0;
         public static final double INTAKE_MOI = 0.025;
         public static final double FEEDER_MOI = 0.025;
-        public static final int BEAM_BREAK_ID = 0;
+        public static final int BEAM_BREAK_ID = 3;
 
 
     }
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 7acbcc26..9dde1ec1 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -51,7 +51,7 @@ public void robotInit() {
         switch (Constants.currentMode) {
             case REAL:
                 // Running on a real robot, log to a USB stick ("/U/logs")
-                Logger.addDataReceiver(new WPILOGWriter());
+//                Logger.addDataReceiver(new WPILOGWriter());
                 Logger.addDataReceiver(new NT4Publisher());
                 break;
 
@@ -96,6 +96,7 @@ public void robotPeriodic() {
 
         // Update mechanism visualiser in sim
         if (Robot.isSimulation()) MechanismVisualiser.periodic();
+//        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
     }
 
     /**
@@ -143,7 +144,9 @@ public void teleopInit() {
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove
         // this line or comment it out.
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.alternating(Color.kBlueViolet, Color.kGreen));
+
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternBreathing(1));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternBreathing(1));
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
@@ -161,8 +164,8 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternWave());
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternWave());
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternFlame(82));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternFlame(82));
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 79158466..6eaffbb0 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -64,7 +64,7 @@ public class RobotContainer {
     public final CommandXboxController copilot = new CommandXboxController(1);
 
     // Dashboard inputs
-    public final LoggedDashboardChooser<Command> autoChooser;
+//    public final LoggedDashboardChooser<Command> autoChooser;
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
@@ -75,29 +75,29 @@ public class RobotContainer {
      */
     public RobotContainer() {
         switch (Constants.currentMode) {
-            case REAL:
-                // Real robot, instantiate hardware IO implementations
-                drivetrain =
-                        new Drivetrain(
-                                new GyroIOPigeon2(odometrySignalRefresher),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
-                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
-                tramp = new Tramp(new TrampIOSparkMax());
-                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
-                elevator = new Elevator(new ElevatorIOTalonFX());
-                indexer =
-                        new Indexer(
-                                new IndexerIOSparkMax()
-                        );
-                aprilTagVision = new AprilTagVision(
-                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
-                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
-                        drivetrain::addVisionMeasurement,
-                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
-                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
-                break;
+//            case REAL:
+//                // Real robot, instantiate hardware IO implementations
+//                drivetrain =
+//                        new Drivetrain(
+//                                new GyroIOPigeon2(odometrySignalRefresher),
+//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
+//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
+//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
+//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
+//                tramp = new Tramp(new TrampIOSparkMax());
+//                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+//                elevator = new Elevator(new ElevatorIOTalonFX());
+//                indexer =
+//                        new Indexer(
+//                                new IndexerIOSparkMax()
+//                        );
+//                aprilTagVision = new AprilTagVision(
+//                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
+//                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
+//                        drivetrain::addVisionMeasurement,
+//                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+//                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
+//                break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
                 drivetrain =
@@ -150,20 +150,20 @@ public RobotContainer() {
 
 
         // Set up auto routines
-        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
-
-        // Set up FF characterization routines
-        autoChooser.addOption(
-                "Drive FF Characterization",
-                new FeedForwardCharacterization(
-                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
-        autoChooser.addOption(
-                "Flywheels FF Characterization",
-                new FeedForwardCharacterization(
-                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
+//        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+//
+//        // Set up FF characterization routines
+//        autoChooser.addOption(
+//                "Drive FF Characterization",
+//                new FeedForwardCharacterization(
+//                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
+//        autoChooser.addOption(
+//                "Flywheels FF Characterization",
+//                new FeedForwardCharacterization(
+//                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
 
         // Configure the button bindings
-        configureButtonBindings();
+//        configureButtonBindings();
     }
 
     /**
@@ -197,6 +197,6 @@ private void configureButtonBindings() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        return autoChooser.get();
+        return null;
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
new file mode 100644
index 00000000..f1dae5d2
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
@@ -0,0 +1,33 @@
+package org.team1540.robot2024.subsystems.led;
+
+public class LedPatternBreathing extends LedPattern{
+    private final int speed;
+    private int saturation;
+    private boolean isReversed = false;
+    public LedPatternBreathing(int speed) {
+        super(true);
+        this.speed = speed;
+    }
+    @Override
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        if (saturation > 255) {
+            saturation = 255;
+            isReversed = !isReversed;
+        }
+
+        if (saturation < 0) {
+            saturation = 0;
+            isReversed = !isReversed;
+        }
+        for (int i = 0; i < buffer.getLength(); i++) {
+            buffer.setHSV(i, 0 , saturation, 255);
+        }
+        if (!isReversed) {
+            saturation = saturation + speed;
+        } else {
+            saturation = saturation - speed;
+        }
+    }
+
+
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
new file mode 100644
index 00000000..b2b71465
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
@@ -0,0 +1,76 @@
+package org.team1540.robot2024.subsystems.led;
+
+
+import java.awt.*;
+import java.util.Random;
+
+public class LedPatternFlame extends LedPattern{
+    private int COOLING;
+    private int SPARKING = 123;
+    private boolean reverseDirection = true;
+    private int[] temperatures = new int[40];
+
+    private static Random generator = new Random();
+
+
+    public LedPatternFlame(int cooling) {
+        super(true);
+        COOLING = cooling;
+    }
+
+    public LedPatternFlame() {
+        super(true);
+        COOLING = 62;
+    }
+
+
+    @Override
+    void apply(ZonedAddressableLEDBuffer buffer) {
+
+        for (int i = 0; i < buffer.getLength(); i++) {
+            temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((COOLING * 10) / buffer.getLength()) + 2));
+        }
+
+        for (int k = buffer.getLength() - 1; k >= 2; k--) {
+            temperatures[k] = (temperatures[k - 1] + temperatures[k - 2] + temperatures[k - 2]) / 3;
+        }
+
+        if (getRandomInt(0, 255) < SPARKING) {
+            int y = getRandomInt(0, 6);
+            temperatures[y] = bit8Addition(temperatures[y], getRandomInt(160, 255));
+        }
+
+        for (int j = 0; j < buffer.getLength(); j++) {
+            int temperature = temperatures[j];
+            Color color = getHeatColor(temperature);
+            int pixelnumber = reverseDirection ? (buffer.getLength() - 1) - j : j;
+            buffer.setRGB(pixelnumber, color.getRed(), color.getGreen(), color.getBlue());
+        }
+    }
+    private Color getHeatColor(int temperature) {
+        Color finalColor;
+        int scaledTemperature = (temperature * 191) / 255;
+        int theHeat = scaledTemperature & 0x3F;
+        theHeat <<= 2;
+        if ((scaledTemperature & 0x80) != 0) {
+            finalColor = new Color(255, 255, theHeat);
+        } else if (((scaledTemperature & 0x40) != 0)) {
+            finalColor = new Color(255, theHeat, 0);
+        } else {
+            finalColor = new Color(theHeat, 0, 0);
+        }
+        return finalColor;
+    }
+
+    private int getRandomInt(int min, int max) {
+        return generator.nextInt(max - min + 1) + min;
+    }
+
+    private int bit8Subtraction(int a, int b) {
+        return (a < b) ? 0 : (a - b);
+    }
+
+    private int bit8Addition(int a, int b) {
+        return Math.min((a + b), 255);
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
index 484c1f80..3c51f1b0 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -17,7 +17,7 @@ public boolean isDynamic() {
     @Override
     public void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
-            int hue = (initialHue + (i * 180 / buffer.getLength())) % 180;
+            int hue = (initialHue + (i * 182 / buffer.getLength())) % 180;
             buffer.setHSV(i, hue, 255, 128);
         }
         initialHue += speed;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
index 8db0fef3..bc2c9c2a 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -20,7 +20,7 @@ public Leds() {
         strip.setData(ledBuffer);
         strip.start();
 
-        buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 0, 40, false);
+        buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 40, false);
         buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
     }
 
diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
index 7b91cddf..b6cdb459 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
@@ -7,7 +7,7 @@
 import org.team1540.robot2024.Constants.Tramp;
 
 public class TrampIOSparkMax implements TrampIO {
-    private final DigitalInput beamBreak = new DigitalInput(0);
+    private final DigitalInput beamBreak = new DigitalInput(4);
     //TODO: Potentially change name :D
     private final CANSparkMax motor = new CANSparkMax(Tramp.TRAMP_MOTOR_ID, MotorType.kBrushless);
     private final RelativeEncoder motorEncoder = motor.getEncoder();

From 26f78fb1233481291714c96dad6671ad19dc71a7 Mon Sep 17 00:00:00 2001
From: "David :D" <KongD@catlin.edu>
Date: Mon, 5 Feb 2024 21:40:30 -0800
Subject: [PATCH 07/14] feat: RSL LEDs

---
 .../java/org/team1540/robot2024/Robot.java    |  4 ++--
 .../subsystems/led/LedPatternBreathing.java   |  1 +
 .../subsystems/led/LedPatternFlame.java       | 12 +++++-----
 .../subsystems/led/LedPatternRSLState.java    | 22 +++++++++++++++++++
 4 files changed, 31 insertions(+), 8 deletions(-)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 9dde1ec1..62feab58 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -145,8 +145,8 @@ public void teleopInit() {
         // continue until interrupted by another command, remove
         // this line or comment it out.
 
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternBreathing(1));
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternBreathing(1));
+        robotContainer.leds.setPattern(Leds.Zone.ZONE1, new LedPatternFlame());
+        robotContainer.leds.setPattern(Leds.Zone.ZONE2, new LedPatternFlame());
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
index f1dae5d2..323ac7d3 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
@@ -22,6 +22,7 @@ void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             buffer.setHSV(i, 0 , saturation, 255);
         }
+
         if (!isReversed) {
             saturation = saturation + speed;
         } else {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
index b2b71465..48ea043b 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
@@ -5,8 +5,8 @@
 import java.util.Random;
 
 public class LedPatternFlame extends LedPattern{
-    private int COOLING;
-    private int SPARKING = 123;
+    private final int cooling;
+    private final int sparking = 123;
     private boolean reverseDirection = true;
     private int[] temperatures = new int[40];
 
@@ -15,12 +15,12 @@ public class LedPatternFlame extends LedPattern{
 
     public LedPatternFlame(int cooling) {
         super(true);
-        COOLING = cooling;
+        this.cooling = cooling;
     }
 
     public LedPatternFlame() {
         super(true);
-        COOLING = 62;
+        cooling = 62;
     }
 
 
@@ -28,14 +28,14 @@ public LedPatternFlame() {
     void apply(ZonedAddressableLEDBuffer buffer) {
 
         for (int i = 0; i < buffer.getLength(); i++) {
-            temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((COOLING * 10) / buffer.getLength()) + 2));
+            temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((cooling * 10) / buffer.getLength()) + 2));
         }
 
         for (int k = buffer.getLength() - 1; k >= 2; k--) {
             temperatures[k] = (temperatures[k - 1] + temperatures[k - 2] + temperatures[k - 2]) / 3;
         }
 
-        if (getRandomInt(0, 255) < SPARKING) {
+        if (getRandomInt(0, 255) < sparking) {
             int y = getRandomInt(0, 6);
             temperatures[y] = bit8Addition(temperatures[y], getRandomInt(160, 255));
         }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
new file mode 100644
index 00000000..a6d31925
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
@@ -0,0 +1,22 @@
+package org.team1540.robot2024.subsystems.led;
+
+import edu.wpi.first.wpilibj.RobotController;
+
+public class LedPatternRSLState extends LedPattern {
+    private LedPattern pattern1;
+    private LedPattern pattern2;
+    public LedPatternRSLState(LedPattern pattern1, LedPattern pattern2) {
+        super(true);
+        this.pattern1 = pattern1;
+        this.pattern2 = pattern2;
+    }
+
+    @Override
+    void apply(ZonedAddressableLEDBuffer buffer) {
+        if (RobotController.getRSLState()) {
+            pattern1.apply(buffer);
+        } else {
+            pattern2.apply(buffer);
+        }
+    }
+}
\ No newline at end of file

From 8ec200fc5bbf242619ef99eb6a22ea6dafa513f6 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Mon, 5 Feb 2024 19:00:15 -0800
Subject: [PATCH 08/14] feat: allow hue selection based on Color

---
 .../robot2024/subsystems/led/LedPattern.java  | 29 +++++++++++++++++++
 .../subsystems/led/LedPatternBreathing.java   |  9 ++++--
 .../subsystems/led/LedPatternFlame.java       | 11 ++++---
 .../subsystems/led/LedPatternRainbow.java     |  2 +-
 .../subsystems/led/LedPatternWave.java        |  8 +++--
 5 files changed, 48 insertions(+), 11 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
index 7c179ff4..4bafffab 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
@@ -1,5 +1,7 @@
 package org.team1540.robot2024.subsystems.led;
 
+import edu.wpi.first.wpilibj.util.Color;
+
 public abstract class LedPattern {
     private final boolean isDynamic;
 
@@ -13,4 +15,31 @@ boolean isDynamic() {
 
     abstract void apply(ZonedAddressableLEDBuffer buffer);
 
+    protected static int getHue(Color color) {
+        final int red = (int)color.red*255;
+        final int green = (int)color.green*255;
+        final int blue = (int)color.blue*255;
+        float min = Math.min(Math.min(red, green), blue);
+        float max = Math.max(Math.max(red, green), blue);
+
+        if (min == max) {
+            return 0;
+        }
+
+        float hue = 0f;
+        if (max == red) {
+            hue = (green - blue) / (max - min);
+
+        } else if (max == green) {
+            hue = 2f + (blue - red) / (max - min);
+
+        } else {
+            hue = 4f + (red - green) / (max - min);
+        }
+
+        hue = hue * 60;
+        if (hue < 0) hue = hue + 360;
+
+        return Math.round(hue);
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
index 323ac7d3..57ca6595 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
@@ -1,12 +1,17 @@
 package org.team1540.robot2024.subsystems.led;
 
+import edu.wpi.first.wpilibj.util.Color;
+
 public class LedPatternBreathing extends LedPattern{
     private final int speed;
+    private final int hue;
     private int saturation;
     private boolean isReversed = false;
-    public LedPatternBreathing(int speed) {
+    public LedPatternBreathing(int speed, Color color) {
+
         super(true);
         this.speed = speed;
+        this.hue = getHue(color);
     }
     @Override
     void apply(ZonedAddressableLEDBuffer buffer) {
@@ -20,7 +25,7 @@ void apply(ZonedAddressableLEDBuffer buffer) {
             isReversed = !isReversed;
         }
         for (int i = 0; i < buffer.getLength(); i++) {
-            buffer.setHSV(i, 0 , saturation, 255);
+            buffer.setHSV(i, hue, saturation, 255);
         }
 
         if (!isReversed) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
index 48ea043b..e68a0eb5 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
@@ -5,12 +5,12 @@
 import java.util.Random;
 
 public class LedPatternFlame extends LedPattern{
+    private static final Random generator = new Random();
+
     private final int cooling;
     private final int sparking = 123;
-    private boolean reverseDirection = true;
-    private int[] temperatures = new int[40];
-
-    private static Random generator = new Random();
+    private final boolean reverseDirection = true;
+    private final int[] temperatures = new int[40];
 
 
     public LedPatternFlame(int cooling) {
@@ -19,8 +19,7 @@ public LedPatternFlame(int cooling) {
     }
 
     public LedPatternFlame() {
-        super(true);
-        cooling = 62;
+        this(62);
     }
 
 
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
index 3c51f1b0..98a1810d 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -15,7 +15,7 @@ public boolean isDynamic() {
     }
 
     @Override
-    public void apply(ZonedAddressableLEDBuffer buffer) {
+    void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             int hue = (initialHue + (i * 182 / buffer.getLength())) % 180;
             buffer.setHSV(i, hue, 255, 128);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
index c5a3e6fc..bd9d1862 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
@@ -1,13 +1,17 @@
 package org.team1540.robot2024.subsystems.led;
 
+import edu.wpi.first.wpilibj.util.Color;
+
 public class LedPatternWave extends LedPattern {
     private static final double degradation = 0.7;
     private static final int delay = 5;
+    private final int hue;
     private int location = 0;
     private int ticker = 0;
 
-    public LedPatternWave() {
+    public LedPatternWave(Color color) {
         super(true);
+        this.hue = getHue(color);
     }
 
     @Override
@@ -21,7 +25,7 @@ void apply(ZonedAddressableLEDBuffer buffer) {
                     distance = buffer.getLength() -i + location;
                 }
             }
-            buffer.setHSV(i, 100, 255, (int) Math.max(255.0-distance*distance*degradation,0));
+            buffer.setHSV(i, hue, 255, (int) Math.max(255.0-distance*distance*degradation,0));
         }
         ticker++;
         if (ticker % delay == 0) {

From 2fba8a2437ad3305f8eb77188479ada8a0437a7d Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Mon, 5 Feb 2024 21:39:06 -0800
Subject: [PATCH 09/14] refactor: clean up pattern files

---
 .../team1540/robot2024/subsystems/led/LedPattern.java  |  8 ++++----
 .../robot2024/subsystems/led/LedPatternBreathing.java  |  5 +++--
 .../robot2024/subsystems/led/LedPatternFlame.java      |  3 ++-
 .../robot2024/subsystems/led/LedPatternRainbow.java    |  5 -----
 .../robot2024/subsystems/led/LedPatternWave.java       | 10 +++++-----
 5 files changed, 14 insertions(+), 17 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
index 4bafffab..bb42b045 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
@@ -9,16 +9,16 @@ protected LedPattern(boolean isDynamic) {
         this.isDynamic = isDynamic;
     }
 
-    boolean isDynamic() {
+    final boolean isDynamic() {
         return isDynamic;
     }
 
     abstract void apply(ZonedAddressableLEDBuffer buffer);
 
     protected static int getHue(Color color) {
-        final int red = (int)color.red*255;
-        final int green = (int)color.green*255;
-        final int blue = (int)color.blue*255;
+        final int red = (int) color.red * 255;
+        final int green = (int) color.green * 255;
+        final int blue = (int) color.blue * 255;
         float min = Math.min(Math.min(red, green), blue);
         float max = Math.max(Math.max(red, green), blue);
 
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
index 57ca6595..94bfcc7c 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
@@ -2,17 +2,18 @@
 
 import edu.wpi.first.wpilibj.util.Color;
 
-public class LedPatternBreathing extends LedPattern{
+public class LedPatternBreathing extends LedPattern {
     private final int speed;
     private final int hue;
     private int saturation;
     private boolean isReversed = false;
-    public LedPatternBreathing(int speed, Color color) {
 
+    public LedPatternBreathing(int speed, Color color) {
         super(true);
         this.speed = speed;
         this.hue = getHue(color);
     }
+
     @Override
     void apply(ZonedAddressableLEDBuffer buffer) {
         if (saturation > 255) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
index e68a0eb5..a0b20551 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
@@ -4,7 +4,7 @@
 import java.awt.*;
 import java.util.Random;
 
-public class LedPatternFlame extends LedPattern{
+public class LedPatternFlame extends LedPattern {
     private static final Random generator = new Random();
 
     private final int cooling;
@@ -46,6 +46,7 @@ void apply(ZonedAddressableLEDBuffer buffer) {
             buffer.setRGB(pixelnumber, color.getRed(), color.getGreen(), color.getBlue());
         }
     }
+
     private Color getHeatColor(int temperature) {
         Color finalColor;
         int scaledTemperature = (temperature * 191) / 255;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
index 98a1810d..62b5162e 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
@@ -9,11 +9,6 @@ public LedPatternRainbow(int speed) {
         this.speed = speed;
     }
 
-    @Override
-    public boolean isDynamic() {
-        return true;
-    }
-
     @Override
     void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
index bd9d1862..f063d517 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
@@ -17,15 +17,15 @@ public LedPatternWave(Color color) {
     @Override
     void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
-            int distance = Math.abs(location-i);
-            if (distance > buffer.getLength()/2) {
+            int distance = Math.abs(location - i);
+            if (distance > buffer.getLength() / 2) {
                 if (location > i) {
-                    distance = buffer.getLength()-location + i;
+                    distance = buffer.getLength() - location + i;
                 } else {
-                    distance = buffer.getLength() -i + location;
+                    distance = buffer.getLength() - i + location;
                 }
             }
-            buffer.setHSV(i, hue, 255, (int) Math.max(255.0-distance*distance*degradation,0));
+            buffer.setHSV(i, hue, 255, (int) Math.max(255.0 - distance * distance * degradation, 0));
         }
         ticker++;
         if (ticker % delay == 0) {

From 102fd7bee0201320a76a24e91d5b5cf632da960c Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Tue, 6 Feb 2024 07:43:59 -0800
Subject: [PATCH 10/14] feat: add triaging

---
 .../team1540/robot2024/RobotContainer.java    | 86 +++++++++++--------
 .../subsystems/led/LedPatternRSLState.java    | 22 -----
 .../robot2024/subsystems/led/LedTriager.java  | 38 ++++++++
 .../robot2024/subsystems/led/Leds.java        | 47 ++++++++--
 .../led/ZonedAddressableLEDBuffer.java        |  8 +-
 .../led/{ => patterns}/LedPattern.java        |  8 +-
 .../{ => patterns}/LedPatternBreathing.java   |  5 +-
 .../led/{ => patterns}/LedPatternFlame.java   | 15 +++-
 .../led/patterns/LedPatternRSLState.java      | 23 +++++
 .../led/{ => patterns}/LedPatternRainbow.java |  6 +-
 .../led/{ => patterns}/LedPatternWave.java    |  5 +-
 .../led/{ => patterns}/SimpleLedPattern.java  |  5 +-
 12 files changed, 182 insertions(+), 86 deletions(-)
 delete mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/LedPattern.java (78%)
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/LedPatternBreathing.java (82%)
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/LedPatternFlame.java (84%)
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/LedPatternRainbow.java (71%)
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/LedPatternWave.java (84%)
 rename src/main/java/org/team1540/robot2024/subsystems/led/{ => patterns}/SimpleLedPattern.java (84%)

diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 6eaffbb0..20ef767d 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -3,13 +3,14 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
 import org.team1540.robot2024.commands.FeedForwardCharacterization;
 import org.team1540.robot2024.commands.SwerveDriveCommand;
@@ -17,7 +18,6 @@
 import org.team1540.robot2024.commands.elevator.ElevatorSetpointCommand;
 import org.team1540.robot2024.commands.indexer.IntakeCommand;
 import org.team1540.robot2024.commands.shooter.ShootSequence;
-import org.team1540.robot2024.commands.shooter.TuneShooterCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.elevator.Elevator;
 import org.team1540.robot2024.subsystems.elevator.ElevatorIO;
@@ -28,6 +28,7 @@
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSim;
 import org.team1540.robot2024.subsystems.indexer.IndexerIOSparkMax;
 import org.team1540.robot2024.subsystems.led.Leds;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
 import org.team1540.robot2024.subsystems.shooter.*;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
 import org.team1540.robot2024.subsystems.tramp.TrampIO;
@@ -64,7 +65,7 @@ public class RobotContainer {
     public final CommandXboxController copilot = new CommandXboxController(1);
 
     // Dashboard inputs
-//    public final LoggedDashboardChooser<Command> autoChooser;
+    public final LoggedDashboardChooser<Command> autoChooser;
 
     public final PhoenixTimeSyncSignalRefresher odometrySignalRefresher = new PhoenixTimeSyncSignalRefresher(SwerveConfig.CAN_BUS);
 
@@ -75,29 +76,29 @@ public class RobotContainer {
      */
     public RobotContainer() {
         switch (Constants.currentMode) {
-//            case REAL:
-//                // Real robot, instantiate hardware IO implementations
-//                drivetrain =
-//                        new Drivetrain(
-//                                new GyroIOPigeon2(odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
-//                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
-//                tramp = new Tramp(new TrampIOSparkMax());
-//                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
-//                elevator = new Elevator(new ElevatorIOTalonFX());
-//                indexer =
-//                        new Indexer(
-//                                new IndexerIOSparkMax()
-//                        );
-//                aprilTagVision = new AprilTagVision(
-//                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
-//                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
-//                        drivetrain::addVisionMeasurement,
-//                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
-//                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
-//                break;
+            case REAL:
+                // Real robot, instantiate hardware IO implementations
+                drivetrain =
+                        new Drivetrain(
+                                new GyroIOPigeon2(odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_LEFT, SwerveFactory.SwerveCorner.FRONT_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.FRONT_RIGHT, SwerveFactory.SwerveCorner.FRONT_RIGHT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
+                                new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
+                tramp = new Tramp(new TrampIOSparkMax());
+                shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+                elevator = new Elevator(new ElevatorIOTalonFX());
+                indexer =
+                        new Indexer(
+                                new IndexerIOSparkMax()
+                        );
+                aprilTagVision = new AprilTagVision(
+                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
+                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
+                        drivetrain::addVisionMeasurement,
+                        () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+                        new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
+                break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
                 drivetrain =
@@ -150,22 +151,31 @@ public RobotContainer() {
 
 
         // Set up auto routines
-//        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
-//
-//        // Set up FF characterization routines
-//        autoChooser.addOption(
-//                "Drive FF Characterization",
-//                new FeedForwardCharacterization(
-//                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
-//        autoChooser.addOption(
-//                "Flywheels FF Characterization",
-//                new FeedForwardCharacterization(
-//                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
+        autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
+
+        // Set up FF characterization routines
+        autoChooser.addOption(
+                "Drive FF Characterization",
+                new FeedForwardCharacterization(
+                        drivetrain, drivetrain::runCharacterizationVolts, drivetrain::getCharacterizationVelocity));
+        autoChooser.addOption(
+                "Flywheels FF Characterization",
+                new FeedForwardCharacterization(
+                        shooter, volts -> shooter.setFlywheelVolts(volts, volts), () -> shooter.getLeftFlywheelSpeed() / 60));
 
         // Configure the button bindings
-//        configureButtonBindings();
+        configureButtonBindings();
+        configureLedBindings();
     }
 
+    private void configureLedBindings() {
+        leds.setFatalPattern(new LedPatternFlame());
+        new Trigger(DriverStation::isDSAttached)
+                .onTrue(Commands.runOnce(leds::clearFatalPattern)
+                            .ignoringDisable(true))
+                .onFalse(Commands.runOnce(() -> leds.setFatalPattern(new LedPatternFlame()))
+                            .ignoringDisable(true));
+    }
     /**
      * Use this method to define your button->command mappings. Buttons can be created by
      * instantiating a {@link GenericHID} or one of its subclasses ({@link
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
deleted file mode 100644
index a6d31925..00000000
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRSLState.java
+++ /dev/null
@@ -1,22 +0,0 @@
-package org.team1540.robot2024.subsystems.led;
-
-import edu.wpi.first.wpilibj.RobotController;
-
-public class LedPatternRSLState extends LedPattern {
-    private LedPattern pattern1;
-    private LedPattern pattern2;
-    public LedPatternRSLState(LedPattern pattern1, LedPattern pattern2) {
-        super(true);
-        this.pattern1 = pattern1;
-        this.pattern2 = pattern2;
-    }
-
-    @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
-        if (RobotController.getRSLState()) {
-            pattern1.apply(buffer);
-        } else {
-            pattern2.apply(buffer);
-        }
-    }
-}
\ No newline at end of file
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java
new file mode 100644
index 00000000..4cacde1c
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/LedTriager.java
@@ -0,0 +1,38 @@
+package org.team1540.robot2024.subsystems.led;
+
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;
+
+
+public class LedTriager {
+    private final LedPattern[] patterns = new LedPattern[Leds.CRITICALITY_COUNT];
+    private final LedPattern defaultPattern = new LedPatternRainbow(1);
+    private boolean isNew = true;
+    public LedPattern getPattern() {
+        for (int i = patterns.length -1; i >= 0; i--) {
+            if (patterns[i] != null) {
+                return patterns[i];
+            }
+        }
+        return defaultPattern;
+    }
+
+    public boolean shouldRefresh() {
+        final boolean val = isNew || getPattern().isDynamic();
+        isNew = false;
+        return val;
+    }
+
+    public void clearPattern(Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = null;
+        isNew = true;
+    }
+
+    public boolean addPattern(LedPattern pattern, Leds.PatternCriticality criticality) {
+        patterns[criticality.ordinal()] = pattern;
+        isNew = true;
+        return getPattern() == pattern;
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
index bc2c9c2a..c20b589d 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -4,16 +4,16 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
 
 
 import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;
 
 public class Leds extends SubsystemBase {
-
     private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.LED_STRIP_LENGTH);
     private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
     private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
-    private final LedPattern[] patterns = new LedPattern[ZONE_COUNT];
+    private final LedTriager[] patterns = new LedTriager[ZONE_COUNT];
 
     public Leds() {
         strip.setLength(ledBuffer.getLength());
@@ -22,29 +22,62 @@ public Leds() {
 
         buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 40, false);
         buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
+        for (int i = 0; i < ZONE_COUNT;i++) {
+            patterns[i] = new LedTriager();
+        }
     }
 
     @Override
     public void periodic() {
         for (int i = 0; i < ZONE_COUNT;i++) {
-            if (patterns[i].isDynamic()) {
-                patterns[i].apply(buffers[i]);
+            if (patterns[i].shouldRefresh()) {
+                patterns[i].getPattern().apply(buffers[i]);
             }
         }
         strip.setData(ledBuffer);
     }
 
+    public void setPattern(Zone zone, LedPattern pattern, PatternCriticality criticality) {
+        patterns[zone.ordinal()].addPattern(pattern, criticality);
+        pattern.setLength(buffers[zone.ordinal()].getLength());
+    }
+
     public void setPattern(Zone zone, LedPattern pattern) {
-        patterns[zone.ordinal()] = pattern;
-        if (!pattern.isDynamic()) {
-            pattern.apply(buffers[zone.ordinal()]);
+        setPattern(zone, pattern, PatternCriticality.INFO);
+    }
+
+    public void clearPattern(Zone zone, PatternCriticality criticality) {
+        patterns[zone.ordinal()].clearPattern(criticality);
+    }
+
+    public void setFatalPattern(LedPattern pattern) {
+        System.out.println("Setting criticality");
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            patterns[i].addPattern(pattern, PatternCriticality.FATAL);
+            pattern.setLength(buffers[i].getLength());
         }
     }
 
+    public void clearFatalPattern() {
+        System.out.println("Clearing criticality");
+        for (int i = 0; i<ZONE_COUNT;i++) {
+            patterns[i].clearPattern(PatternCriticality.FATAL);
+        }
+
+    }
+
 
     private static final int ZONE_COUNT=Zone.values().length;
     public enum Zone {
         ZONE1,
         ZONE2;
     }
+    static final int CRITICALITY_COUNT=PatternCriticality.values().length;
+    public enum PatternCriticality {
+        DECORATION,
+        INFO,
+        CRITICAL,
+        FATAL
+    }
+
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
index 692d78a7..f7337cf2 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/ZonedAddressableLEDBuffer.java
@@ -5,10 +5,10 @@
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
-class ZonedAddressableLEDBuffer {
-    public final boolean isInverted;
-    public final int start;
-    public final int length;
+public class ZonedAddressableLEDBuffer {
+    private final boolean isInverted;
+    private final int start;
+    private final int length;
     private final AddressableLEDBuffer buffer;
 
     public ZonedAddressableLEDBuffer(AddressableLEDBuffer buffer, int start, int end, boolean isInverted) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java
similarity index 78%
rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java
index bb42b045..1328f009 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPattern.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public abstract class LedPattern {
     private final boolean isDynamic;
@@ -9,11 +10,12 @@ protected LedPattern(boolean isDynamic) {
         this.isDynamic = isDynamic;
     }
 
-    final boolean isDynamic() {
+    public final boolean isDynamic() {
         return isDynamic;
     }
 
-    abstract void apply(ZonedAddressableLEDBuffer buffer);
+    public abstract void apply(ZonedAddressableLEDBuffer buffer);
+    public void setLength(int length) {}
 
     protected static int getHue(Color color) {
         final int red = (int) color.red * 255;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java
similarity index 82%
rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java
index 94bfcc7c..57a0de3c 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternBreathing.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternBreathing.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public class LedPatternBreathing extends LedPattern {
     private final int speed;
@@ -15,7 +16,7 @@ public LedPatternBreathing(int speed, Color color) {
     }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         if (saturation > 255) {
             saturation = 255;
             isReversed = !isReversed;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
similarity index 84%
rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
index a0b20551..b79bb4a2 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternFlame.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
@@ -1,16 +1,19 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
+
 import java.awt.*;
 import java.util.Random;
 
 public class LedPatternFlame extends LedPattern {
     private static final Random generator = new Random();
+    private static final boolean reverseDirection = true;
 
     private final int cooling;
     private final int sparking = 123;
-    private final boolean reverseDirection = true;
-    private final int[] temperatures = new int[40];
+
+    private int[] temperatures;
 
 
     public LedPatternFlame(int cooling) {
@@ -22,9 +25,13 @@ public LedPatternFlame() {
         this(62);
     }
 
+    @Override
+    public void setLength(int length) {
+        this.temperatures = new int[length];
+    }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
 
         for (int i = 0; i < buffer.getLength(); i++) {
             temperatures[i] = bit8Subtraction(temperatures[i], getRandomInt(0, ((cooling * 10) / buffer.getLength()) + 2));
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
new file mode 100644
index 00000000..36ac5ed7
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
@@ -0,0 +1,23 @@
+package org.team1540.robot2024.subsystems.led.patterns;
+
+import edu.wpi.first.wpilibj.RobotController;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
+
+public class LedPatternRSLState extends LedPattern {
+    private final LedPattern a;
+    private final LedPattern b;
+    public LedPatternRSLState(LedPattern a, LedPattern b) {
+        super(true);
+        this.a = a;
+        this.b = b;
+    }
+
+    @Override
+    public void apply(ZonedAddressableLEDBuffer buffer) {
+        if (RobotController.getRSLState()) {
+            a.apply(buffer);
+        } else {
+            b.apply(buffer);
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java
similarity index 71%
rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java
index 62b5162e..83abeab9 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternRainbow.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRainbow.java
@@ -1,4 +1,6 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
+
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public class LedPatternRainbow extends LedPattern {
     private final int speed;
@@ -10,7 +12,7 @@ public LedPatternRainbow(int speed) {
     }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             int hue = (initialHue + (i * 182 / buffer.getLength())) % 180;
             buffer.setHSV(i, hue, 255, 128);
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java
similarity index 84%
rename from src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java
index f063d517..41f45c94 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/LedPatternWave.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternWave.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public class LedPatternWave extends LedPattern {
     private static final double degradation = 0.7;
@@ -15,7 +16,7 @@ public LedPatternWave(Color color) {
     }
 
     @Override
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             int distance = Math.abs(location - i);
             if (distance > buffer.getLength() / 2) {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java
similarity index 84%
rename from src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
rename to src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java
index 3863a7e9..7ff0a952 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/SimpleLedPattern.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/SimpleLedPattern.java
@@ -1,6 +1,7 @@
-package org.team1540.robot2024.subsystems.led;
+package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.util.Color;
+import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 import java.util.function.BiConsumer;
 
@@ -11,7 +12,7 @@ private SimpleLedPattern(BiConsumer<ZonedAddressableLEDBuffer, Integer> applier)
         this.applier = applier;
     }
 
-    void apply(ZonedAddressableLEDBuffer buffer) {
+    public void apply(ZonedAddressableLEDBuffer buffer) {
         for (int i = 0; i < buffer.getLength(); i++) {
             this.applier.accept(buffer, i);
         }

From 0335fe4eb516e2d0103e6fc077e013a94cc43baf Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Tue, 13 Feb 2024 07:45:45 -0800
Subject: [PATCH 11/14] feat: fix bindings, add led tuning mode

---
 .../java/org/team1540/robot2024/Constants.java |  2 +-
 .../java/org/team1540/robot2024/Robot.java     | 18 +++++++++++++-----
 .../org/team1540/robot2024/RobotContainer.java |  4 ++--
 .../robot2024/subsystems/led/Leds.java         | 14 ++++++--------
 .../led/patterns/LedPatternFlame.java          |  2 +-
 .../led/patterns/LedPatternRSLState.java       |  5 +++++
 6 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index d0d75e42..c1bf8e9a 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -20,7 +20,7 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
-    public static final int LED_STRIP_PORT_PWM = 1;
+    public static final int LED_STRIP_PORT_PWM = 9;
     public static final int LED_STRIP_LENGTH= 80;
     public enum Mode {
         /**
diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 62feab58..9f9f1772 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -11,6 +11,11 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 import org.team1540.robot2024.subsystems.led.*;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternFlame;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRSLState;
+import org.team1540.robot2024.subsystems.led.patterns.LedPatternRainbow;
+import org.team1540.robot2024.subsystems.led.patterns.SimpleLedPattern;
+import org.team1540.robot2024.util.LoggedTunableNumber;
 import org.team1540.robot2024.util.MechanismVisualiser;
 
 /**
@@ -23,6 +28,10 @@ public class Robot extends LoggedRobot {
     private Command autonomousCommand;
     private RobotContainer robotContainer;
 
+    LoggedTunableNumber led_r = new LoggedTunableNumber("led/r", 0);
+    LoggedTunableNumber led_g = new LoggedTunableNumber("led/g", 0);
+    LoggedTunableNumber led_b = new LoggedTunableNumber("led/b", 0);
+
     /**
      * This function is run when the robot is first started up and should be used for any
      * initialization code.
@@ -104,8 +113,7 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1, SimpleLedPattern.solid(Color.kBlueViolet));
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2, SimpleLedPattern.solid(Color.kPaleVioletRed));
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternFlame());
     }
 
     /**
@@ -120,7 +128,7 @@ public void disabledPeriodic() {
      */
     @Override
     public void autonomousInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,SimpleLedPattern.alternating(Color.kBlueViolet, Color.kCrimson));
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,LedPatternRSLState.matchingColors());
         autonomousCommand = robotContainer.getAutonomousCommand();
         // schedule the autonomous command (example)
         if (autonomousCommand != null) {
@@ -164,8 +172,7 @@ public void teleopPeriodic() {
      */
     @Override
     public void testInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1,new LedPatternFlame(82));
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2,new LedPatternFlame(82));
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,new LedPatternRainbow(1));
         // Cancels all running commands at the start of test mode.
         CommandScheduler.getInstance().cancelAll();
     }
@@ -175,6 +182,7 @@ public void testInit() {
      */
     @Override
     public void testPeriodic() {
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK,SimpleLedPattern.solid(new Color(led_r.get(), led_g.get(), led_b.get())));
     }
 
     /**
diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java
index 20ef767d..73acdd58 100644
--- a/src/main/java/org/team1540/robot2024/RobotContainer.java
+++ b/src/main/java/org/team1540/robot2024/RobotContainer.java
@@ -169,11 +169,11 @@ public RobotContainer() {
     }
 
     private void configureLedBindings() {
-        leds.setFatalPattern(new LedPatternFlame());
+        leds.setFatalPattern(LedPatternFlame::new);
         new Trigger(DriverStation::isDSAttached)
                 .onTrue(Commands.runOnce(leds::clearFatalPattern)
                             .ignoringDisable(true))
-                .onFalse(Commands.runOnce(() -> leds.setFatalPattern(new LedPatternFlame()))
+                .onFalse(Commands.runOnce(() -> leds.setFatalPattern(LedPatternFlame::new))
                             .ignoringDisable(true));
     }
     /**
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
index c20b589d..8f7b7bdc 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -7,6 +7,8 @@
 import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
 
 
+import java.util.function.Supplier;
+
 import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;
 
 public class Leds extends SubsystemBase {
@@ -20,8 +22,7 @@ public Leds() {
         strip.setData(ledBuffer);
         strip.start();
 
-        buffers[Zone.ZONE1.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 40, false);
-        buffers[Zone.ZONE2.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 40, 80, false);
+        buffers[Zone.ELEVATOR_BACK.ordinal()] = new ZonedAddressableLEDBuffer(ledBuffer, 1, 41, false);
         for (int i = 0; i < ZONE_COUNT;i++) {
             patterns[i] = new LedTriager();
         }
@@ -50,27 +51,24 @@ public void clearPattern(Zone zone, PatternCriticality criticality) {
         patterns[zone.ordinal()].clearPattern(criticality);
     }
 
-    public void setFatalPattern(LedPattern pattern) {
-        System.out.println("Setting criticality");
+    public void setFatalPattern(Supplier<LedPattern> patternSupplier) {
         for (int i = 0; i<ZONE_COUNT;i++) {
+            LedPattern pattern = patternSupplier.get();
             patterns[i].addPattern(pattern, PatternCriticality.FATAL);
             pattern.setLength(buffers[i].getLength());
         }
     }
 
     public void clearFatalPattern() {
-        System.out.println("Clearing criticality");
         for (int i = 0; i<ZONE_COUNT;i++) {
             patterns[i].clearPattern(PatternCriticality.FATAL);
         }
-
     }
 
 
     private static final int ZONE_COUNT=Zone.values().length;
     public enum Zone {
-        ZONE1,
-        ZONE2;
+        ELEVATOR_BACK,
     }
     static final int CRITICALITY_COUNT=PatternCriticality.values().length;
     public enum PatternCriticality {
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
index b79bb4a2..4ebd6c15 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternFlame.java
@@ -8,7 +8,7 @@
 
 public class LedPatternFlame extends LedPattern {
     private static final Random generator = new Random();
-    private static final boolean reverseDirection = true;
+    private static final boolean reverseDirection = false;
 
     private final int cooling;
     private final int sparking = 123;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
index 36ac5ed7..b212c315 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/patterns/LedPatternRSLState.java
@@ -1,6 +1,7 @@
 package org.team1540.robot2024.subsystems.led.patterns;
 
 import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.util.Color;
 import org.team1540.robot2024.subsystems.led.ZonedAddressableLEDBuffer;
 
 public class LedPatternRSLState extends LedPattern {
@@ -20,4 +21,8 @@ public void apply(ZonedAddressableLEDBuffer buffer) {
             b.apply(buffer);
         }
     }
+
+    public static LedPattern matchingColors() {
+        return new LedPatternRSLState(SimpleLedPattern.solid(new Color(1, 0.1, 0)), SimpleLedPattern.solid(Color.kBlack));
+    }
 }
\ No newline at end of file

From 86ab759e8d19f5f9da95a05854ef6ca5d676cca4 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Wed, 14 Feb 2024 15:52:10 -0800
Subject: [PATCH 12/14] fix: oops

---
 src/main/java/org/team1540/robot2024/Robot.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 9f9f1772..689a81b4 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -153,8 +153,7 @@ public void teleopInit() {
         // continue until interrupted by another command, remove
         // this line or comment it out.
 
-        robotContainer.leds.setPattern(Leds.Zone.ZONE1, new LedPatternFlame());
-        robotContainer.leds.setPattern(Leds.Zone.ZONE2, new LedPatternFlame());
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, LedPatternRSLState.matchingColors());
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }

From 060e9d926d97a975f1cdfb5463bc9f0e6e29ec3d Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Wed, 14 Feb 2024 16:19:28 -0800
Subject: [PATCH 13/14] fix: chill out on disabled

---
 src/main/java/org/team1540/robot2024/Robot.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/org/team1540/robot2024/Robot.java b/src/main/java/org/team1540/robot2024/Robot.java
index 689a81b4..e7486d27 100644
--- a/src/main/java/org/team1540/robot2024/Robot.java
+++ b/src/main/java/org/team1540/robot2024/Robot.java
@@ -113,7 +113,7 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledInit() {
-        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternFlame());
+        robotContainer.leds.setPattern(Leds.Zone.ELEVATOR_BACK, new LedPatternRainbow(1));
     }
 
     /**

From 5e6016b25dd91a0c287de08096b9eb210554cfc0 Mon Sep 17 00:00:00 2001
From: Zach R <zarutman+git@gmail.com>
Date: Wed, 14 Feb 2024 16:28:11 -0800
Subject: [PATCH 14/14] refactor: move constants around

---
 src/main/java/org/team1540/robot2024/Constants.java        | 6 ++++--
 .../java/org/team1540/robot2024/subsystems/led/Leds.java   | 7 +++----
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index d2c1c3de..13f2a4cd 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -23,8 +23,10 @@ public final class Constants {
     private static final Mode simMode = Mode.SIM; // Can also be Mode.REPLAY
 
     public static final Mode currentMode = Robot.isReal() ? Mode.REAL : simMode;
-    public static final int LED_STRIP_PORT_PWM = 9;
-    public static final int LED_STRIP_LENGTH= 80;
+    public static final class Leds {
+        public static final int LED_STRIP_PORT_PWM = 9;
+        public static final int LED_STRIP_LENGTH= 80;
+    }
     public enum Mode {
         /**
          * Running on a real robot.
diff --git a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
index 8f7b7bdc..f4746aa3 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/led/Leds.java
@@ -3,16 +3,15 @@
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import org.team1540.robot2024.Constants;
-import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
 
+import org.team1540.robot2024.subsystems.led.patterns.LedPattern;
 
 import java.util.function.Supplier;
 
-import static org.team1540.robot2024.Constants.LED_STRIP_PORT_PWM;
+import static org.team1540.robot2024.Constants.Leds.*;
 
 public class Leds extends SubsystemBase {
-    private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.LED_STRIP_LENGTH);
+    private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(LED_STRIP_LENGTH);
     private final AddressableLED strip = new AddressableLED(LED_STRIP_PORT_PWM);
     private final ZonedAddressableLEDBuffer[] buffers = new ZonedAddressableLEDBuffer[ZONE_COUNT];
     private final LedTriager[] patterns = new LedTriager[ZONE_COUNT];