LEDs

src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java

@@ -0,0 +1,209 @@
+package org.trigon.hardware.misc.leds;
+
+import com.ctre.phoenix.led.LarsonAnimation;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.util.Color;
+
+import java.util.function.Supplier;
+
+/**
+ * A LED strip that is controlled by an AddressableLED.
+ */
+public class AddressableLEDStrip extends LEDStrip {
+    private static AddressableLED LED;
+    private static AddressableLEDBuffer LED_BUFFER;
+
+    private int lastBreatheLED;
+    private double lastLEDAnimationChangeTime = 0;
+    private double rainbowFirstPixelHue = 0;
+    private boolean isLEDAnimationChanged = false;
+    private int amountOfColorFlowLEDs = 0;
+
+    /**
+     * Sets and configures the AddressableLED and AddressableLEDBuffer instances to be used for controlling the LED strip.
+     * Must be set before using any LED strips. Should only be called once.
+     *
+     * @param port              the port of the LED strip
+     * @param totalAmountOfLEDs the total amount of LEDs in all LED strips
+     */
+    public static void initiateAddressableLED(int port, int totalAmountOfLEDs) {
+        if (LED_BUFFER == null)
+            LED_BUFFER = new AddressableLEDBuffer(totalAmountOfLEDs);
+
+        if (LED == null) {
+            LED = new AddressableLED(port);
+            LED.setLength(totalAmountOfLEDs);
+            LED.start();
+        }
+    }
+
+    /**
+     * Constructs a new AddressableLEDStrip. Before any commands are sent to the LED strip, the setAddressableLED and setAddressableLEDBuffer methods must be called.
+     *
+     * @param inverted     whether the LED strip is inverted
+     * @param numberOfLEDs the amount of LEDs in the strip
+     * @param indexOffset  the offset of the first LED in the strip
+     */
+    AddressableLEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) {
+        super(inverted, numberOfLEDs, indexOffset);
+        resetLEDSettings();
+    }
+
+    @Override
+    public void periodic() {
+        currentAnimation.run();
+        LED.setData(LED_BUFFER);
+    }
+
+    @Override
+    void clearLEDColors() {
+        staticColor(Color.kBlack);
+    }
+
+    @Override
+    void blink(Color firstColor, double speed) {
+        final double correctedSpeed = 1 - speed;
+        final double currentTime = Timer.getFPGATimestamp();
+
+        if (currentTime - lastLEDAnimationChangeTime > correctedSpeed) {
+            lastLEDAnimationChangeTime = currentTime;
+            isLEDAnimationChanged = !isLEDAnimationChanged;
+        }
+
+        if (isLEDAnimationChanged) {
+            staticColor(firstColor);
+            return;
+        }
+        clearLEDColors();
+    }
+
+    @Override
+    void staticColor(Color color) {
+        setLEDColors(color, 0, numberOfLEDs - 1);
+    }
+
+    @Override
+    void breathe(Color color, int breathingLEDs, double speed, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
+        clearLEDColors();
+        final boolean correctedInverted = this.inverted != inverted;
+        final double moveLEDTimeSeconds = 1 - speed;
+        final double currentTime = Timer.getFPGATimestamp();
+
+        if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
+            lastLEDAnimationChangeTime = currentTime;
+            if (correctedInverted)
+                lastBreatheLED--;
+            else
+                lastBreatheLED++;
+        }
+
+        checkIfBreathingHasHitEnd(breathingLEDs, correctedInverted, bounceMode);
+        setBreathingLEDs(color, breathingLEDs, bounceMode);
+    }
+
+    @Override
+    void colorFlow(Color color, double speed, boolean inverted) {
+        clearLEDColors();
+        final boolean correctedInverted = this.inverted != inverted;
+        final double moveLEDTimeSeconds = 1 - speed;
+        final double currentTime = Timer.getFPGATimestamp();
+
+        if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
+            lastLEDAnimationChangeTime = currentTime;
+            if (isLEDAnimationChanged)
+                amountOfColorFlowLEDs--;
+            else
+                amountOfColorFlowLEDs++;
+        }
+
+        checkIfColorFlowHasHitEnd();
+        setLEDColors(color, correctedInverted ? numberOfLEDs - amountOfColorFlowLEDs - 1 : 0, correctedInverted ? numberOfLEDs - 1 : amountOfColorFlowLEDs);
+    }
+
+    @Override
+    void alternateColor(Color firstColor, Color secondColor) {
+        for (int i = 0; i < numberOfLEDs; i++)
+            LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? firstColor : secondColor);
+    }
+
+    @Override
+    void rainbow(double brightness, double speed, boolean inverted) {
+        final boolean correctedInverted = this.inverted != inverted;
+        final int adjustedBrightness = (int) (brightness * 255);
+        final int hueIncrement = (int) (speed * 8);
+
+        for (int led = 0; led < numberOfLEDs; led++) {
+            final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180);
+            LED_BUFFER.setHSV(led + indexOffset, hue, 255, adjustedBrightness);
+        }
+
+        if (correctedInverted) {
+            rainbowFirstPixelHue -= hueIncrement;
+            if (rainbowFirstPixelHue < 0)
+                rainbowFirstPixelHue += 180;
+            return;
+        }
+        rainbowFirstPixelHue += hueIncrement;
+        rainbowFirstPixelHue %= 180;
+    }
+
+    @Override
+    void sectionColor(Supplier<Color>[] colors) {
+        final int amountOfSections = colors.length;
+        final int LEDsPerSection = (int) Math.floor(numberOfLEDs / amountOfSections);
+
+        for (int i = 0; i < amountOfSections; i++)
+            setLEDColors(
+                    inverted ? colors[amountOfSections - i - 1].get() : colors[i].get(),
+                    LEDsPerSection * i,
+                    i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDsPerSection * (i + 1) - 1
+            );
+    }
+
+    @Override
+    void resetLEDSettings() {
+        lastBreatheLED = indexOffset;
+        lastLEDAnimationChangeTime = Timer.getFPGATimestamp();
+        rainbowFirstPixelHue = 0;
+        isLEDAnimationChanged = false;
+        amountOfColorFlowLEDs = 0;
+    }
+
+    private void checkIfBreathingHasHitEnd(int amountOfBreathingLEDs, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
+        final int bounceModeAddition = switch (bounceMode) {
+            case Back -> amountOfBreathingLEDs;
+            case Center -> amountOfBreathingLEDs / 2;
+            default -> 0;
+        };
+
+        if (inverted ? (lastBreatheLED < indexOffset + bounceModeAddition) : (lastBreatheLED >= numberOfLEDs + indexOffset + bounceModeAddition))
+            lastBreatheLED = inverted ? indexOffset + numberOfLEDs : indexOffset;
+    }
+
+    private void setBreathingLEDs(Color color, int breathingLEDs, LarsonAnimation.BounceMode bounceMode) {
+        for (int i = 0; i < breathingLEDs; i++) {
+            if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs)
+                LED_BUFFER.setLED(lastBreatheLED - i, color);
+
+            else if (lastBreatheLED - i < indexOffset + numberOfLEDs) {
+                if (bounceMode.equals(LarsonAnimation.BounceMode.Back) || bounceMode.equals(LarsonAnimation.BounceMode.Center) && i > breathingLEDs / 2)
+                    return;
+                LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color);
+            }
+        }
+    }
+
+    private void checkIfColorFlowHasHitEnd() {
+        if (amountOfColorFlowLEDs >= numberOfLEDs || amountOfColorFlowLEDs < 0) {
+            amountOfColorFlowLEDs = amountOfColorFlowLEDs < 0 ? amountOfColorFlowLEDs + 1 : amountOfColorFlowLEDs - 1;
+            isLEDAnimationChanged = !isLEDAnimationChanged;
+        }
+    }
+
+    private void setLEDColors(Color color, int startIndex, int endIndex) {
+        for (int i = 0; i <= endIndex - startIndex; i++)
+            LED_BUFFER.setLED(startIndex + indexOffset + i, color);
+    }
+}

