Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add LED pattern API for easily animating addressable LEDs (#6344)

Browse Source 
Add LEDReader and LEDWriter helper interfaces to facilitate composing simple patterns into more complex ones, e.g. LEDPattern.solid(Color.kBlue).breathe(Seconds.of(0.75)). Pattern composition relies on changing out the write behavior; for example, offsetBy increments the indexes to write to; while blink will switch between playing a base pattern and turning off all the LEDs.

Add a view class for splitting a single large buffer into smaller distinct sections, which is useful for dealing with long chained LED strips mounted on different parts of a robot. Views cannot be written directly to an LED strip (in fact, trying to do so won't even compile).

Adds some utility methods to the Color class for interpolating between two colors, and support color representations with 32-bit integers to avoid object allocations.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Sam Carlberg
2024-06-05 22:41:10 -04:00
committed by GitHub
parent 5221069bcc
commit ad18fa62ee
19 changed files with 3610 additions and 311 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
View File

@@ -4,11 +4,8 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
/** Buffer storage for Addressable LEDs. */
public class AddressableLEDBuffer {
public class AddressableLEDBuffer implements LEDReader, LEDWriter {
byte[] m_buffer;
/**
@@ -28,6 +25,7 @@ public class AddressableLEDBuffer {
* @param g the g value [0-255]
* @param b the b value [0-255]
*/
@Override
public void setRGB(int index, int r, int g, int b) {
m_buffer[index * 4] = (byte) b;
m_buffer[(index * 4) + 1] = (byte) g;
@@ -35,109 +33,23 @@ public class AddressableLEDBuffer {
m_buffer[(index * 4) + 3] = 0;
}
/**
* Sets a specific led in the buffer.
*
* @param index the index to write
* @param h the h value [0-180)
* @param s the s value [0-255]
* @param v the v value [0-255]
*/
public void setHSV(final int index, final int h, final int s, final int v) {
if (s == 0) {
setRGB(index, v, v, v);
return;
}
// The below algorithm is copied from Color.fromHSV and moved here for
// performance reasons.
// Loosely based on
// https://en.wikipedia.org/wiki/HSL_and_HSV#HSV_to_RGB
// The hue range is split into 60 degree regions where in each region there
// is one rgb component at a low value (m), one at a high value (v) and one
// that changes (X) from low to high (X+m) or high to low (v-X)
// Difference between highest and lowest value of any rgb component
final int chroma = (s * v) / 255;
// Because hue is 0-180 rather than 0-360 use 30 not 60
final int region = (h / 30) % 6;
// Remainder converted from 0-30 to 0-255
final int remainder = (int) Math.round((h % 30) * (255 / 30.0));
// Value of the lowest rgb component
final int m = v - chroma;
// Goes from 0 to chroma as hue increases
final int X = (chroma * remainder) >> 8;
switch (region) {
case 0 -> setRGB(index, v, X + m, m);
case 1 -> setRGB(index, v - X, v, m);
case 2 -> setRGB(index, m, v, X + m);
case 3 -> setRGB(index, m, v - X, v);
case 4 -> setRGB(index, X + m, m, v);
default -> setRGB(index, v, m, v - X);
}
}
/**
* Sets a specific LED in the buffer.
*
* @param index The index to write
* @param color The color of the LED
*/
public void setLED(int index, Color color) {
setRGB(index, (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
}
/**
* Sets a specific LED in the buffer.
*
* @param index The index to write
* @param color The color of the LED
*/
public void setLED(int index, Color8Bit color) {
setRGB(index, color.red, color.green, color.blue);
}
/**
* Gets the buffer length.
*
* @return the buffer length
*/
@Override
public int getLength() {
return m_buffer.length / 4;
}
/**
* Gets the color at the specified index.
*
* @param index the index to get
* @return the LED color at the specified index
*/
public Color8Bit getLED8Bit(int index) {
return new Color8Bit(getRed(index), getGreen(index), getBlue(index));
}
/**
* Gets the color at the specified index.
*
* @param index the index to get
* @return the LED color at the specified index
*/
public Color getLED(int index) {
return new Color(getRed(index) / 255.0, getGreen(index) / 255.0, getBlue(index) / 255.0);
}
/**
* Gets the red channel of the color at the specified index.
*
* @param index the index of the LED to read
* @return the value of the red channel, from [0, 255]
*/
@Override
public int getRed(int index) {
return m_buffer[index * 4 + 2] & 0xFF;
}
@@ -148,6 +60,7 @@ public class AddressableLEDBuffer {
* @param index the index of the LED to read
* @return the value of the green channel, from [0, 255]
*/
@Override
public int getGreen(int index) {
return m_buffer[index * 4 + 1] & 0xFF;
}
@@ -158,41 +71,22 @@ public class AddressableLEDBuffer {
* @param index the index of the LED to read
* @return the value of the blue channel, from [0, 255]
*/
@Override
public int getBlue(int index) {
return m_buffer[index * 4] & 0xFF;
}
/**
* A functional interface that allows for iteration over an LED buffer without manually writing an
* indexed for-loop.
*/
@FunctionalInterface
public interface IndexedColorIterator {
/**
* Accepts an index of an LED in the buffer and the red, green, and blue components of the
* currently stored color for that LED.
*
* @param index the index of the LED in the buffer that the red, green, and blue channels
* corresponds to
* @param r the value of the red channel of the color currently in the buffer at index {@code i}
* @param g the value of the green channel of the color currently in the buffer at index {@code
* i}
* @param b the value of the blue channel of the color currently in the buffer at index {@code
* i}
*/
void accept(int index, int r, int g, int b);
}
/**
* Iterates over the LEDs in the buffer, starting from index 0. The iterator function is passed
* the current index of iteration, along with the values for the red, green, and blue components
* of the color written to the LED at that index.
* Creates a view of a subsection of this data buffer, starting from (and including) {@code
* startingIndex} and ending on (and including) {@code endingIndex}. Views cannot be written
* directly to an {@link AddressableLED}, but are useful tools for logically separating different
* sections of an LED strip for independent control.
*
* @param iterator the iterator function to call for each LED in the buffer.
* @param startingIndex the first index in this buffer that the view should encompass (inclusive)
* @param endingIndex the last index in this buffer that the view should encompass (inclusive)
* @return the view object
*/
public void forEach(IndexedColorIterator iterator) {
for (int i = 0; i < getLength(); i++) {
iterator.accept(i, getRed(i), getGreen(i), getBlue(i));
}
public AddressableLEDBufferView createView(int startingIndex, int endingIndex) {
return new AddressableLEDBufferView(this, startingIndex, endingIndex);
}
}

163
wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBufferView.java Normal file
View File

@@ -0,0 +1,163 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
/**
* A view of another addressable LED buffer. Views CANNOT be written directly to an LED strip; the
* backing buffer must be written instead. However, views provide an easy way to split a large LED
* strip into smaller sections (which may be reversed from the orientation of the LED strip as a
* whole) that can be animated individually without modifying LEDs outside those sections.
*/
public class AddressableLEDBufferView implements LEDReader, LEDWriter {
private final LEDReader m_backingReader;
private final LEDWriter m_backingWriter;
private final int m_startingIndex;
private final int m_endingIndex;
private final int m_length;
/**
* Creates a new view of a buffer. A view will be reversed if the starting index is after the
* ending index; writing front-to-back in the view will write in the back-to-front direction on
* the underlying buffer.
*
* @param backingBuffer the backing buffer to view
* @param startingIndex the index of the LED in the backing buffer that the view should start from
* @param endingIndex the index of the LED in the backing buffer that the view should end on
* @param <B> the type of the buffer object to create a view for
*/
public <B extends LEDReader & LEDWriter> AddressableLEDBufferView(
B backingBuffer, int startingIndex, int endingIndex) {
this(
requireNonNullParam(backingBuffer, "backingBuffer", "AddressableLEDBufferView"),
backingBuffer,
startingIndex,
endingIndex);
}
/**
* Creates a new view of a buffer. A view will be reversed if the starting index is after the
* ending index; writing front-to-back in the view will write in the back-to-front direction on
* the underlying buffer.
*
* @param backingReader the backing LED data reader
* @param backingWriter the backing LED data writer
* @param startingIndex the index of the LED in the backing buffer that the view should start from
* @param endingIndex the index of the LED in the backing buffer that the view should end on
*/
public AddressableLEDBufferView(
LEDReader backingReader, LEDWriter backingWriter, int startingIndex, int endingIndex) {
requireNonNullParam(backingReader, "backingReader", "AddressableLEDBufferView");
requireNonNullParam(backingWriter, "backingWriter", "AddressableLEDBufferView");
if (startingIndex < 0 || startingIndex >= backingReader.getLength()) {
throw new IndexOutOfBoundsException("Start index out of range: " + startingIndex);
}
if (endingIndex < 0 || endingIndex >= backingReader.getLength()) {
throw new IndexOutOfBoundsException("End index out of range: " + endingIndex);
}
m_backingReader = backingReader;
m_backingWriter = backingWriter;
m_startingIndex = startingIndex;
m_endingIndex = endingIndex;
m_length = Math.abs(endingIndex - startingIndex) + 1;
}
/**
* Creates a view that operates on the same range as this one, but goes in reverse order. This is
* useful for serpentine runs of LED strips connected front-to-end; simply reverse the view for
* reversed sections and animations will move in the same physical direction along both strips.
*
* @return the reversed view
*/
public AddressableLEDBufferView reversed() {
return new AddressableLEDBufferView(this, m_length - 1, 0);
}
@Override
public int getLength() {
return m_length;
}
@Override
public void setRGB(int index, int r, int g, int b) {
m_backingWriter.setRGB(nativeIndex(index), r, g, b);
}
@Override
public Color getLED(int index) {
// override to delegate to the backing buffer to avoid 3x native index lookups & bounds checks
return m_backingReader.getLED(nativeIndex(index));
}
@Override
public Color8Bit getLED8Bit(int index) {
// override to delegate to the backing buffer to avoid 3x native index lookups & bounds checks
return m_backingReader.getLED8Bit(nativeIndex(index));
}
@Override
public int getRed(int index) {
return m_backingReader.getRed(nativeIndex(index));
}
@Override
public int getGreen(int index) {
return m_backingReader.getGreen(nativeIndex(index));
}
@Override
public int getBlue(int index) {
return m_backingReader.getBlue(nativeIndex(index));
}
/**
* Checks if this view is reversed with respect to its backing buffer.
*
* @return true if the view is reversed, false otherwise
*/
public boolean isReversed() {
return m_endingIndex < m_startingIndex;
}
/**
* Converts a view-local index in the range [start, end] to a global index in the range [0,
* length].
*
* @param viewIndex the view-local index
* @return the corresponding global index
* @throws IndexOutOfBoundsException if the view index is not contained withing the bounds of this
* view
*/
private int nativeIndex(int viewIndex) {
if (isReversed()) {
// 0 1 2 3 4 5 6 7 8 9 10
// ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓
// [_, _, _, _, (d, c, b, a), _, _, _]
// ↑ ↑ ↑ ↑
// 3 2 1 0
if (viewIndex < 0 || viewIndex > m_startingIndex) {
throw new IndexOutOfBoundsException(viewIndex);
}
return m_startingIndex - viewIndex;
} else {
// 0 1 2 3 4 5 6 7 8 9 10
// ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓ ↓
// [_, _, _, _, (a, b, c, d), _, _, _]
// ↑ ↑ ↑ ↑
// 0 1 2 3
if (viewIndex < 0 || viewIndex > m_endingIndex) {
throw new IndexOutOfBoundsException(viewIndex);
}
return m_startingIndex + viewIndex;
}
}
}

644
wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java Normal file
View File

@@ -0,0 +1,644 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.Microsecond;
import static edu.wpi.first.units.Units.Microseconds;
import static edu.wpi.first.units.Units.Value;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.units.Dimensionless;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.collections.LongToObjectHashMap;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.util.Color;
import java.util.Map;
import java.util.Objects;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
/**
* An LED pattern controls lights on an LED strip to command patterns of color that may change over
* time. Dynamic patterns should synchronize on an external clock for timed-based animations ({@link
* WPIUtilJNI#now()} is recommended, since it can be mocked in simulation and unit tests), or on
* some other dynamic input (see {@link #synchronizedBlink(BooleanSupplier)}, for example).
*
* <p>Patterns should be updated periodically in order for animations to play smoothly. For example,
* a hypothetical LED subsystem could create a {@code Command} that will continuously apply the
* pattern to its LED data buffer as part of the main periodic loop.
*
* <pre><code>
* public class LEDs extends SubsystemBase {
* private final AddressableLED m_led = new AddressableLED(0);
* private final AddressableLEDBuffer m_ledData = new AddressableLEDBuffer(120);
*
* public LEDs() {
* m_led.setLength(120);
* m_led.start();
* }
*
* {@literal @}Override
* public void periodic() {
* m_led.writeData(m_ledData);
* }
*
* public Command runPattern(LEDPattern pattern) {
* return run(() -> pattern.applyTo(m_ledData));
* }
* }
* </code></pre>
*
* <p>LED patterns are stateless, and as such can be applied to multiple LED strips (or different
* sections of the same LED strip, since the roboRIO can only drive a single LED strip). In this
* example, we split the single buffer into two views - one for the section of the LED strip on the
* left side of a robot, and another view for the section of LEDs on the right side. The same
* pattern is able to be applied to both sides.
*
* <pre><code>
* public class LEDs extends SubsystemBase {
* private final AddressableLED m_led = new AddressableLED(0);
* private final AddressableLEDBuffer m_ledData = new AddressableLEDBuffer(60);
* private final AddressableLEDBufferView m_leftData = m_ledData.createView(0, 29);
* private final AddressableLEDBufferView m_rightData = m_ledData.createView(30, 59).reversed();
*
* public LEDs() {
* m_led.setLength(60);
* m_led.start();
* }
*
* {@literal @}Override
* public void periodic() {
* m_led.writeData(m_ledData);
* }
*
* public Command runPattern(LEDPattern pattern) {
* // Use the single input pattern to drive both sides
* return runSplitPatterns(pattern, pattern);
* }
*
* public Command runSplitPatterns(LEDPattern left, LEDPattern right) {
* return run(() -> {
* left.applyTo(m_leftData);
* right.applyTo(m_rightData);
* });
* }
* }
* </code></pre>
*/
@FunctionalInterface
public interface LEDPattern {
/**
* Writes the pattern to an LED buffer. Dynamic animations should be called periodically (such as
* with a command or with a periodic method) to refresh the buffer over time.
*
* <p>This method is intentionally designed to use separate objects for reading and writing data.
* By splitting them up, we can easily modify the behavior of some base pattern to make it {@link
* #scrollAtRelativeSpeed(Measure) scroll}, {@link #blink(Measure, Measure) blink}, or {@link
* #breathe(Measure) breathe} by intercepting the data writes to transform their behavior to
* whatever we like.
*
* @param reader data reader for accessing buffer length and current colors
* @param writer data writer for setting new LED colors on the buffer
*/
void applyTo(LEDReader reader, LEDWriter writer);
/**
* Convenience for {@link #applyTo(LEDReader, LEDWriter)} when one object provides both a read and
* a write interface. This is most helpful for playing an animated pattern directly on an {@link
* AddressableLEDBuffer} for the sake of code clarity.
*
* <pre><code>
* AddressableLEDBuffer data = new AddressableLEDBuffer(120);
* LEDPattern pattern = ...
*
* void periodic() {
* pattern.applyTo(data);
* }
* </code></pre>
*
* @param readWriter the object to use for both reading and writing to a set of LEDs
* @param <T> the type of the object that can both read and write LED data
*/
default <T extends LEDReader & LEDWriter> void applyTo(T readWriter) {
applyTo(readWriter, readWriter);
}
/**
* Creates a pattern that displays this one in reverse. Scrolling patterns will scroll in the
* opposite direction (but at the same speed). It will treat the end of an LED strip as the start,
* and the start of the strip as the end. This can be useful for making ping-pong patterns that
* travel from one end of an LED strip to the other, then reverse direction and move back to the
* start. This can also be useful when working with LED strips connected in a serpentine pattern
* (where the start of one strip is connected to the end of the previous one); however, consider
* using a {@link AddressableLEDBufferView#reversed() reversed view} of the overall buffer for
* that segment rather than reversing patterns.
*
* @return the reverse pattern
* @see AddressableLEDBufferView#reversed()
*/
default LEDPattern reversed() {
return (reader, writer) -> {
int bufLen = reader.getLength();
applyTo(reader, (i, r, g, b) -> writer.setRGB((bufLen - 1) - i, r, g, b));
};
}
/**
* Creates a pattern that plays this one, but offset by a certain number of LEDs. The offset
* pattern will wrap around, if necessary.
*
* @param offset how many LEDs to offset by
* @return the offset pattern
*/
default LEDPattern offsetBy(int offset) {
return (reader, writer) -> {
int bufLen = reader.getLength();
applyTo(
reader,
(i, r, g, b) -> {
int shiftedIndex = Math.floorMod(i + offset, bufLen);
writer.setRGB(shiftedIndex, r, g, b);
});
};
}
/**
* Creates a pattern that plays this one scrolling up the buffer. The velocity controls how fast
* the pattern returns back to its original position, and is in terms of the length of the LED
* strip; scrolling across a segment that is 10 LEDs long will travel twice as fast as on a
* segment that's only 5 LEDs long (assuming equal LED density on both segments).
*
* <p>For example, scrolling a pattern by one quarter of any LED strip's length per second,
* regardless of the total number of LEDs on that strip:
*
* <pre>
* LEDPattern rainbow = LEDPattern.rainbow(255, 255);
* LEDPattern scrollingRainbow = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(25));
* </pre>
*
* @param velocity how fast the pattern should move, in terms of how long it takes to do a full
* scroll along the length of LEDs and return back to the starting position
* @return the scrolling pattern
*/
default LEDPattern scrollAtRelativeSpeed(Measure<Velocity<Dimensionless>> velocity) {
final double periodMicros = 1 / velocity.in(Value.per(Microsecond));
return (reader, writer) -> {
int bufLen = reader.getLength();
long now = WPIUtilJNI.now();
// index should move by (buf.length) / (period)
double t = (now % (long) periodMicros) / periodMicros;
int offset = (int) (t * bufLen);
applyTo(
reader,
(i, r, g, b) -> {
// floorMod so if the offset is negative, we still get positive outputs
int shiftedIndex = Math.floorMod(i + offset, bufLen);
writer.setRGB(shiftedIndex, r, g, b);
});
};
}
/**
* Creates a pattern that plays this one scrolling up an LED strip. A negative velocity makes the
* pattern play in reverse.
*
* <p>For example, scrolling a pattern at 4 inches per second along an LED strip with 60 LEDs per
* meter:
*
* <pre>
* // LEDs per meter, a known value taken from the spec sheet of our particular LED strip
* Measure&lt;Distance&gt; LED_SPACING = Meters.of(1.0 / 60);
*
* LEDPattern rainbow = LEDPattern.rainbow();
* LEDPattern scrollingRainbow =
* rainbow.scrollAtAbsoluteSpeed(InchesPerSecond.of(4), LED_SPACING);
* </pre>
*
* <p>Note that this pattern will scroll <i>faster</i> if applied to a less dense LED strip (such
* as 30 LEDs per meter), or <i>slower</i> if applied to a denser LED strip (such as 120 or 144
* LEDs per meter).
*
* @param velocity how fast the pattern should move along a physical LED strip
* @param ledSpacing the distance between adjacent LEDs on the physical LED strip
* @return the scrolling pattern
*/
default LEDPattern scrollAtAbsoluteSpeed(
Measure<Velocity<Distance>> velocity, Measure<Distance> ledSpacing) {
// eg velocity = 10 m/s, spacing = 0.01m
// meters per micro = 1e-5 m/us
// micros per LED = 1e-2 m / (1e-5 m/us) = 1e-3 us
var metersPerMicro = velocity.in(Meters.per(Microsecond));
var microsPerLED = (int) (ledSpacing.in(Meters) / metersPerMicro);
return (reader, writer) -> {
int bufLen = reader.getLength();
long now = WPIUtilJNI.now();
// every step in time that's a multiple of microsPerLED will increment the offset by 1
var offset = now / microsPerLED;
applyTo(
reader,
(i, r, g, b) -> {
// floorMod so if the offset is negative, we still get positive outputs
int shiftedIndex = Math.floorMod(i + offset, bufLen);
writer.setRGB(shiftedIndex, r, g, b);
});
};
}
/**
* Creates a pattern that switches between playing this pattern and turning the entire LED strip
* off.
*
* @param onTime how long the pattern should play for, per cycle
* @param offTime how long the pattern should be turned off for, per cycle
* @return the blinking pattern
*/
default LEDPattern blink(Measure<Time> onTime, Measure<Time> offTime) {
final long totalTimeMicros = (long) (onTime.in(Microseconds) + offTime.in(Microseconds));
final long onTimeMicros = (long) onTime.in(Microseconds);
return (reader, writer) -> {
if (WPIUtilJNI.now() % totalTimeMicros < onTimeMicros) {
applyTo(reader, writer);
} else {
kOff.applyTo(reader, writer);
}
};
}
/**
* Like {@link #blink(Measure, Measure) blink(onTime, offTime)}, but where the "off" time is
* exactly equal to the "on" time.
*
* @param onTime how long the pattern should play for (and be turned off for), per cycle
* @return the blinking pattern
*/
default LEDPattern blink(Measure<Time> onTime) {
return blink(onTime, onTime);
}
/**
* Creates a pattern that blinks this one on and off in sync with a true/false signal. The pattern
* will play while the signal outputs {@code true}, and will turn off while the signal outputs
* {@code false}.
*
* @param signal the signal to synchronize with
* @return the blinking pattern
*/
default LEDPattern synchronizedBlink(BooleanSupplier signal) {
return (reader, writer) -> {
if (signal.getAsBoolean()) {
applyTo(reader, writer);
} else {
kOff.applyTo(reader, writer);
}
};
}
/**
* Creates a pattern that brightens and dims this one over time. Brightness follows a sinusoidal
* pattern.
*
* @param period how fast the breathing pattern should complete a single cycle
* @return the breathing pattern
*/
default LEDPattern breathe(Measure<Time> period) {
final long periodMicros = (long) period.in(Microseconds);
return (reader, writer) -> {
applyTo(
reader,
(i, r, g, b) -> {
// How far we are in the cycle, in the range [0, 1)
double t = (WPIUtilJNI.now() % periodMicros) / (double) periodMicros;
double phase = t * 2 * Math.PI;
// Apply the cosine function and shift its output from [-1, 1] to [0, 1]
// Use cosine so the period starts at 100% brightness
double dim = (Math.cos(phase) + 1) / 2.0;
int output = Color.lerpRGB(0, 0, 0, r, g, b, dim);
writer.setRGB(
i,
Color.unpackRGB(output, Color.RGBChannel.kRed),
Color.unpackRGB(output, Color.RGBChannel.kGreen),
Color.unpackRGB(output, Color.RGBChannel.kBlue));
});
};
}
/**
* Creates a pattern that plays this pattern overlaid on another. Anywhere this pattern sets an
* LED to off (or {@link Color#kBlack}), the base pattern will be displayed instead.
*
* @param base the base pattern to overlay on top of
* @return the combined overlay pattern
*/
default LEDPattern overlayOn(LEDPattern base) {
return (reader, writer) -> {
// write the base pattern down first...
base.applyTo(reader, writer);
// ... then, overwrite with the illuminated LEDs from the overlay
applyTo(
reader,
(i, r, g, b) -> {
if (r != 0 || g != 0 || b != 0) {
writer.setRGB(i, r, g, b);
}
});
};
}
/**
* Creates a pattern that displays outputs as a combination of this pattern and another. Color
* values are calculated as the average color of both patterns; if both patterns set the same LED
* to the same color, then it is set to that color, but if one pattern sets to one color and the
* other pattern sets it to off, then it will show the color of the first pattern but at
* approximately half brightness. This is different from {@link #overlayOn}, which will show the
* base pattern at full brightness if the overlay is set to off at that position.
*
* @param other the pattern to blend with
* @return the blended pattern
*/
default LEDPattern blend(LEDPattern other) {
return (reader, writer) -> {
applyTo(reader, writer);
other.applyTo(
reader,
(i, r, g, b) -> {
int blendedRGB =
Color.lerpRGB(
reader.getRed(i), reader.getGreen(i), reader.getBlue(i), r, g, b, 0.5);
writer.setRGB(
i,
Color.unpackRGB(blendedRGB, Color.RGBChannel.kRed),
Color.unpackRGB(blendedRGB, Color.RGBChannel.kGreen),
Color.unpackRGB(blendedRGB, Color.RGBChannel.kBlue));
});
};
}
/**
* Similar to {@link #blend(LEDPattern)}, but performs a bitwise mask on each color channel rather
* than averaging the colors for each LED. This can be helpful for displaying only a portion of
* the base pattern by applying a mask that sets the desired area to white, and all other areas to
* black. However, it can also be used to display only certain color channels or hues; for
* example, masking with {@code LEDPattern.color(Color.kRed)} will turn off the green and blue
* channels on the output pattern, leaving only the red LEDs to be illuminated.
*
* @param mask the mask to apply
* @return the masked pattern
*/
default LEDPattern mask(LEDPattern mask) {
return (reader, writer) -> {
// Apply the current pattern down as normal...
applyTo(reader, writer);
mask.applyTo(
reader,
(i, r, g, b) -> {
// ... then perform a bitwise AND operation on each channel to apply the mask
writer.setRGB(i, r & reader.getRed(i), g & reader.getGreen(i), b & reader.getBlue(i));
});
};
}
/**
* Creates a pattern that plays this one, but at a different brightness. Brightness multipliers
* are applied per-channel in the RGB space; no HSL or HSV conversions are applied. Multipliers
* are also uncapped, which may result in the original colors washing out and appearing less
* saturated or even just a bright white.
*
* <p>This method is predominantly intended for dimming LEDs to avoid painfully bright or
* distracting patterns from playing (apologies to the 2024 NE Greater Boston field staff).
*
* <p>For example, dimming can be done simply by adding a call to `atBrightness` at the end of a
* pattern:
*
* <pre>
* // Solid red, but at 50% brightness
* LEDPattern.solid(Color.kRed).atBrightness(Percent.of(50));
*
* // Solid white, but at only 10% (i.e. ~0.5V)
* LEDPattern.solid(Color.kWhite).atBrightness(Percent.of(10));
* </pre>
*
* @param relativeBrightness the multiplier to apply to all channels to modify brightness
* @return the input pattern, displayed at
*/
default LEDPattern atBrightness(Measure<Dimensionless> relativeBrightness) {
double multiplier = relativeBrightness.in(Value);
return (reader, writer) -> {
applyTo(
reader,
(i, r, g, b) -> {
// Clamp RGB values to keep them in the range [0, 255].
// Otherwise, the casts to byte would result in values like 256 wrapping to 0
writer.setRGB(
i,
(int) MathUtil.clamp(r * multiplier, 0, 255),
(int) MathUtil.clamp(g * multiplier, 0, 255),
(int) MathUtil.clamp(b * multiplier, 0, 255));
});
};
}
/** A pattern that turns off all LEDs. */
LEDPattern kOff = solid(Color.kBlack);
/**
* Creates a pattern that displays a single static color along the entire length of the LED strip.
*
* @param color the color to display
* @return the pattern
*/
static LEDPattern solid(Color color) {
return (reader, writer) -> {
int bufLen = reader.getLength();
for (int led = 0; led < bufLen; led++) {
writer.setLED(led, color);
}
};
}
/**
* Creates a pattern that works as a mask layer for {@link #mask(LEDPattern)} that illuminates
* only the portion of the LED strip corresponding with some progress. The mask pattern will start
* from the base and set LEDs to white at a proportion equal to the progress returned by the
* function. Some usages for this could be for displaying progress of a flywheel to its target
* velocity, progress of a complex autonomous sequence, or the height of an elevator.
*
* <p>For example, creating a mask for displaying a red-to-blue gradient, starting from the red
* end, based on where an elevator is in its range of travel.
*
* <pre>
* LEDPattern basePattern = gradient(Color.kRed, Color.kBlue);
* LEDPattern progressPattern =
* basePattern.mask(progressMaskLayer(() -> elevator.getHeight() / elevator.maxHeight());
* </pre>
*
* @param progressSupplier the function to call to determine the progress. This should return
* values in the range [0, 1]; any values outside that range will be clamped.
* @return the mask pattern
*/
static LEDPattern progressMaskLayer(DoubleSupplier progressSupplier) {
return (reader, writer) -> {
double progress = MathUtil.clamp(progressSupplier.getAsDouble(), 0, 1);
int bufLen = reader.getLength();
int max = (int) (bufLen * progress);
for (int led = 0; led < max; led++) {
writer.setLED(led, Color.kWhite);
}
for (int led = max; led < bufLen; led++) {
writer.setLED(led, Color.kBlack);
}
};
}
/**
* Display a set of colors in steps across the length of the LED strip. No interpolation is done
* between colors. Colors are specified by the first LED on the strip to show that color. The last
* color in the map will be displayed all the way to the end of the strip. LEDs positioned before
* the first specified step will be turned off (you can think of this as if there's a 0 -> black
* step by default)
*
* <pre>
* // Display red from 0-33%, white from 33% - 67%, and blue from 67% to 100%
* steps(Map.of(0.00, Color.kRed, 0.33, Color.kWhite, 0.67, Color.kBlue))
*
* // Half off, half on
* steps(Map.of(0.5, Color.kWhite))
* </pre>
*
* @param steps a map of progress to the color to start displaying at that position along the LED
* strip
* @return a motionless step pattern
*/
static LEDPattern steps(Map<? extends Number, Color> steps) {
if (steps.isEmpty()) {
// no colors specified
DriverStation.reportWarning("Creating LED steps with no colors!", false);
return kOff;
}
if (steps.size() == 1 && steps.keySet().iterator().next().doubleValue() == 0) {
// only one color specified, just show a static color
DriverStation.reportWarning("Creating LED steps with only one color!", false);
return solid(steps.values().iterator().next());
}
return (reader, writer) -> {
int bufLen = reader.getLength();
// precompute relevant positions for this buffer so we don't need to do a check
// on every single LED index
var stopPositions = new LongToObjectHashMap<Color>();
steps.forEach(
(progress, color) -> {
stopPositions.put((int) Math.floor(progress.doubleValue() * bufLen), color);
});
Color currentColor = Color.kBlack;
for (int led = 0; led < bufLen; led++) {
currentColor = Objects.requireNonNullElse(stopPositions.get(led), currentColor);
writer.setLED(led, currentColor);
}
};
}
/**
* Creates a pattern that displays a non-animated gradient of colors across the entire length of
* the LED strip. The gradient wraps around so the start and end of the strip are the same color,
* which allows the gradient to be modified with a scrolling effect with no discontinuities.
* Colors are evenly distributed along the full length of the LED strip.
*
* @param colors the colors to display in the gradient
* @return a motionless gradient pattern
*/
static LEDPattern gradient(Color... colors) {
if (colors.length == 0) {
// Nothing to display
DriverStation.reportWarning("Creating a gradient with no colors!", false);
return kOff;
}
if (colors.length == 1) {
// No gradients with one color
DriverStation.reportWarning("Creating a gradient with only one color!", false);
return solid(colors[0]);
}
final int numSegments = colors.length;
return (reader, writer) -> {
int bufLen = reader.getLength();
int ledsPerSegment = bufLen / numSegments;
for (int led = 0; led < bufLen; led++) {
int colorIndex = (led / ledsPerSegment) % numSegments;
int nextColorIndex = (colorIndex + 1) % numSegments;
double t = (led / (double) ledsPerSegment) % 1;
Color color = colors[colorIndex];
Color nextColor = colors[nextColorIndex];
int gradientColor =
Color.lerpRGB(
color.red,
color.green,
color.blue,
nextColor.red,
nextColor.green,
nextColor.blue,
t);
writer.setRGB(
led,
Color.unpackRGB(gradientColor, Color.RGBChannel.kRed),
Color.unpackRGB(gradientColor, Color.RGBChannel.kGreen),
Color.unpackRGB(gradientColor, Color.RGBChannel.kBlue));
}
};
}
/**
* Creates an LED pattern that displays a rainbow across the color wheel. The rainbow pattern will
* stretch across the entire length of the LED strip.
*
* @param saturation the saturation of the HSV colors, in [0, 255]
* @param value the value of the HSV colors, in [0, 255]
* @return the rainbow pattern
*/
static LEDPattern rainbow(int saturation, int value) {
return (reader, writer) -> {
int bufLen = reader.getLength();
for (int i = 0; i < bufLen; i++) {
int hue = ((i * 180) / bufLen) % 180;
writer.setHSV(i, hue, saturation, value);
}
};
}
}

99
wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDReader.java Normal file
View File

@@ -0,0 +1,99 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
/** Generic interface for reading data from an LED buffer. */
public interface LEDReader {
/**
* Gets the length of the buffer.
*
* @return the buffer length
*/
int getLength();
/**
* Gets the most recently written color for a particular LED in the buffer.
*
* @param index the index of the LED
* @return the LED color
* @throws IndexOutOfBoundsException if the index is negative or greater than {@link #getLength()}
*/
default Color getLED(int index) {
return new Color(getRed(index) / 255.0, getGreen(index) / 255.0, getBlue(index) / 255.0);
}
/**
* Gets the most recently written color for a particular LED in the buffer.
*
* @param index the index of the LED
* @return the LED color
* @throws IndexOutOfBoundsException if the index is negative or greater than {@link #getLength()}
*/
default Color8Bit getLED8Bit(int index) {
return new Color8Bit(getRed(index), getGreen(index), getBlue(index));
}
/**
* Gets the red channel of the color at the specified index.
*
* @param index the index of the LED to read
* @return the value of the red channel, from [0, 255]
*/
int getRed(int index);
/**
* Gets the green channel of the color at the specified index.
*
* @param index the index of the LED to read
* @return the value of the green channel, from [0, 255]
*/
int getGreen(int index);
/**
* Gets the blue channel of the color at the specified index.
*
* @param index the index of the LED to read
* @return the value of the blue channel, from [0, 255]
*/
int getBlue(int index);
/**
* A functional interface that allows for iteration over an LED buffer without manually writing an
* indexed for-loop.
*/
@FunctionalInterface
interface IndexedColorIterator {
/**
* Accepts an index of an LED in the buffer and the red, green, and blue components of the
* currently stored color for that LED.
*
* @param index the index of the LED in the buffer that the red, green, and blue channels
* corresponds to
* @param r the value of the red channel of the color currently in the buffer at index {@code i}
* @param g the value of the green channel of the color currently in the buffer at index {@code
* i}
* @param b the value of the blue channel of the color currently in the buffer at index {@code
* i}
*/
void accept(int index, int r, int g, int b);
}
/**
* Iterates over the LEDs in the buffer, starting from index 0. The iterator function is passed
* the current index of iteration, along with the values for the red, green, and blue components
* of the color written to the LED at that index.
*
* @param iterator the iterator function to call for each LED in the buffer.
*/
default void forEach(IndexedColorIterator iterator) {
int bufLen = getLength();
for (int i = 0; i < bufLen; i++) {
iterator.accept(i, getRed(i), getGreen(i), getBlue(i));
}
}
}

65
wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDWriter.java Normal file
View File

@@ -0,0 +1,65 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;
/** Generic interface for writing data to an LED buffer. */
@FunctionalInterface
public interface LEDWriter {
/**
* Sets the RGB value for an LED at a specific index on a LED buffer.
*
* @param index the index of the LED to write to
* @param r the value of the red channel, in [0, 255]
* @param g the value of the green channel, in [0, 255]
* @param b the value of the blue channel, in [0, 255]
*/
void setRGB(int index, int r, int g, int b);
/**
* Sets a specific led in the buffer.
*
* @param index the index to write
* @param h the h value [0-180)
* @param s the s value [0-255]
* @param v the v value [0-255]
*/
default void setHSV(int index, int h, int s, int v) {
if (s == 0) {
setRGB(index, v, v, v);
return;
}
int packedRGB = Color.hsvToRgb(h, s, v);
setRGB(
index,
Color.unpackRGB(packedRGB, Color.RGBChannel.kRed),
Color.unpackRGB(packedRGB, Color.RGBChannel.kGreen),
Color.unpackRGB(packedRGB, Color.RGBChannel.kBlue));
}
/**
* Sets the RGB value for an LED at a specific index on a LED buffer.
*
* @param index the index of the LED to write to
* @param color the color to set
*/
default void setLED(int index, Color color) {
setRGB(index, (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
}
/**
* Sets the RGB value for an LED at a specific index on a LED buffer.
*
* @param index the index of the LED to write to
* @param color the color to set
*/
default void setLED(int index, Color8Bit color) {
setRGB(index, color.red, color.green, color.blue);
}
}

212
wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
View File

@@ -105,35 +105,11 @@ public class Color {
* @return The color
*/
public static Color fromHSV(int h, int s, int v) {
// Loosely based on
// https://en.wikipedia.org/wiki/HSL_and_HSV#HSV_to_RGB
// The hue range is split into 60 degree regions where in each region there
// is one rgb component at a low value (m), one at a high value (v) and one
// that changes (X) from low to high (X+m) or high to low (v-X)
// Difference between highest and lowest value of any rgb component
final int chroma = (s * v) / 255;
// Because hue is 0-180 rather than 0-360 use 30 not 60
final int region = (h / 30) % 6;
// Remainder converted from 0-30 to 0-255
final int remainder = (int) Math.round((h % 30) * (255 / 30.0));
// Value of the lowest rgb component
final int m = v - chroma;
// Goes from 0 to chroma as hue increases
final int X = (chroma * remainder) >> 8;
return switch (region) {
case 0 -> new Color(v, X + m, m);
case 1 -> new Color(v - X, v, m);
case 2 -> new Color(m, v, X + m);
case 3 -> new Color(m, v - X, v);
case 4 -> new Color(X + m, m, v);
default -> new Color(v, m, v - X);
};
int rgb = hsvToRgb(h, s, v);
return new Color(
unpackRGB(rgb, RGBChannel.kRed),
unpackRGB(rgb, RGBChannel.kGreen),
unpackRGB(rgb, RGBChannel.kBlue));
}
@Override
@@ -179,6 +155,184 @@ public class Color {
return MathUtil.clamp(Math.ceil(value * (1 << 12)) / (1 << 12), 0.0, 1.0);
}
// Helper methods
/**
* Converts HSV values to RGB values. The returned RGB color is packed into a 32-bit integer for
* memory performance reasons.
*
* @param h The h value [0-180)
* @param s The s value [0-255]
* @param v The v value [0-255]
* @return the packed RGB color
*/
public static int hsvToRgb(int h, int s, int v) {
// Loosely based on
// https://en.wikipedia.org/wiki/HSL_and_HSV#HSV_to_RGB
// The hue range is split into 60 degree regions where in each region there
// is one rgb component at a low value (m), one at a high value (v) and one
// that changes (X) from low to high (X+m) or high to low (v-X)
// Difference between highest and lowest value of any rgb component
final int chroma = (s * v) / 255;
// Because hue is 0-180 rather than 0-360 use 30 not 60
final int region = (h / 30) % 6;
// Remainder converted from 0-30 to 0-255
final int remainder = (int) Math.round((h % 30) * (255 / 30.0));
// Value of the lowest rgb component
final int m = v - chroma;
// Goes from 0 to chroma as hue increases
final int X = (chroma * remainder) >> 8;
int red;
int green;
int blue;
switch (region) {
case 0:
red = v;
green = X + m;
blue = m;
break;
case 1:
red = v - X;
green = v;
blue = m;
break;
case 2:
red = m;
green = v;
blue = X + m;
break;
case 3:
red = m;
green = v - X;
blue = v;
break;
case 4:
red = X + m;
green = m;
blue = v;
break;
default:
red = v;
green = m;
blue = v - X;
break;
}
return packRGB(red, green, blue);
}
/** Represents a color channel in an RGB color. */
public enum RGBChannel {
/** The red channel of an RGB color. */
kRed,
/** The green channel of an RGB color. */
kGreen,
/** The blue channel of an RGB color. */
kBlue
}
/**
* Packs 3 RGB values into a single 32-bit integer. These values can be unpacked with {@link
* #unpackRGB(int, RGBChannel)} to retrieve the values. This is helpful for avoiding memory
* allocations of new {@code Color} objects and its resulting garbage collector pressure.
*
* @param r the value of the first channel to pack
* @param g the value of the second channel to pack
* @param b the value of the third channel to pack
* @return the packed integer
*/
public static int packRGB(int r, int g, int b) {
return (r & 0xFF) << 16 | (g & 0xFF) << 8 | (b & 0xFF);
}
/**
* Unpacks a single color channel from a packed 32-bit RGB integer.
*
* <p>Note: Packed RGB colors are expected to be in byte order [empty][red][green][blue].
*
* @param packedColor the packed color to extract from
* @param channel the color channel to unpack
* @return the value of the stored color channel
*/
public static int unpackRGB(int packedColor, RGBChannel channel) {
return switch (channel) {
case kRed -> (packedColor >> 16) & 0xFF;
case kGreen -> (packedColor >> 8) & 0xFF;
case kBlue -> packedColor & 0xFF;
};
}
/**
* Performs a linear interpolation between two colors in the RGB colorspace.
*
* @param a the first color to interpolate from
* @param b the second color to interpolate from
* @param t the interpolation scale in [0, 1]
* @return the interpolated color
*/
public static Color lerpRGB(Color a, Color b, double t) {
int packedRGB = lerpRGB(a.red, a.green, a.blue, b.red, b.green, b.blue, t);
return new Color(
unpackRGB(packedRGB, RGBChannel.kRed),
unpackRGB(packedRGB, RGBChannel.kGreen),
unpackRGB(packedRGB, RGBChannel.kBlue));
}
/**
* Linearly interpolates between two RGB colors represented by the (r1, g1, b1) and (r2, g2, b2)
* triplets. For memory performance reasons, the output color is returned packed into a single
* 32-bit integer; use {@link #unpackRGB(int, RGBChannel)} to extract the values for the
* individual red, green, and blue channels.
*
* @param r1 the red value of the first color, in [0, 1]
* @param g1 the green value of the first color, in [0, 1]
* @param b1 the blue value of the first color, in [0, 1]
* @param r2 the red value of the second color, in [0, 1]
* @param g2 the green value of the second color, in [0, 1]
* @param b2 the blue value of the second color, in [0, 1]
* @param t the interpolation value, in [0, 1]
* @return the interpolated color, packed in a 32-bit integer
*/
public static int lerpRGB(
double r1, double g1, double b1, double r2, double g2, double b2, double t) {
return lerpRGB(
(int) (r1 * 255),
(int) (g1 * 255),
(int) (b1 * 255),
(int) (r2 * 255),
(int) (g2 * 255),
(int) (b2 * 255),
t);
}
/**
* Linearly interpolates between two RGB colors represented by the (r1, g1, b1) and (r2, g2, b2)
* triplets. For memory performance reasons, the output color is returned packed into a single
* 32-bit integer; use {@link #unpackRGB(int, RGBChannel)} to extract the values for the
* individual red, green, and blue channels.
*
* @param r1 the red value of the first color, in [0, 255]
* @param g1 the green value of the first color, in [0, 255]
* @param b1 the blue value of the first color, in [0, 255]
* @param r2 the red value of the second color, in [0, 255]
* @param g2 the green value of the second color, in [0, 255]
* @param b2 the blue value of the second color, in [0, 255]
* @param t the interpolation value, in [0, 1]
* @return the interpolated color, packed in a 32-bit integer
*/
public static int lerpRGB(int r1, int g1, int b1, int r2, int g2, int b2, double t) {
return packRGB(
(int) MathUtil.interpolate(r1, r2, t),
(int) MathUtil.interpolate(g1, g2, t),
(int) MathUtil.interpolate(b1, b2, t));
}
/*
* FIRST Colors
*/