src/main/java/org/trigon/hardware/misc/leds/CANdleLEDStrip.java

@@ -0,0 +1,159 @@
+package org.trigon.hardware.misc.leds;
+
+import com.ctre.phoenix.led.*;
+import edu.wpi.first.wpilibj.util.Color;
+import org.trigon.hardware.RobotHardwareStats;
+
+import java.util.function.Supplier;
+
+/**
+ * A LED strip that is controlled by a CANdle, and uses AddressableLED for simulation.
+ */
+public class CANdleLEDStrip extends LEDStrip {
+    private static CANdle CANDLE;
+    private static int LAST_CREATED_LED_STRIP_ANIMATION_SLOT = 0;
+    private final int animationSlot;
+
+    /**
+     * Sets the CANdle instance to be used for controlling the LED strips. Must be set before using any LED strips. Should only be called once
+     *
+     * @param candle the CANdle instance to be used
+     */
+    public static void setCANdle(CANdle candle) {
+        if (CANDLE == null)
+            CANDLE = candle;
+    }
+
+    /**
+     * Sets the total amount of LEDs in all LED strips for simulation.
+     * Must be set before using any LED strips in simulation. Should only be called once.
+     *
+     * @param totalAmountOfLEDs the total amount of LEDs in all LED strips
+     */
+    public static void setTotalAmountOfLEDs(int totalAmountOfLEDs) {
+        if (RobotHardwareStats.isSimulation() || RobotHardwareStats.isReplay())
+            AddressableLEDStrip.initiateAddressableLED(0, totalAmountOfLEDs);
+    }
+
+    /**
+     * Constructs a new CANdleLEDStrip. Before any commands are sent to the LED strip, the setCANdle method must be called.
+     *
+     * @param inverted     whether the LED strip is inverted
+     * @param numberOfLEDs the amount of LEDs in the strip
+     * @param indexOffset  the offset of the first LED in the strip
+     */
+    CANdleLEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) {
+        super(inverted, numberOfLEDs, indexOffset);
+        animationSlot = LAST_CREATED_LED_STRIP_ANIMATION_SLOT;
+        LAST_CREATED_LED_STRIP_ANIMATION_SLOT++;
+    }
+
+    @Override
+    void clearLEDColors() {
+        CANDLE.clearAnimation(animationSlot);
+    }
+
+    @Override
+    void blink(Color firstColor, double speed) {
+        CANDLE.animate(
+                new SingleFadeAnimation(
+                        (int) firstColor.red,
+                        (int) firstColor.green,
+                        (int) firstColor.blue,
+                        0,
+                        speed,
+                        this.numberOfLEDs,
+                        indexOffset
+                ),
+                animationSlot
+        );
+    }
+
+    @Override
+    void staticColor(Color color) {
+        CANDLE.setLEDs((int) color.red, (int) color.green, (int) color.blue, 0, indexOffset, numberOfLEDs);
+    }
+
+    @Override
+    void breathe(Color color, int amountOfBreathingLEDs, double speed, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
+        CANDLE.animate(
+                new LarsonAnimation(
+                        (int) color.red,
+                        (int) color.green,
+                        (int) color.blue,
+                        0,
+                        speed,
+                        this.numberOfLEDs,
+                        bounceMode,
+                        amountOfBreathingLEDs,
+                        indexOffset
+                ),
+                animationSlot
+        );
+    }
+
+    @Override
+    void alternateColor(Color firstColor, Color secondColor) {
+        for (int i = 0; i < numberOfLEDs; i++)
+            CANDLE.setLEDs(
+                    (int) (i % 2 == 0 ? firstColor.red : secondColor.red),
+                    (int) (i % 2 == 0 ? firstColor.green : secondColor.green),
+                    (int) (i % 2 == 0 ? firstColor.blue : secondColor.blue),
+                    0,
+                    i + indexOffset,
+                    1
+            );
+    }
+
+    @Override
+    void colorFlow(Color color, double speed, boolean inverted) {
+        final boolean correctedInverted = this.inverted != inverted;
+        CANDLE.animate(
+                new ColorFlowAnimation(
+                        (int) color.red,
+                        (int) color.green,
+                        (int) color.blue,
+                        0,
+                        speed,
+                        this.numberOfLEDs,
+                        correctedInverted ? ColorFlowAnimation.Direction.Backward : ColorFlowAnimation.Direction.Forward,
+                        indexOffset
+                ),
+                animationSlot
+        );
+    }
+
+    @Override
+    void rainbow(double brightness, double speed, boolean inverted) {
+        final boolean correctedInverted = this.inverted != inverted;
+        CANDLE.animate(
+                new RainbowAnimation(
+                        brightness,
+                        speed,
+                        this.numberOfLEDs,
+                        correctedInverted,
+                        indexOffset
+                ),
+                animationSlot
+        );
+    }
+
+    @Override
+    void sectionColor(Supplier<Color>[] colors) {
+        final int LEDSPerSection = (int) Math.floor(numberOfLEDs / colors.length);
+        setSectionColor(colors.length, LEDSPerSection, colors);
+    }
+
+    private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier<Color>[] colors) {
+        for (int i = 0; i < amountOfSections; i++) {
+            CANDLE.setLEDs(
+                    (int) (inverted ? colors[amountOfSections - i - 1].get().red : colors[i].get().red),
+                    (int) (inverted ? colors[amountOfSections - i - 1].get().green : colors[i].get().green),
+                    (int) (inverted ? colors[amountOfSections - i - 1].get().blue : colors[i].get().blue),
+                    0,
+                    LEDSPerSection * i + indexOffset,
+                    i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1
+            );
+        }
+    }
+}