From ad18fa62ee06cceb745f221f25ab84edbc9c7f88 Mon Sep 17 00:00:00 2001 From: Sam Carlberg Date: Wed, 5 Jun 2024 22:41:10 -0400 Subject: [PATCH] [wpilib] Add LED pattern API for easily animating addressable LEDs (#6344) 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 --- wpilibc/src/main/native/cpp/LEDPattern.cpp | 305 +++++++ .../src/main/native/include/frc/LEDPattern.h | 355 ++++++++ .../src/test/native/cpp/LEDPatternTest.cpp | 801 ++++++++++++++++++ .../cpp/examples/AddressableLED/cpp/Robot.cpp | 19 +- .../examples/AddressableLED/include/Robot.h | 16 +- .../AddressableLED/cpp/RainbowTest.cpp | 52 -- .../cpp/examples/AddressableLED/cpp/main.cpp | 16 - .../first/wpilibj/AddressableLEDBuffer.java | 136 +-- .../wpilibj/AddressableLEDBufferView.java | 163 ++++ .../edu/wpi/first/wpilibj/LEDPattern.java | 644 ++++++++++++++ .../java/edu/wpi/first/wpilibj/LEDReader.java | 99 +++ .../java/edu/wpi/first/wpilibj/LEDWriter.java | 65 ++ .../edu/wpi/first/wpilibj/util/Color.java | 212 ++++- .../wpilibj/AddressableLEDBufferViewTest.java | 69 ++ .../edu/wpi/first/wpilibj/LEDPatternTest.java | 798 +++++++++++++++++ .../edu/wpi/first/wpilibj/util/ColorTest.java | 41 + .../examples/addressableled/Robot.java | 39 +- .../examples/addressableled/RainbowTest.java | 54 -- .../src/main/native/include/frc/MathUtil.h | 37 + 19 files changed, 3610 insertions(+), 311 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/LEDPattern.cpp create mode 100644 wpilibc/src/main/native/include/frc/LEDPattern.h create mode 100644 wpilibc/src/test/native/cpp/LEDPatternTest.cpp delete mode 100644 wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp delete mode 100644 wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/main.cpp create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBufferView.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDReader.java create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDWriter.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferViewTest.java create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/LEDPatternTest.java delete mode 100644 wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/addressableled/RainbowTest.java diff --git a/wpilibc/src/main/native/cpp/LEDPattern.cpp b/wpilibc/src/main/native/cpp/LEDPattern.cpp new file mode 100644 index 0000000000..2b745e9645 --- /dev/null +++ b/wpilibc/src/main/native/cpp/LEDPattern.cpp @@ -0,0 +1,305 @@ +// 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. + +#include "frc/LEDPattern.h" + +#include +#include +#include + +#include +#include + +#include "frc/MathUtil.h" + +using namespace frc; + +LEDPattern::LEDPattern(LEDPatternFn impl) : m_impl(std::move(impl)) {} + +void LEDPattern::ApplyTo(std::span data, + LEDWriterFn writer) const { + m_impl(data, writer); +} + +void LEDPattern::ApplyTo(std::span data) const { + ApplyTo(data, [&](int index, Color color) { data[index].SetLED(color); }); +} + +LEDPattern LEDPattern::Reversed() { + return LEDPattern{[self = *this](auto data, auto writer) { + self.ApplyTo(data, [&](int i, Color color) { + writer((data.size() - 1) - i, color); + }); + }}; +} + +LEDPattern LEDPattern::OffsetBy(int offset) { + return LEDPattern{[=, self = *this](auto data, auto writer) { + self.ApplyTo(data, [&data, &writer, offset](int i, Color color) { + int shiftedIndex = + frc::FloorMod(i + offset, static_cast(data.size())); + writer(shiftedIndex, color); + }); + }}; +} + +LEDPattern LEDPattern::ScrollAtRelativeSpeed(units::hertz_t velocity) { + // velocity is in terms of LED lengths per second (1_hz = full cycle per + // second, 0.5_hz = half cycle per second, 2_hz = two cycles per second) + // Invert and multiply by 1,000,000 to get microseconds + double periodMicros = 1e6 / velocity.value(); + + return LEDPattern{[=, self = *this](auto data, auto writer) { + auto bufLen = data.size(); + auto now = wpi::Now(); + + // index should move by (bufLen) / (period) + double t = + (now % static_cast(std::floor(periodMicros))) / periodMicros; + int offset = static_cast(std::floor(t * bufLen)); + + self.ApplyTo(data, [=](int i, Color color) { + // floorMod so if the offset is negative, we still get positive outputs + int shiftedIndex = frc::FloorMod(i + offset, static_cast(bufLen)); + writer(shiftedIndex, color); + }); + }}; +} + +LEDPattern LEDPattern::ScrollAtAbsoluteSpeed( + units::meters_per_second_t velocity, units::meter_t ledSpacing) { + // Velocity is in terms of meters per second + // Multiply by 1,000,000 to use microseconds instead of seconds + auto microsPerLed = + static_cast(std::floor((ledSpacing / velocity).value() * 1e6)); + + return LEDPattern{[=, self = *this](auto data, auto writer) { + auto bufLen = data.size(); + auto now = wpi::Now(); + + // every step in time that's a multiple of microsPerLED will increment + // the offset by 1 + // cast unsigned int64 `now` to a signed int64 so we can get negative + // offset values for negative velocities + auto offset = static_cast(now) / microsPerLed; + + self.ApplyTo(data, [=, &writer](int i, Color color) { + // FloorMod so if the offset is negative, we still get positive outputs + int shiftedIndex = frc::FloorMod(i + offset, static_cast(bufLen)); + + writer(shiftedIndex, color); + }); + }}; +} + +LEDPattern LEDPattern::Blink(units::second_t onTime, units::second_t offTime) { + auto totalMicros = units::microsecond_t{onTime + offTime}.to(); + auto onMicros = units::microsecond_t{onTime}.to(); + + return LEDPattern{[=, self = *this](auto data, auto writer) { + if (wpi::Now() % totalMicros < onMicros) { + self.ApplyTo(data, writer); + } else { + LEDPattern::kOff.ApplyTo(data, writer); + } + }}; +} + +LEDPattern LEDPattern::Blink(units::second_t onTime) { + return LEDPattern::Blink(onTime, onTime); +} + +LEDPattern LEDPattern::SynchronizedBlink(std::function signal) { + return LEDPattern{[=, self = *this](auto data, auto writer) { + if (signal()) { + self.ApplyTo(data, writer); + } else { + LEDPattern::kOff.ApplyTo(data, writer); + } + }}; +} + +LEDPattern LEDPattern::Breathe(units::second_t period) { + auto periodMicros = units::microsecond_t{period}; + + return LEDPattern{[periodMicros, self = *this](auto data, auto writer) { + self.ApplyTo(data, [&writer, periodMicros](int i, Color color) { + double t = (wpi::Now() % periodMicros.to()) / + periodMicros.to(); + double phase = t * 2 * std::numbers::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 = (std::cos(phase) + 1) / 2.0; + + writer(i, Color{color.red * dim, color.green * dim, color.blue * dim}); + }); + }}; +} + +LEDPattern LEDPattern::OverlayOn(const LEDPattern& base) { + return LEDPattern{[self = *this, base](auto data, auto writer) { + // write the base pattern down first... + base.ApplyTo(data, writer); + + // ... then, overwrite with illuminated LEDs from the overlay + self.ApplyTo(data, [&](int i, Color color) { + if (color.red > 0 || color.green > 0 || color.blue > 0) { + writer(i, color); + } + }); + }}; +} + +LEDPattern LEDPattern::Blend(const LEDPattern& other) { + return LEDPattern{[self = *this, other](auto data, auto writer) { + // Apply the current pattern down as normal... + self.ApplyTo(data, writer); + + other.ApplyTo(data, [&](int i, Color color) { + // ... then read the result and average it with the output from the other + // pattern + writer(i, Color{(data[i].r / 255.0 + color.red) / 2, + (data[i].g / 255.0 + color.green) / 2, + (data[i].b / 255.0 + color.blue) / 2}); + }); + }}; +} + +LEDPattern LEDPattern::Mask(const LEDPattern& mask) { + return LEDPattern{[self = *this, mask](auto data, auto writer) { + // Apply the current pattern down as normal... + self.ApplyTo(data, writer); + + mask.ApplyTo(data, [&](int i, Color color) { + auto currentColor = data[i]; + // ... then perform a bitwise AND operation on each channel to apply the + // mask + writer(i, Color{currentColor.r & static_cast(255 * color.red), + currentColor.g & static_cast(255 * color.green), + currentColor.b & static_cast(255 * color.blue)}); + }); + }}; +} + +LEDPattern LEDPattern::AtBrightness(double relativeBrightness) { + return LEDPattern{[relativeBrightness, self = *this](auto data, auto writer) { + self.ApplyTo(data, [&](int i, Color color) { + writer(i, Color{color.red * relativeBrightness, + color.green * relativeBrightness, + color.blue * relativeBrightness}); + }); + }}; +} + +// Static constants and functions + +LEDPattern LEDPattern::kOff = LEDPattern::Solid(Color::kBlack); + +LEDPattern LEDPattern::Solid(const Color color) { + return LEDPattern{[=](auto data, auto writer) { + auto bufLen = data.size(); + for (size_t i = 0; i < bufLen; i++) { + writer(i, color); + } + }}; +} + +LEDPattern LEDPattern::ProgressMaskLayer( + std::function progressFunction) { + return LEDPattern{[=](auto data, auto writer) { + double progress = std::clamp(progressFunction(), 0.0, 1.0); + auto bufLen = data.size(); + size_t max = bufLen * progress; + + for (size_t led = 0; led < max; led++) { + writer(led, Color::kWhite); + } + for (size_t led = max; led < bufLen; led++) { + writer(led, Color::kBlack); + } + }}; +} + +LEDPattern LEDPattern::Steps(std::span> steps) { + if (steps.size() == 0) { + // no colors specified + return LEDPattern::kOff; + } + if (steps.size() == 1 && steps[0].first == 0) { + // only one color specified, just show a static color + return LEDPattern::Solid(steps[0].second); + } + + return LEDPattern{[steps = std::vector(steps.begin(), steps.end())]( + auto data, auto writer) { + auto bufLen = data.size(); + + // precompute relevant positions for this buffer so we don't need to do a + // check on every single LED index + std::unordered_map stopPositions; + + for (auto step : steps) { + stopPositions[std::floor(step.first * bufLen)] = step.second; + } + auto currentColor = Color::kBlack; + for (size_t led = 0; led < bufLen; led++) { + if (stopPositions.contains(led)) { + currentColor = stopPositions[led]; + } + writer(led, currentColor); + } + }}; +} + +LEDPattern LEDPattern::Steps( + std::initializer_list> steps) { + return Steps(std::span{steps.begin(), steps.end()}); +} + +LEDPattern LEDPattern::Gradient(std::span colors) { + if (colors.size() == 0) { + // no colors specified + return LEDPattern::kOff; + } + if (colors.size() == 1) { + // only one color specified, just show a static color + return LEDPattern::Solid(colors[0]); + } + + return LEDPattern{[colors = std::vector(colors.begin(), colors.end())]( + auto data, auto writer) { + size_t numSegments = colors.size(); + auto bufLen = data.size(); + int ledsPerSegment = bufLen / numSegments; + + for (size_t led = 0; led < bufLen; led++) { + int colorIndex = (led / ledsPerSegment) % numSegments; + int nextColorIndex = (colorIndex + 1) % numSegments; + double t = std::fmod(led / static_cast(ledsPerSegment), 1.0); + + auto color = colors[colorIndex]; + auto nextColor = colors[nextColorIndex]; + + Color gradientColor{wpi::Lerp(color.red, nextColor.red, t), + wpi::Lerp(color.green, nextColor.green, t), + wpi::Lerp(color.blue, nextColor.blue, t)}; + writer(led, gradientColor); + } + }}; +} + +LEDPattern LEDPattern::Gradient(std::initializer_list colors) { + return Gradient(std::span{colors.begin(), colors.end()}); +} + +LEDPattern LEDPattern::Rainbow(int saturation, int value) { + return LEDPattern{[=](auto data, auto writer) { + auto bufLen = data.size(); + for (size_t led = 0; led < bufLen; led++) { + int hue = ((led * 180) / bufLen) % 180; + writer(led, Color::FromHSV(hue, saturation, value)); + } + }}; +} diff --git a/wpilibc/src/main/native/include/frc/LEDPattern.h b/wpilibc/src/main/native/include/frc/LEDPattern.h new file mode 100644 index 0000000000..37e743d5ba --- /dev/null +++ b/wpilibc/src/main/native/include/frc/LEDPattern.h @@ -0,0 +1,355 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +#include "frc/AddressableLED.h" +#include "util/Color.h" + +namespace frc { + +/** + * Sets the LED at the given index to the given color. + */ +using LEDWriterFn = std::function; + +/** + * Accepts a data buffer (1st argument) and a callback (2nd argument) for + * writing data. + */ +using LEDPatternFn = + std::function, LEDWriterFn)>; + +class LEDPattern { + public: + explicit LEDPattern(LEDPatternFn impl); + + /** + * 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. + * + * 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 scroll, blink, or breathe by intercepting + * the data writes to transform their behavior to whatever we like. + * + * @param data the current data of the LED strip + * @param writer data writer for setting new LED colors on the LED strip + */ + void ApplyTo(std::span data, + LEDWriterFn writer) const; + + /** + * 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. + * + * 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 scroll, blink, or breathe by intercepting + * the data writes to transform their behavior to whatever we like. + * + * @param data the current data of the LED strip + */ + void ApplyTo(std::span data) const; + + /** + * 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). + * + * @return the reverse pattern + */ + [[nodiscard]] + LEDPattern Reversed(); + + /** + * Creates a pattern that displays 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 + */ + [[nodiscard]] + LEDPattern OffsetBy(int offset); + + /** + * 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). + */ + [[nodiscard]] + LEDPattern ScrollAtRelativeSpeed(units::hertz_t velocity); + + /** + * Creates a pattern that plays this one scrolling up an LED strip. A negative + * velocity makes the pattern play in reverse. + * + *

For example, scrolling a pattern at 4 inches per second along an LED + * strip with 60 LEDs per meter: + * + * 

+   *   // LEDs per meter, a known value taken from the spec sheet of our
+   * particular LED strip units::meter_t LED_SPACING = units::meter_t{1 /60.0};
+   *
+   *   frc::LEDPattern rainbow = frc::LEDPattern::Rainbow();
+   *   frc::LEDPattern scrollingRainbow =
+   *     rainbow.ScrollAtAbsoluteSpeed(units::inches_per_second_t{4},
+   * LED_SPACING);
+   *
+ * + *

Note that this pattern will scroll faster if applied to a less + * dense LED strip (such as 30 LEDs per meter), or slower 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 + */ + [[nodiscard]] + LEDPattern ScrollAtAbsoluteSpeed(units::meters_per_second_t velocity, + units::meter_t ledSpacing); + + /** + * 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 + */ + [[nodiscard]] + LEDPattern Blink(units::second_t onTime, units::second_t offTime); + + /** + * Like {@link LEDPattern::Blink(units::second_t)}, 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 + */ + [[nodiscard]] + LEDPattern Blink(units::second_t 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 + */ + [[nodiscard]] + LEDPattern SynchronizedBlink(std::function signal); + + /** + * 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 + */ + [[nodiscard]] + LEDPattern Breathe(units::second_t period); + + /** + * Creates a pattern that plays this pattern overlaid on another. Anywhere + * this pattern sets an LED to off (or {@link frc::Color::kBlack}), the base + * pattern will be displayed instead. + * + * @param base the base pattern to overlay on top of + * @return the combined overlay pattern + */ + [[nodiscard]] + LEDPattern OverlayOn(const LEDPattern& base); + + /** + * 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 + * LEDPattern::OverlayOn(const LEDPattern&)}, 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 + */ + [[nodiscard]] + LEDPattern Blend(const LEDPattern& other); + + /** + * Similar to {@link LEDPattern::Blend(const 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 + */ + [[nodiscard]] + LEDPattern Mask(const LEDPattern& mask); + + /** + * 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. + * + *

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). + * + *

For example, dimming can be done simply by adding a call to + * `atBrightness` at the end of a pattern: + * + * 

+   *   // Solid red, but at 50% brightness
+   *   frc::LEDPattern::Solid(frc::Color::kRed).AtBrightness(0.5);
+   *
+   *   // Solid white, but at only 10% (i.e. ~0.5V)
+   *   frc::LEDPattern::Solid(frc:Color::kWhite).AtBrightness(0.1);
+   *
+ * + * @param relativeBrightness the multiplier to apply to all channels to modify + * brightness + * @return the input pattern, displayed at + */ + [[nodiscard]] + LEDPattern AtBrightness(double relativeBrightness); + + /** A pattern that turns off all LEDs. */ + static LEDPattern kOff; + + /** + * 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(const Color color); + + /** + * Creates a pattern that works as a mask layer for {@link + * LEDPattern::Mask(const 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. + * + *

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. + * + * 

+   * frc::LEDPattern basePattern =
+   *   frc::LEDPattern::Gradient(frc::Color::kRed, frc::Color::kBlue);
+   * frc::LEDPattern progressPattern =
+   *   basePattern.Mask(frc::LEDPattern::ProgressMaskLayer([&]() {
+   *     return elevator.GetHeight() / elevator.MaxHeight();
+   *   });
+   *
+ * + * @param progressFunction 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(std::function progressFunction); + + /** + * 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). + * + * @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(std::span> steps); + + /** + * 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). + * + * @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( + std::initializer_list> steps); + + /** + * 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(std::span colors); + + /** + * 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(std::initializer_list colors); + + /** + * 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); + + private: + LEDPatternFn m_impl; +}; +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/LEDPatternTest.cpp b/wpilibc/src/test/native/cpp/LEDPatternTest.cpp new file mode 100644 index 0000000000..5ea506c3c0 --- /dev/null +++ b/wpilibc/src/test/native/cpp/LEDPatternTest.cpp @@ -0,0 +1,801 @@ +// 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. + +#include +#include +#include + +#include "frc/LEDPattern.h" +#include "frc/MathUtil.h" + +namespace frc { + +static LEDPattern whiteYellowPurple{[](auto data, auto writer) { + for (size_t led = 0; led < data.size(); led++) { + switch (led % 3) { + case 0: + writer(led, Color::kWhite); + break; + case 1: + writer(led, Color::kYellow); + break; + case 2: + writer(led, Color::kPurple); + break; + } + } +}}; + +void AssertIndexColor(std::span data, int index, + Color color); +Color LerpColors(Color a, Color b, double t); + +TEST(LEDPatternTest, SolidColor) { + LEDPattern pattern = LEDPattern::Solid(Color::kYellow); + std::array buffer; + + // prefill + for (int i = 0; i < 5; i++) { + buffer[i].SetLED(Color::kPurple); + } + + pattern.ApplyTo(buffer); + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, Color::kYellow); + } +} + +TEST(LEDPatternTest, EmptyGradientSetsToBlack) { + std::array colors; + LEDPattern pattern = LEDPattern::Gradient(colors); + std::array buffer; + pattern.ApplyTo(buffer); + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, Color::kBlack); + } +} + +TEST(LEDPatternTest, SingleColorGradientSetsSolid) { + std::array colors{Color::kYellow}; + LEDPattern pattern = LEDPattern::Gradient(colors); + std::array buffer; + pattern.ApplyTo(buffer); + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, Color::kYellow); + } +} + +TEST(LEDPatternTest, Gradient2Colors) { + std::array colors{Color::kYellow, Color::kPurple}; + LEDPattern pattern = LEDPattern::Gradient(colors); + std::array buffer; + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kYellow); + AssertIndexColor(buffer, 25, + LerpColors(Color::kYellow, Color::kPurple, 25 / 49.0)); + AssertIndexColor(buffer, 49, Color::kPurple); + AssertIndexColor(buffer, 74, + LerpColors(Color::kPurple, Color::kYellow, 25 / 49.0)); + AssertIndexColor(buffer, 98, Color::kYellow); +} + +TEST(LEDPatternTest, Gradient3Colors) { + std::array colors{Color::kYellow, Color::kPurple, Color::kWhite}; + LEDPattern pattern = LEDPattern::Gradient(colors); + std::array buffer; + pattern.ApplyTo(buffer); + + AssertIndexColor(buffer, 0, Color::kYellow); + AssertIndexColor(buffer, 25, + LerpColors(Color::kYellow, Color::kPurple, 25 / 33.0)); + AssertIndexColor(buffer, 33, Color::kPurple); + AssertIndexColor(buffer, 58, + LerpColors(Color::kPurple, Color::kWhite, 25 / 33.0)); + AssertIndexColor(buffer, 66, Color::kWhite); + AssertIndexColor(buffer, 91, + LerpColors(Color::kWhite, Color::kYellow, 25 / 33.0)); + AssertIndexColor(buffer, 98, + LerpColors(Color::kWhite, Color::kYellow, 32 / 33.0)); +} + +TEST(LEDPatternTest, EmptyStepsSetsToBlack) { + std::array, 0> steps; + LEDPattern pattern = LEDPattern::Steps(steps); + std::array buffer; + + // prefill + for (int i = 0; i < 5; i++) { + buffer[i].SetLED(Color::kPurple); + } + + pattern.ApplyTo(buffer); + + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, Color::kBlack); + } +} + +TEST(LEDPatternTest, SingleStepSetsSolid) { + std::array, 1> steps{std::pair{0.0, Color::kYellow}}; + LEDPattern pattern = LEDPattern::Steps(steps); + std::array buffer; + + pattern.ApplyTo(buffer); + + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, Color::kYellow); + } +} + +TEST(LEDPatternTest, SingleHalfStepSetsHalfOffHalfColor) { + std::array, 1> steps{std::pair{0.5, Color::kYellow}}; + LEDPattern pattern = LEDPattern::Steps(steps); + std::array buffer; + + pattern.ApplyTo(buffer); + + // [0, 48] should be black... + for (int i = 0; i < 49; i++) { + AssertIndexColor(buffer, i, Color::kBlack); + } + + // ... and [49, ] should be the color that was set + for (int i = 49; i < 99; i++) { + AssertIndexColor(buffer, i, Color::kYellow); + } +} + +TEST(LEDPatternTest, ScrollRelativeForward) { + // A black to white gradient + LEDPattern pattern = LEDPattern{[=](auto data, auto writer) { + for (size_t led = 0; led < data.size(); led++) { + int ch = static_cast(led % 256); + writer(led, Color{ch, ch, ch}); + } + }}; + std::array buffer; + + // Scrolling at 1/256th of the buffer per second, + // or 1 individual diode per second + auto scroll = pattern.ScrollAtRelativeSpeed(units::hertz_t{1 / 256.0}); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + + for (int time = 0; time < 500; time++) { + // convert time (seconds) to microseconds + now = time * 1000000ull; + + scroll.ApplyTo(buffer); + + for (size_t led = 0; led < buffer.size(); led++) { + SCOPED_TRACE( + fmt::format("LED {} of 256, run {} of 500", led + 1, time + 1)); + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, + // 255)] Value for every channel should DECREASE by 1 in each timestep, + // wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = frc::FloorMod(static_cast(led - time), 256); + AssertIndexColor(buffer, led, Color{ch, ch, ch}); + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, ScrollRelativeBackward) { + // A black to white gradient + LEDPattern pattern = LEDPattern{[=](auto data, auto writer) { + for (size_t led = 0; led < data.size(); led++) { + int ch = static_cast(led % 256); + writer(led, Color{ch, ch, ch}); + } + }}; + std::array buffer; + + // Scrolling at 1/256th of the buffer per second, + // or 1 individual diode per second + auto scroll = pattern.ScrollAtRelativeSpeed(units::hertz_t{-1 / 256.0}); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + + for (int time = 0; time < 500; time++) { + // convert time (seconds) to microseconds + now = time * 1000000ull; + + scroll.ApplyTo(buffer); + + for (size_t led = 0; led < buffer.size(); led++) { + SCOPED_TRACE( + fmt::format("LED {} of 256, run {} of 500", led + 1, time + 1)); + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, + // 255)] Value for every channel should DECREASE by 1 in each timestep, + // wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = frc::FloorMod(static_cast(led + time), 256); + AssertIndexColor(buffer, led, Color{ch, ch, ch}); + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, ScrollAbsoluteForward) { + // A black to white gradient + LEDPattern pattern = LEDPattern{[](auto data, auto writer) { + for (size_t led = 0; led < data.size(); led++) { + int ch = static_cast(led % 256); + writer(led, Color{ch, ch, ch}); + } + }}; + std::array buffer; + // scroll at 16 m/s, LED spacing = 2cm + // buffer is 256 LEDs, so total length = 512cm = 5.12m + // scrolling at 16 m/s yields a period of 0.32 seconds, + // or 0.00125 seconds per LED (800 LEDs/s) + auto scroll = pattern.ScrollAtAbsoluteSpeed(16_mps, 2_cm); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + + for (int time = 0; time < 500; time++) { + // convert time (seconds) to microseconds + now = time * 1250ull; // 1.25ms per LED + + scroll.ApplyTo(buffer); + + for (size_t led = 0; led < buffer.size(); led++) { + SCOPED_TRACE( + fmt::format("LED {} of 256, run {} of 500", led + 1, time + 1)); + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, + // 255)] Value for every channel should DECREASE by 1 in each timestep, + // wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = frc::FloorMod(static_cast(led - time), 256); + AssertIndexColor(buffer, led, Color{ch, ch, ch}); + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, ScrollAbsoluteBackward) { + // A black to white gradient + LEDPattern pattern = LEDPattern{[](auto data, auto writer) { + for (size_t led = 0; led < data.size(); led++) { + int ch = static_cast(led % 256); + writer(led, Color{ch, ch, ch}); + } + }}; + std::array buffer; + // scroll at 16 m/s, LED spacing = 2cm + // buffer is 256 LEDs, so total length = 512cm = 5.12m + // scrolling at 16 m/s yields a period of 0.32 seconds, + // or 0.00125 seconds per LED (800 LEDs/s) + auto scroll = pattern.ScrollAtAbsoluteSpeed(-16_mps, 2_cm); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + + for (int time = 0; time < 500; time++) { + // convert time (seconds) to microseconds + now = time * 1250ull; // 1.25ms per LED + + scroll.ApplyTo(buffer); + + for (size_t led = 0; led < buffer.size(); led++) { + SCOPED_TRACE( + fmt::format("LED {} of 256, run {} of 500", led + 1, time + 1)); + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, + // 255)] Value for every channel should DECREASE by 1 in each timestep, + // wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = frc::FloorMod(static_cast(led + time), 256); + AssertIndexColor(buffer, led, Color{ch, ch, ch}); + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, RainbowFullSize) { + std::array buffer; + int saturation = 255; + int value = 255; + LEDPattern pattern = LEDPattern::Rainbow(saturation, value); + pattern.ApplyTo(buffer); + + for (int led = 0; led < 180; led++) { + AssertIndexColor(buffer, led, Color::FromHSV(led, saturation, value)); + } +} + +TEST(LEDPatternTest, RainbowHalfSize) { + std::array buffer; + int saturation = 42; + int value = 87; + LEDPattern pattern = LEDPattern::Rainbow(saturation, value); + pattern.ApplyTo(buffer); + + for (int led = 0; led < 90; led++) { + AssertIndexColor(buffer, led, Color::FromHSV(led * 2, saturation, value)); + } +} + +TEST(LEDPatternTest, RainbowThirdSize) { + std::array buffer; + int saturation = 191; + int value = 255; + LEDPattern pattern = LEDPattern::Rainbow(saturation, value); + pattern.ApplyTo(buffer); + + for (int led = 0; led < 60; led++) { + SCOPED_TRACE(fmt::format("LED {} of 60", led + 1)); + AssertIndexColor(buffer, led, Color::FromHSV(led * 3, saturation, value)); + } +} + +TEST(LEDPatternTest, RainbowDoubleSize) { + std::array buffer; + int saturation = 212; + int value = 93; + LEDPattern pattern = LEDPattern::Rainbow(saturation, value); + pattern.ApplyTo(buffer); + + for (int led = 0; led < 360; led++) { + SCOPED_TRACE(fmt::format("LED {} of 360", led + 1)); + AssertIndexColor(buffer, led, Color::FromHSV(led / 2, saturation, value)); + } +} + +TEST(LEDPatternTest, RainbowOddSize) { + std::array buffer; + double scale = 180.0 / 127; + int saturation = 73; + int value = 128; + LEDPattern pattern = LEDPattern::Rainbow(saturation, value); + pattern.ApplyTo(buffer); + + for (int led = 0; led < 127; led++) { + SCOPED_TRACE(fmt::format("LED {} of 127", led + 1)); + AssertIndexColor( + buffer, led, + Color::FromHSV(static_cast(led * scale), saturation, value)); + } +} + +TEST(LEDPatternTest, ReverseSolid) { + std::array buffer; + const auto color = Color::kRosyBrown; + + auto solid = LEDPattern::Solid(color); + auto pattern = solid.Reversed(); + + pattern.ApplyTo(buffer); + + for (int led = 0; led < 90; led++) { + SCOPED_TRACE(fmt::format("LED {} of 90", led + 1)); + AssertIndexColor(buffer, led, Color::kRosyBrown); + } +} + +TEST(LEDPatternTest, ReverseSteps) { + std::array buffer; + std::array, 2> steps{std::pair{0.0, Color::kPlum}, + std::pair{0.5, Color::kYellow}}; + auto stepPattern = LEDPattern::Steps(steps); + auto pattern = stepPattern.Reversed(); + + pattern.ApplyTo(buffer); + + // colors should be swapped; yellow first, then plum + for (int led = 0; led < 50; led++) { + SCOPED_TRACE(fmt::format("LED {} of 100", led + 1)); + AssertIndexColor(buffer, led, Color::kYellow); + } + for (int led = 50; led < 100; led++) { + SCOPED_TRACE(fmt::format("LED {} of 100", led + 1)); + AssertIndexColor(buffer, led, Color::kPlum); + } +} + +TEST(LEDPatternTest, OffsetPositive) { + std::array buffer; + auto offset = whiteYellowPurple.OffsetBy(1); + offset.ApplyTo(buffer); + + for (int led = 0; led < 21; led++) { + SCOPED_TRACE(fmt::format("LED {} of 21", led + 1)); + switch (led % 3) { + case 0: + AssertIndexColor(buffer, led, Color::kPurple); + break; + case 1: + AssertIndexColor(buffer, led, Color::kWhite); + break; + case 2: + AssertIndexColor(buffer, led, Color::kYellow); + break; + } + } +} + +TEST(LEDPatternTest, OffsetNegative) { + std::array buffer; + auto offset = whiteYellowPurple.OffsetBy(-1); + offset.ApplyTo(buffer); + + for (int led = 0; led < 21; led++) { + SCOPED_TRACE(fmt::format("LED {} of 21", led + 1)); + switch (led % 3) { + case 0: + AssertIndexColor(buffer, led, Color::kYellow); + break; + case 1: + AssertIndexColor(buffer, led, Color::kPurple); + break; + case 2: + AssertIndexColor(buffer, led, Color::kWhite); + break; + } + } +} + +TEST(LEDPatternTest, OffsetZero) { + std::array buffer; + auto offset = whiteYellowPurple.OffsetBy(0); + offset.ApplyTo(buffer); + + for (int led = 0; led < 21; led++) { + SCOPED_TRACE(fmt::format("LED {} of 21", led + 1)); + switch (led % 3) { + case 0: + AssertIndexColor(buffer, led, Color::kWhite); + break; + case 1: + AssertIndexColor(buffer, led, Color::kYellow); + break; + case 2: + AssertIndexColor(buffer, led, Color::kPurple); + break; + } + } +} + +TEST(LEDPatternTest, BlinkSymmetric) { + std::array buffer; + auto white = LEDPattern::Solid(Color::kWhite); + + // on for 2 seconds, off for 2 seconds + auto pattern = white.Blink(2_s); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + for (int t = 0; t < 8; t++) { + now = t * 1000000ull; // time travel 1 second + SCOPED_TRACE(fmt::format("Time {} seconds", t)); + pattern.ApplyTo(buffer); + + switch (t) { + case 0: + case 1: + case 4: + case 5: + AssertIndexColor(buffer, 0, Color::kWhite); + break; + case 2: + case 3: + case 6: + case 7: + AssertIndexColor(buffer, 0, Color::kBlack); + break; + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, BlinkAsymmetric) { + std::array buffer; + auto white = LEDPattern::Solid(Color::kWhite); + + // on for 3 seconds, off for 1 second + auto pattern = white.Blink(3_s, 1_s); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + for (int t = 0; t < 8; t++) { + now = t * 1000000ull; // time travel 1 second + SCOPED_TRACE(fmt::format("Time {} seconds", t)); + pattern.ApplyTo(buffer); + + switch (t) { + case 0: + case 1: + case 2: // first period + case 4: + case 5: + case 6: // second period + AssertIndexColor(buffer, 0, Color::kWhite); + break; + case 3: + case 7: + AssertIndexColor(buffer, 0, Color::kBlack); + break; + } + } + + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, BlinkInSync) { + std::array buffer; + auto white = LEDPattern::Solid(Color::kWhite); + + bool flag = false; + auto condition = [&flag]() { return flag; }; + + auto pattern = white.SynchronizedBlink(condition); + + SCOPED_TRACE("Flag off"); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kBlack); + + SCOPED_TRACE("Flag on"); + flag = true; + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kWhite); + + SCOPED_TRACE("Flag off"); + flag = false; + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kBlack); +} + +TEST(LEDPatternTest, Breathe) { + Color midGray{0.5, 0.5, 0.5}; + std::array buffer; + auto white = LEDPattern::Solid(Color::kWhite); + auto pattern = white.Breathe(4_us); + + static uint64_t now = 0ull; + WPI_SetNowImpl([] { return now; }); + + { + now = 0ull; // start + SCOPED_TRACE(fmt::format("Time {}", now)); + + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kWhite); + } + { + now = 1ull; // midway (down) + SCOPED_TRACE(fmt::format("Time {}", now)); + + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, midGray); + } + { + now = 2ull; // bottom + SCOPED_TRACE(fmt::format("Time {}", now)); + + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kBlack); + } + { + now = 3ull; // midway (up) + SCOPED_TRACE(fmt::format("Time {}", now)); + + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, midGray); + } + { + now = 4ull; // back to start + SCOPED_TRACE(fmt::format("Time {}", now)); + + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kWhite); + } + WPI_SetNowImpl(nullptr); // cleanup +} + +TEST(LEDPatternTest, OverlaySolidOnSolid) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kWhite); + auto overlay = LEDPattern::Solid(Color::kYellow); + auto pattern = overlay.OverlayOn(base); + pattern.ApplyTo(buffer); + + AssertIndexColor(buffer, 0, Color::kYellow); +} + +TEST(LEDPatternTest, OverlayNearlyBlack) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kWhite); + auto overlay = LEDPattern::Solid(Color{1, 0, 0}); + auto pattern = overlay.OverlayOn(base); + pattern.ApplyTo(buffer); + + AssertIndexColor(buffer, 0, Color{1, 0, 0}); +} + +TEST(LEDPatternTest, OverlayMixed) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kWhite); + std::array, 2> steps{std::pair{0.0, Color::kYellow}, + std::pair{0.5, Color::kBlack}}; + auto overlay = LEDPattern::Steps(steps); + auto pattern = overlay.OverlayOn(base); + pattern.ApplyTo(buffer); + + AssertIndexColor(buffer, 0, Color::kYellow); + AssertIndexColor(buffer, 1, Color::kWhite); +} + +TEST(LEDPatternTest, Blend) { + std::array buffer; + + auto pattern1 = LEDPattern::Solid(Color::kBlue); + auto pattern2 = LEDPattern::Solid(Color::kRed); + auto blend = pattern1.Blend(pattern2); + blend.ApplyTo(buffer); + + // Individual RGB channels are averaged + // #0000FF blended with #FF0000 yields #7F007F + AssertIndexColor(buffer, 0, Color{127, 0, 127}); +} + +TEST(LEDPatternTest, BinaryMask) { + std::array buffer; + + Color color{123, 123, 123}; + auto base = LEDPattern::Solid(color); + + // first 50% mask on, last 50% mask off + std::array, 2> steps{std::pair{0.0, Color::kWhite}, + std::pair{0.5, Color::kBlack}}; + auto mask = LEDPattern::Steps(steps); + auto masked = base.Mask(mask); + masked.ApplyTo(buffer); + + for (int i = 0; i < 5; i++) { + AssertIndexColor(buffer, i, color); + } + + for (int i = 5; i < 10; i++) { + AssertIndexColor(buffer, i, Color::kBlack); + } +} + +TEST(LEDPatternTest, ChannelwiseMask) { + std::array buffer; + + Color baseColor{123, 123, 123}; + Color halfGray{0.5, 0.5, 0.5}; + auto base = LEDPattern::Solid(baseColor); + std::array, 5> steps{ + std::pair{0.0, Color::kRed}, std::pair{0.2, Color::kLime}, + std::pair{0.4, Color::kBlue}, std::pair{0.6, halfGray}, + std::pair{0.8, Color::kWhite}}; + auto mask = LEDPattern::Steps(steps); + auto masked = base.Mask(mask); + masked.ApplyTo(buffer); + + AssertIndexColor(buffer, 0, Color{123, 0, 0}); + AssertIndexColor(buffer, 1, Color{0, 123, 0}); + AssertIndexColor(buffer, 2, Color{0, 0, 123}); + + // mask channels are all 0b00111111, base is 0b00111011, + // so the AND should give us the unmodified base color + AssertIndexColor(buffer, 3, baseColor); + AssertIndexColor(buffer, 4, baseColor); +} + +TEST(LEDPatternTest, ProcessMaskLayer) { + std::array buffer; + + double progress = 0.0; + auto maskLayer = + LEDPattern::ProgressMaskLayer([&progress]() { return progress; }); + + for (double t = 0; t <= 1.0; t += 0.01) { + SCOPED_TRACE(fmt::format("Time {}", t)); + progress = t; + maskLayer.ApplyTo(buffer); + + int lastMaskedLED = static_cast(t * 100); + for (int i = 0; i < lastMaskedLED; i++) { + SCOPED_TRACE(fmt::format("LED {}", i)); + AssertIndexColor(buffer, i, Color::kWhite); + } + for (int i = lastMaskedLED; i < 100; i++) { + SCOPED_TRACE(fmt::format("LED {}", i)); + AssertIndexColor(buffer, i, Color::kBlack); + } + } +} + +TEST(LEDPatternTest, ZeroBrightness) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kRed); + auto pattern = base.AtBrightness(0); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kBlack); +} + +TEST(LEDPatternTest, SameBrightness) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kMagenta); + auto pattern = base.AtBrightness(1.0); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kMagenta); +} + +TEST(LEDPatternTest, HigherBrightness) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kMagenta); + auto pattern = base.AtBrightness(4 / 3.0); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kMagenta); +} + +TEST(LEDPatternTest, NegativeBrightness) { + std::array buffer; + + auto base = LEDPattern::Solid(Color::kWhite); + auto pattern = base.AtBrightness(-1.0); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kBlack); +} + +TEST(LEDPatternTest, ClippingBrightness) { + std::array buffer; + auto base = LEDPattern::Solid(Color::kMidnightBlue); + auto pattern = base.AtBrightness(100); + pattern.ApplyTo(buffer); + AssertIndexColor(buffer, 0, Color::kWhite); +} + +void AssertIndexColor(std::span data, int index, + Color color) { + frc::Color8Bit color8bit{color}; + + EXPECT_EQ(0, data[index].padding); + EXPECT_EQ(color8bit.red, data[index].r & 0xFF); + EXPECT_EQ(color8bit.green, data[index].g & 0xFF); + EXPECT_EQ(color8bit.blue, data[index].b & 0xFF); +} + +Color LerpColors(Color a, Color b, double t) { + return Color{wpi::Lerp(a.red, b.red, t), wpi::Lerp(a.green, b.green, t), + wpi::Lerp(a.blue, b.blue, t)}; +} +} // namespace frc diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp index d3356c65b8..97c0925e4f 100644 --- a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp @@ -13,27 +13,12 @@ void Robot::RobotInit() { } void Robot::RobotPeriodic() { - // Fill the buffer with a rainbow - Rainbow(); + // Run the rainbow pattern and apply it to the buffer + m_scrollingRainbow.ApplyTo(m_ledBuffer); // Set the LEDs m_led.SetData(m_ledBuffer); } -void Robot::Rainbow() { - // For every pixel - for (int i = 0; i < kLength; i++) { - // Calculate the hue - hue is easier for rainbows because the color - // shape is a circle so only one value needs to precess - const auto pixelHue = (firstPixelHue + (i * 180 / kLength)) % 180; - // Set the value - m_ledBuffer[i].SetHSV(pixelHue, 255, 128); - } - // Increase by to make the rainbow "move" - firstPixelHue += 3; - // Check bounds - firstPixelHue %= 180; -} - #ifndef RUNNING_FRC_TESTS int main() { return frc::StartRobot(); diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/AddressableLED/include/Robot.h index 77090b6a57..488f5e5991 100644 --- a/wpilibcExamples/src/main/cpp/examples/AddressableLED/include/Robot.h +++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/include/Robot.h @@ -7,11 +7,11 @@ #include #include +#include #include class Robot : public frc::TimedRobot { public: - void Rainbow(); void RobotInit() override; void RobotPeriodic() override; @@ -23,6 +23,16 @@ class Robot : public frc::TimedRobot { frc::AddressableLED m_led{9}; std::array m_ledBuffer; // Reuse the buffer - // Store what the last hue of the first pixel is - int firstPixelHue = 0; + + // Our LED strip has a density of 120 LEDs per meter + units::meter_t kLedSpacing{1 / 120.0}; + + // Create an LED pattern that will display a rainbow across + // all hues at maximum saturation and half brightness + frc::LEDPattern m_rainbow = frc::LEDPattern::Rainbow(255, 128); + + // Create a new pattern that scrolls the rainbow pattern across the LED + // strip, moving at a speed of 1 meter per second. + frc::LEDPattern m_scrollingRainbow = + m_rainbow.ScrollAtAbsoluteSpeed(1_mps, kLedSpacing); }; diff --git a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp b/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp deleted file mode 100644 index ca06df9cdd..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/RainbowTest.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// 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. - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "Robot.h" - -void AssertIndexColor(std::array data, - int index, int hue, int saturation, int value); - -TEST(RainbowTest, RainbowPattern) { - Robot robot; - robot.RobotInit(); - frc::sim::AddressableLEDSim ledSim = - frc::sim::AddressableLEDSim::CreateForChannel(9); - EXPECT_TRUE(ledSim.GetRunning()); - EXPECT_EQ(60, ledSim.GetLength()); - - auto rainbowFirstPixelHue = 0; - std::array data; - for (int iteration = 0; iteration < 100; iteration++) { - SCOPED_TRACE(fmt::format("Iteration {} of 100", iteration)); - robot.RobotPeriodic(); - ledSim.GetData(data.data()); - for (int i = 0; i < 60; i++) { - SCOPED_TRACE(fmt::format("LED {} of 60", i)); - auto hue = (rainbowFirstPixelHue + (i * 180 / 60)) % 180; - AssertIndexColor(data, i, hue, 255, 128); - } - rainbowFirstPixelHue += 3; - rainbowFirstPixelHue %= 180; - } -} - -void AssertIndexColor(std::array data, - int index, int hue, int saturation, int value) { - frc::Color8Bit color{frc::Color::FromHSV(hue, saturation, value)}; - - EXPECT_EQ(0, data[index].padding); - EXPECT_EQ(color.red, data[index].r & 0xFF); - EXPECT_EQ(color.green, data[index].g & 0xFF); - EXPECT_EQ(color.blue, data[index].b & 0xFF); -} diff --git a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/main.cpp b/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/main.cpp deleted file mode 100644 index 38d5cfa2fe..0000000000 --- a/wpilibcExamples/src/test/cpp/examples/AddressableLED/cpp/main.cpp +++ /dev/null @@ -1,16 +0,0 @@ -// 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. - -#include -#include - -/** - * Runs all unit tests. - */ -int main(int argc, char** argv) { - HAL_Initialize(500, 0); - ::testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java index f751a31a5e..28bbc4b4c5 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java @@ -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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBufferView.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBufferView.java new file mode 100644 index 0000000000..1f7cfb0d6e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBufferView.java @@ -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 the type of the buffer object to create a view for + */ + public 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; + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java new file mode 100644 index 0000000000..3d39b502c0 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDPattern.java @@ -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). + * + *

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. + * + * 


+ *   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));
+ *     }
+ *   }
+ *
+ * + *

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. + * + * 


+ *   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);
+ *       });
+ *     }
+ *   }
+ *
+ */ +@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. + * + *

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. + * + * 


+   *   AddressableLEDBuffer data = new AddressableLEDBuffer(120);
+   *   LEDPattern pattern = ...
+   *
+   *   void periodic() {
+   *     pattern.applyTo(data);
+   *   }
+   *
+ * + * @param readWriter the object to use for both reading and writing to a set of LEDs + * @param the type of the object that can both read and write LED data + */ + default 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). + * + *

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: + * + * 

+   *   LEDPattern rainbow = LEDPattern.rainbow(255, 255);
+   *   LEDPattern scrollingRainbow = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(25));
+   *
+ * + * @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) { + 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. + * + *

For example, scrolling a pattern at 4 inches per second along an LED strip with 60 LEDs per + * meter: + * + * 

+   *   // LEDs per meter, a known value taken from the spec sheet of our particular LED strip
+   *   Measure<Distance> LED_SPACING = Meters.of(1.0 / 60);
+   *
+   *   LEDPattern rainbow = LEDPattern.rainbow();
+   *   LEDPattern scrollingRainbow =
+   *     rainbow.scrollAtAbsoluteSpeed(InchesPerSecond.of(4), LED_SPACING);
+   *
+ * + *

Note that this pattern will scroll faster if applied to a less dense LED strip (such + * as 30 LEDs per meter), or slower 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, Measure 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

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). + * + *

For example, dimming can be done simply by adding a call to `atBrightness` at the end of a + * pattern: + * + * 

+   *   // 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));
+   *
+ * + * @param relativeBrightness the multiplier to apply to all channels to modify brightness + * @return the input pattern, displayed at + */ + default LEDPattern atBrightness(Measure 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. + * + *

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. + * + * 

+   *   LEDPattern basePattern = gradient(Color.kRed, Color.kBlue);
+   *   LEDPattern progressPattern =
+   *     basePattern.mask(progressMaskLayer(() -> elevator.getHeight() / elevator.maxHeight());
+   *
+ * + * @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) + * + * 
+   *   // 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))
+   *
+ * + * @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 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(); + 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); + } + }; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDReader.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDReader.java new file mode 100644 index 0000000000..176884a922 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDReader.java @@ -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)); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDWriter.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDWriter.java new file mode 100644 index 0000000000..076291d770 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/LEDWriter.java @@ -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); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java index c7c441daf7..0653367ca9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java @@ -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. + * + *

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 */ diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferViewTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferViewTest.java new file mode 100644 index 0000000000..9ec78fdc6f --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferViewTest.java @@ -0,0 +1,69 @@ +// 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 org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.wpilibj.util.Color; +import org.junit.jupiter.api.Test; + +class AddressableLEDBufferViewTest { + @Test + void singleLED() { + var buffer = new AddressableLEDBuffer(10); + var view = new AddressableLEDBufferView(buffer, 5, 5); + var color = Color.kAqua; + view.setLED(0, color); + assertEquals(color, buffer.getLED(5)); + assertEquals(color, view.getLED(0)); + } + + @Test + void segment() { + var buffer = new AddressableLEDBuffer(10); + var view = new AddressableLEDBufferView(buffer, 2, 8); + view.setLED(0, Color.kAqua); + assertEquals(Color.kAqua, buffer.getLED(2)); + + view.setLED(6, Color.kAzure); + assertEquals(Color.kAzure, buffer.getLED(8)); + } + + @Test + void manualReversed() { + var buffer = new AddressableLEDBuffer(10); + var view = new AddressableLEDBufferView(buffer, 8, 2); + + // LED 0 in the view should write to LED 8 on the real buffer + view.setLED(0, Color.kAqua); + assertEquals(Color.kAqua, buffer.getLED(8)); + + // .. and LED 6 in the view should write to LED 2 on the real buffer + view.setLED(6, Color.kAzure); + assertEquals(Color.kAzure, buffer.getLED(2)); + } + + @Test + void fullManualReversed() { + var buffer = new AddressableLEDBuffer(10); + var view = new AddressableLEDBufferView(buffer, 9, 0); + view.setLED(0, Color.kWhite); + assertEquals(Color.kWhite, buffer.getLED(9)); + + buffer.setLED(8, Color.kRed); + assertEquals(Color.kRed, view.getLED(1)); + } + + @Test + void reversed() { + var buffer = new AddressableLEDBuffer(10); + var view = new AddressableLEDBufferView(buffer, 0, 9).reversed(); + view.setLED(0, Color.kWhite); + assertEquals(Color.kWhite, buffer.getLED(9)); + + view.setLED(9, Color.kRed); + assertEquals(Color.kRed, buffer.getLED(0)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/LEDPatternTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LEDPatternTest.java new file mode 100644 index 0000000000..f6d38ed748 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/LEDPatternTest.java @@ -0,0 +1,798 @@ +// 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.Centimeters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Microsecond; +import static edu.wpi.first.units.Units.Microseconds; +import static edu.wpi.first.units.Units.Percent; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Value; +import static edu.wpi.first.wpilibj.util.Color.kBlack; +import static edu.wpi.first.wpilibj.util.Color.kBlue; +import static edu.wpi.first.wpilibj.util.Color.kLime; +import static edu.wpi.first.wpilibj.util.Color.kMagenta; +import static edu.wpi.first.wpilibj.util.Color.kMidnightBlue; +import static edu.wpi.first.wpilibj.util.Color.kPurple; +import static edu.wpi.first.wpilibj.util.Color.kRed; +import static edu.wpi.first.wpilibj.util.Color.kWhite; +import static edu.wpi.first.wpilibj.util.Color.kYellow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.fail; + +import edu.wpi.first.util.WPIUtilJNI; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import java.util.Map; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicReference; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class LEDPatternTest { + // Applies a pattern of White, Yellow, Purple to LED triplets + LEDPattern m_whiteYellowPurple = + (reader, writer) -> { + for (int led = 0; led < reader.getLength(); led++) { + switch (led % 3) { + case 0: + writer.setLED(led, kWhite); + break; + case 1: + writer.setLED(led, kYellow); + break; + case 2: + writer.setLED(led, kPurple); + break; + default: + fail("Bad test setup"); + break; + } + } + }; + + @BeforeEach + void setUp() { + WPIUtilJNI.enableMockTime(); + WPIUtilJNI.setMockTime(0L); + } + + @AfterEach + void tearDown() { + WPIUtilJNI.setMockTime(0L); + WPIUtilJNI.disableMockTime(); + } + + @Test + void solidColor() { + LEDPattern pattern = LEDPattern.solid(kYellow); + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + pattern.applyTo(buffer); + + for (int i = 0; i < buffer.getLength(); i++) { + assertEquals(kYellow, buffer.getLED(i)); + } + } + + @Test + void gradient0SetsToBlack() { + LEDPattern pattern = LEDPattern.gradient(); + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setRGB(i, 127, 128, 129); + } + + pattern.applyTo(buffer); + + for (int i = 0; i < buffer.getLength(); i++) { + assertEquals(kBlack, buffer.getLED(i)); + } + } + + @Test + void gradient1SetsToSolid() { + LEDPattern pattern = LEDPattern.gradient(kYellow); + + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + pattern.applyTo(buffer); + + for (int i = 0; i < buffer.getLength(); i++) { + assertEquals(kYellow, buffer.getLED(i)); + } + } + + @Test + void gradient2Colors() { + LEDPattern pattern = LEDPattern.gradient(kYellow, kPurple); + + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + pattern.applyTo(buffer); + + assertColorEquals(kYellow, buffer.getLED(0)); + assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25 / 49.0), buffer.getLED(25)); + assertColorEquals(kPurple, buffer.getLED(49)); + assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25 / 49.0), buffer.getLED(73)); + assertColorEquals(kYellow, buffer.getLED(98)); + } + + @Test + void gradient3Colors() { + LEDPattern pattern = LEDPattern.gradient(kYellow, kPurple, kWhite); + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + pattern.applyTo(buffer); + + assertColorEquals(kYellow, buffer.getLED(0)); + assertColorEquals(Color.lerpRGB(kYellow, kPurple, 25.0 / 33.0), buffer.getLED(25)); + assertColorEquals(kPurple, buffer.getLED(33)); + assertColorEquals(Color.lerpRGB(kPurple, kWhite, 25.0 / 33.0), buffer.getLED(58)); + assertColorEquals(kWhite, buffer.getLED(66)); + assertColorEquals(Color.lerpRGB(kWhite, kYellow, 25.0 / 33.0), buffer.getLED(91)); + assertColorEquals(Color.lerpRGB(kWhite, kYellow, 32.0 / 33.0), buffer.getLED(98)); + } + + @Test + void step0SetsToBlack() { + LEDPattern pattern = LEDPattern.steps(Map.of()); + + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setRGB(i, 127, 128, 129); + } + + pattern.applyTo(buffer); + for (int i = 0; i < 99; i++) { + assertColorEquals(kBlack, buffer.getLED(i)); + } + } + + @Test + void step1SetsToSolid() { + LEDPattern pattern = LEDPattern.steps(Map.of(0.0, kYellow)); + + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + + pattern.applyTo(buffer); + for (int i = 0; i < 99; i++) { + assertColorEquals(kYellow, buffer.getLED(i)); + } + } + + @Test + void step1HalfSetsToHalfOffHalfColor() { + LEDPattern pattern = LEDPattern.steps(Map.of(0.50, kYellow)); + + AddressableLEDBuffer buffer = new AddressableLEDBuffer(99); + pattern.applyTo(buffer); + + // [0, 48] should be black... + for (int i = 0; i < 49; i++) { + assertColorEquals(kBlack, buffer.getLED(i)); + } + // ... and [49, ] should be the color that was set + for (int i = 49; i < buffer.getLength(); i++) { + assertColorEquals(kYellow, buffer.getLED(i)); + } + } + + @Test + void scrollForward() { + var buffer = new AddressableLEDBuffer(256); + + LEDPattern base = + (reader, writer) -> { + for (int led = 0; led < reader.getLength(); led++) { + writer.setRGB(led, led % 256, led % 256, led % 256); + } + }; + + // scroll forwards 1/256th (1 LED) per microsecond - this makes mock time easier + var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(1 / 256.0)); + + for (int time = 0; time < 500; time++) { + WPIUtilJNI.setMockTime(time); + scroll.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)] + // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = Math.floorMod(led - time, 256); + + assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led)); + } + } + } + + @Test + void scrollBackward() { + var buffer = new AddressableLEDBuffer(256); + + LEDPattern base = + (reader, writer) -> { + for (int led = 0; led < reader.getLength(); led++) { + writer.setRGB(led, led % 256, led % 256, led % 256); + } + }; + + // scroll backwards 1/256th (1 LED) per microsecond - this makes mock time easier + var scroll = base.scrollAtRelativeSpeed(Value.per(Microsecond).of(-1 / 256.0)); + + for (int time = 0; time < 500; time++) { + WPIUtilJNI.setMockTime(time); + scroll.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)] + // Value for every channel should INCREASE by 1 in each timestep, wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (1, 2, 3, ..., 255, 0) + // t=2, channel value = (2, 3, 4, ..., 0, 1) + // t=255, channel value = (255, 0, 1, ..., 253, 254) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = Math.floorMod(led + time, 256); + + assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led)); + } + } + } + + @Test + void scrollAbsoluteSpeedForward() { + var buffer = new AddressableLEDBuffer(256); + + LEDPattern base = + (reader, writer) -> { + for (int led = 0; led < reader.getLength(); led++) { + writer.setRGB(led, led % 256, led % 256, led % 256); + } + }; + + // scroll at 16 m/s, LED spacing = 2cm + // buffer is 256 LEDs, so total length = 512cm = 5.12m + // scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s) + var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(16), Centimeters.of(2)); + + for (int time = 0; time < 500; time++) { + WPIUtilJNI.setMockTime(time * 1_250); // 1.25ms per LED + scroll.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)] + // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = Math.floorMod(led - time, 256); + + assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led)); + } + } + } + + @Test + void scrollAbsoluteSpeedBackward() { + var buffer = new AddressableLEDBuffer(256); + + LEDPattern base = + (reader, writer) -> { + for (int led = 0; led < reader.getLength(); led++) { + writer.setRGB(led, led % 256, led % 256, led % 256); + } + }; + + // scroll at 16 m/s, LED spacing = 2cm + // buffer is 256 LEDs, so total length = 512cm = 5.12m + // scrolling at 16 m/s yields a period of 0.32 seconds, or 0.00125 seconds per LED (800 LEDs/s) + var scroll = base.scrollAtAbsoluteSpeed(MetersPerSecond.of(-16), Centimeters.of(2)); + + for (int time = 0; time < 500; time++) { + WPIUtilJNI.setMockTime(time * 1_250); // 1.25ms per LED + scroll.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + // Base: [(0, 0, 0) (1, 1, 1) (2, 2, 2) (3, 3, 3) (4, 4, 4) ... (255, 255, 255)] + // Value for every channel should DECREASE by 1 in each timestep, wrapping around 0 and 255 + + // t=0, channel value = (0, 1, 2, ..., 254, 255) + // t=1, channel value = (255, 0, 1, ..., 253, 254) + // t=2, channel value = (254, 255, 0, ..., 252, 253) + // t=255, channel value = (1, 2, 3, ..., 255, 0) + // t=256, channel value = (0, 1, 2, ..., 254, 255) + int ch = Math.floorMod(led + time, 256); + + assertEquals(new Color8Bit(ch, ch, ch), buffer.getLED8Bit(led)); + } + } + } + + @Test + void rainbowAtFullSize() { + var buffer = new AddressableLEDBuffer(180); + + int saturation = 255; + int value = 255; + var pattern = LEDPattern.rainbow(saturation, value); + + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.fromHSV(led, saturation, value), buffer.getLED(led)); + } + } + + @Test + void rainbowAtHalfSize() { + var buffer = new AddressableLEDBuffer(90); + + int saturation = 42; + int value = 87; + var pattern = LEDPattern.rainbow(saturation, value); + + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.fromHSV(led * 2, saturation, value), buffer.getLED(led)); + } + } + + @Test + void rainbowAtOneThirdSize() { + var buffer = new AddressableLEDBuffer(60); + + int saturation = 191; + int value = 255; + var pattern = LEDPattern.rainbow(saturation, value); + + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.fromHSV(led * 3, saturation, value), buffer.getLED(led)); + } + } + + @Test + void rainbowAtDoubleSize() { + var buffer = new AddressableLEDBuffer(360); + + int saturation = 212; + int value = 93; + var pattern = LEDPattern.rainbow(saturation, value); + + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.fromHSV(led / 2, saturation, value), buffer.getLED(led)); + } + } + + @Test + void rainbowOddSize() { + var buffer = new AddressableLEDBuffer(127); + double scale = 180.0 / buffer.getLength(); + + int saturation = 73; + int value = 128; + var pattern = LEDPattern.rainbow(saturation, value); + + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.fromHSV((int) (led * scale), saturation, value), buffer.getLED(led)); + } + } + + @Test + void reverseSolid() { + var buffer = new AddressableLEDBuffer(90); + + var pattern = LEDPattern.solid(Color.kRosyBrown).reversed(); + pattern.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + assertColorEquals(Color.kRosyBrown, buffer.getLED(led)); + } + } + + @Test + void reverseSteps() { + var buffer = new AddressableLEDBuffer(100); + + var pattern = LEDPattern.steps(Map.of(0, kWhite, 0.5, kYellow)).reversed(); + pattern.applyTo(buffer); + + // colors should be swapped; yellow first, then white + for (int led = 0; led < buffer.getLength(); led++) { + if (led < 50) { + assertColorEquals(kYellow, buffer.getLED(led)); + } else { + assertColorEquals(kWhite, buffer.getLED(led)); + } + } + } + + @Test + void offsetPositive() { + var buffer = new AddressableLEDBuffer(21); + + // offset repeats PWY + var offset = m_whiteYellowPurple.offsetBy(1); + offset.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + Color color = buffer.getLED(led); + switch (led % 3) { + case 0: + assertColorEquals(kPurple, color); + break; + case 1: + assertColorEquals(kWhite, color); + break; + case 2: + assertColorEquals(kYellow, color); + break; + default: + fail("Bad test setup"); + break; + } + } + } + + @Test + void offsetNegative() { + var buffer = new AddressableLEDBuffer(21); + + // offset repeats YPW + var offset = m_whiteYellowPurple.offsetBy(-1); + offset.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + Color color = buffer.getLED(led); + switch (led % 3) { + case 0: + assertColorEquals(kYellow, color); + break; + case 1: + assertColorEquals(kPurple, color); + break; + case 2: + assertColorEquals(kWhite, color); + break; + default: + fail("Bad test setup"); + break; + } + } + } + + @Test + void offsetZero() { + var buffer = new AddressableLEDBuffer(21); + + // offset copies the base pattern, WYP + var offset = m_whiteYellowPurple.offsetBy(0); + offset.applyTo(buffer); + + for (int led = 0; led < buffer.getLength(); led++) { + Color color = buffer.getLED(led); + switch (led % 3) { + case 0: + assertColorEquals(kWhite, color); + break; + case 1: + assertColorEquals(kYellow, color); + break; + case 2: + assertColorEquals(kPurple, color); + break; + default: + fail("Bad test setup"); + break; + } + } + } + + @Test + void blinkSymmetric() { + // on for 2 seconds, off for 2 seconds + var pattern = LEDPattern.solid(kWhite).blink(Seconds.of(2)); + + var buffer = new AddressableLEDBuffer(1); + + for (int t = 0; t < 8; t++) { + WPIUtilJNI.setMockTime(t * 1_000_000L); // time travel 1 second + pattern.applyTo(buffer); + + Color color = buffer.getLED(0); + switch (t) { + case 0: + case 1: + case 4: + case 5: + assertColorEquals(kWhite, color); + break; + case 2: + case 3: + case 6: + case 7: + assertColorEquals(kBlack, color); + break; + default: + fail("Bad test setup"); + break; + } + } + } + + @Test + void blinkAsymmetric() { + // on for 3 seconds, off for 1 second + var pattern = LEDPattern.solid(kWhite).blink(Seconds.of(3), Seconds.of(1)); + + var buffer = new AddressableLEDBuffer(1); + + for (int t = 0; t < 8; t++) { + WPIUtilJNI.setMockTime(t * 1_000_000L); // time travel 1 second + pattern.applyTo(buffer); + + Color color = buffer.getLED(0); + switch (t) { + case 0: + case 1: + case 2: // first period + case 4: + case 5: + case 6: // second period + assertColorEquals(kWhite, color); + break; + case 3: + case 7: + assertColorEquals(kBlack, color); + break; + default: + fail("Bad test setup"); + break; + } + } + } + + @Test + void blinkInSync() { + AtomicBoolean condition = new AtomicBoolean(false); + var pattern = LEDPattern.solid(kWhite).synchronizedBlink(condition::get); + + var buffer = new AddressableLEDBuffer(1); + + pattern.applyTo(buffer); + assertColorEquals(kBlack, buffer.getLED(0)); + + condition.set(true); + pattern.applyTo(buffer); + assertColorEquals(kWhite, buffer.getLED(0)); + + condition.set(false); + pattern.applyTo(buffer); + assertColorEquals(kBlack, buffer.getLED(0)); + } + + @Test + void breathe() { + final Color midGray = new Color(0.5, 0.5, 0.5); + + var pattern = LEDPattern.solid(kWhite).breathe(Microseconds.of(4)); + + var buffer = new AddressableLEDBuffer(1); + + { + WPIUtilJNI.setMockTime(0); // start + pattern.applyTo(buffer); + assertColorEquals(kWhite, buffer.getLED(0)); + } + + { + WPIUtilJNI.setMockTime(1); // midway (down) + pattern.applyTo(buffer); + assertColorEquals(midGray, buffer.getLED(0)); + } + + { + WPIUtilJNI.setMockTime(2); // bottom + pattern.applyTo(buffer); + assertColorEquals(kBlack, buffer.getLED(0)); + } + + { + WPIUtilJNI.setMockTime(3); // midway (up) + pattern.applyTo(buffer); + assertColorEquals(midGray, buffer.getLED(0)); + } + + { + WPIUtilJNI.setMockTime(4); // back to start + pattern.applyTo(buffer); + assertColorEquals(kWhite, buffer.getLED(0)); + } + } + + @Test + void overlaySolidOnSolid() { + var overlay = LEDPattern.solid(kYellow).overlayOn(LEDPattern.solid(kWhite)); + + var buffer = new AddressableLEDBuffer(1); + overlay.applyTo(buffer); + + assertColorEquals(kYellow, buffer.getLED(0)); + } + + @Test + void overlayNearlyBlack() { + Color overlayColor = new Color(new Color8Bit(1, 0, 0)); + var overlay = LEDPattern.solid(overlayColor).overlayOn(LEDPattern.solid(kWhite)); + + var buffer = new AddressableLEDBuffer(1); + overlay.applyTo(buffer); + + assertColorEquals(overlayColor, buffer.getLED(0)); + } + + @Test + void overlayMixed() { + var overlay = + LEDPattern.steps(Map.of(0, kYellow, 0.5, kBlack)).overlayOn(LEDPattern.solid(kWhite)); + + var buffer = new AddressableLEDBuffer(2); + overlay.applyTo(buffer); + + assertColorEquals(kYellow, buffer.getLED(0)); + assertColorEquals(kWhite, buffer.getLED(1)); + } + + @Test + void blend() { + var pattern1 = LEDPattern.solid(kBlue); + var pattern2 = LEDPattern.solid(kRed); + var blend = pattern1.blend(pattern2); + + var buffer = new AddressableLEDBuffer(1); + blend.applyTo(buffer); + + // Individual RGB channels are averaged; #0000FF blended with #FF0000 yields #7F007F + assertColorEquals(new Color(127, 0, 127), buffer.getLED(0)); + } + + @Test + void binaryMask() { + Color color = new Color(123, 123, 123); + var base = LEDPattern.solid(color); + // first 50% mask on, last 50% mask off + var mask = LEDPattern.steps(Map.of(0, kWhite, 0.5, kBlack)); + var masked = base.mask(mask); + + var buffer = new AddressableLEDBuffer(10); + masked.applyTo(buffer); + + for (int i = 0; i < 5; i++) { + assertColorEquals(color, buffer.getLED(i)); + } + + for (int i = 5; i < 10; i++) { + assertColorEquals(kBlack, buffer.getLED(i)); + } + } + + @Test + void channelwiseMask() { + Color baseColor = new Color(123, 123, 123); + Color halfGray = new Color(0.5, 0.5, 0.5); + var base = LEDPattern.solid(baseColor); + + var mask = + LEDPattern.steps(Map.of(0, kRed, 0.2, kLime, 0.4, kBlue, 0.6, halfGray, 0.8, kWhite)); + + var masked = base.mask(mask); + + var buffer = new AddressableLEDBuffer(5); + masked.applyTo(buffer); + + assertColorEquals(new Color(123, 0, 0), buffer.getLED(0)); // red channel only + assertColorEquals(new Color(0, 123, 0), buffer.getLED(1)); // green channel only + assertColorEquals(new Color(0, 0, 123), buffer.getLED(2)); // blue channel only + + // mask channels are all 0b00111111, base is 0b00111011, + // so the AND should give us the unmodified base color + assertColorEquals(baseColor, buffer.getLED(3)); + assertColorEquals(baseColor, buffer.getLED(4)); // full color allowed + } + + @Test + void progressMaskLayer() { + var progress = new AtomicReference<>(0.0); + var maskLayer = LEDPattern.progressMaskLayer(progress::get); + var buffer = new AddressableLEDBuffer(100); + + for (double t = 0; t <= 1.0; t += 0.01) { + progress.set(t); + maskLayer.applyTo(buffer); + + int lastMaskedLED = (int) (t * 100); + for (int i = 0; i < lastMaskedLED; i++) { + assertColorEquals( + kWhite, + buffer.getLED(i), + "Progress " + lastMaskedLED + "%, LED " + i + " should be WHITE"); + } + + for (int i = lastMaskedLED; i < 100; i++) { + assertColorEquals( + kBlack, + buffer.getLED(i), + "Progress " + lastMaskedLED + "% , LED " + i + " should be BLACK"); + } + } + } + + @Test + void zeroBrightness() { + var pattern = LEDPattern.solid(kRed).atBrightness(Percent.zero()); + var buffer = new AddressableLEDBuffer(1); + pattern.applyTo(buffer); + + assertColorEquals(kBlack, buffer.getLED(0)); + } + + @Test + void sameBrightness() { + var pattern = LEDPattern.solid(kMagenta).atBrightness(Percent.of(100)); + var buffer = new AddressableLEDBuffer(1); + pattern.applyTo(buffer); + + assertColorEquals(kMagenta, buffer.getLED(0)); + } + + @Test + void higherBrightness() { + var pattern = LEDPattern.solid(kMagenta).atBrightness(Value.of(4 / 3.0)); + var buffer = new AddressableLEDBuffer(1); + pattern.applyTo(buffer); + + assertColorEquals(kMagenta, buffer.getLED(0)); + } + + @Test + void negativeBrightness() { + var pattern = LEDPattern.solid(kWhite).atBrightness(Percent.of(-1000)); + var buffer = new AddressableLEDBuffer(1); + pattern.applyTo(buffer); + + assertColorEquals(kBlack, buffer.getLED(0)); + } + + @Test + void clippingBrightness() { + var pattern = LEDPattern.solid(kMidnightBlue).atBrightness(Percent.of(10000)); + var buffer = new AddressableLEDBuffer(1); + pattern.applyTo(buffer); + + assertColorEquals(kWhite, buffer.getLED(0)); + } + + void assertColorEquals(Color expected, Color actual) { + assertEquals(new Color8Bit(expected), new Color8Bit(actual)); + } + + void assertColorEquals(Color expected, Color actual, String message) { + assertEquals(new Color8Bit(expected), new Color8Bit(actual), message); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java index 1b32afdaa9..76901c2f40 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java @@ -4,10 +4,16 @@ package edu.wpi.first.wpilibj.util; +import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.params.provider.Arguments.arguments; +import java.util.stream.Stream; import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; class ColorTest { @Test @@ -82,4 +88,39 @@ class ColorTest { assertEquals("#FF8040", color.toHexString()); assertEquals("#FF8040", color.toString()); } + + @ParameterizedTest + @MethodSource("hsvToRgbProvider") + void hsvToRgb(int h, int s, int v, int r, int g, int b) { + int rgb = Color.hsvToRgb(h, s, v); + int R = Color.unpackRGB(rgb, Color.RGBChannel.kRed); + int G = Color.unpackRGB(rgb, Color.RGBChannel.kGreen); + int B = Color.unpackRGB(rgb, Color.RGBChannel.kBlue); + + assertAll( + () -> assertEquals(r, R, "R value didn't match"), + () -> assertEquals(g, G, "G value didn't match"), + () -> assertEquals(b, B, "B value didn't match")); + } + + private static Stream hsvToRgbProvider() { + return Stream.of( + arguments(0, 0, 0, 0, 0, 0), // Black + arguments(0, 0, 255, 255, 255, 255), // White + arguments(0, 255, 255, 255, 0, 0), // Red + arguments(60, 255, 255, 0, 255, 0), // Lime + arguments(120, 255, 255, 0, 0, 255), // Blue + arguments(30, 255, 255, 255, 255, 0), // Yellow + arguments(90, 255, 255, 0, 255, 255), // Cyan + arguments(150, 255, 255, 255, 0, 255), // Magenta + arguments(0, 0, 191, 191, 191, 191), // Silver + arguments(0, 0, 128, 128, 128, 128), // Gray + arguments(0, 255, 128, 128, 0, 0), // Maroon + arguments(30, 255, 128, 128, 128, 0), // Olive + arguments(60, 255, 128, 0, 128, 0), // Green + arguments(150, 255, 128, 128, 0, 128), // Purple + arguments(90, 255, 128, 0, 128, 128), // Teal + arguments(120, 255, 128, 0, 0, 128) // Navy + ); + } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java index 5995cd044f..3c5b2f7eb0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java @@ -4,15 +4,31 @@ package edu.wpi.first.wpilibj.examples.addressableled; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; + +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.TimedRobot; public class Robot extends TimedRobot { private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; - // Store what the last hue of the first pixel is - private int m_rainbowFirstPixelHue; + + // Create an LED pattern that will display a rainbow across + // all hues at maximum saturation and half brightness + private final LEDPattern m_rainbow = LEDPattern.rainbow(255, 128); + + // Our LED strip has a density of 120 LEDs per meter + private static final Measure kLedSpacing = Meters.of(1 / 120.0); + + // Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a speed + // of 1 meter per second. + private final LEDPattern m_scrollingRainbow = + m_rainbow.scrollAtAbsoluteSpeed(MetersPerSecond.of(1), kLedSpacing); @Override public void robotInit() { @@ -33,24 +49,9 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { - // Fill the buffer with a rainbow - rainbow(); + // Update the buffer with the rainbow animation + m_scrollingRainbow.applyTo(m_ledBuffer); // Set the LEDs m_led.setData(m_ledBuffer); } - - private void rainbow() { - // For every pixel - for (var i = 0; i < m_ledBuffer.getLength(); i++) { - // Calculate the hue - hue is easier for rainbows because the color - // shape is a circle so only one value needs to precess - final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180; - // Set the value - m_ledBuffer.setHSV(i, hue, 255, 128); - } - // Increase by to make the rainbow "move" - m_rainbowFirstPixelHue += 3; - // Check bounds - m_rainbowFirstPixelHue %= 180; - } } diff --git a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/addressableled/RainbowTest.java b/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/addressableled/RainbowTest.java deleted file mode 100644 index 90acfb08bd..0000000000 --- a/wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/addressableled/RainbowTest.java +++ /dev/null @@ -1,54 +0,0 @@ -// 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.examples.addressableled; - -import static org.junit.jupiter.api.Assertions.assertAll; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import edu.wpi.first.hal.HAL; -import edu.wpi.first.wpilibj.simulation.AddressableLEDSim; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import org.junit.jupiter.api.Test; - -class RainbowTest { - @Test - void rainbowPatternTest() { - HAL.initialize(500, 0); - try (var robot = new Robot()) { - robot.robotInit(); - AddressableLEDSim ledSim = AddressableLEDSim.createForChannel(9); - assertTrue(ledSim.getRunning()); - assertEquals(60, ledSim.getLength()); - - var rainbowFirstPixelHue = 0; - for (int iteration = 0; iteration < 100; iteration++) { - robot.robotPeriodic(); - var data = ledSim.getData(); - for (int i = 0; i < 60; i++) { - final var hue = (rainbowFirstPixelHue + (i * 180 / 60)) % 180; - assertIndexColor(data, i, hue, 255, 128); - } - rainbowFirstPixelHue += 3; - rainbowFirstPixelHue %= 180; - } - } - } - - private void assertIndexColor(byte[] data, int index, int hue, int saturation, int value) { - var color = new Color8Bit(Color.fromHSV(hue, saturation, value)); - int b = data[index * 4]; - int g = data[(index * 4) + 1]; - int r = data[(index * 4) + 2]; - int z = data[(index * 4) + 3]; - - assertAll( - () -> assertEquals(0, z), - () -> assertEquals(color.red, r & 0xFF), - () -> assertEquals(color.green, g & 0xFF), - () -> assertEquals(color.blue, b & 0xFF)); - } -} diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 26f106ef6b..76e11ce64b 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -169,4 +169,41 @@ constexpr units::radian_t AngleModulus(units::radian_t angle) { units::radian_t{std::numbers::pi}); } +// floorDiv and floorMod algorithms taken from Java + +/** + * Returns the largest (closest to positive infinity) + * {@code int} value that is less than or equal to the algebraic quotient. + * + * @param x the dividend + * @param y the divisor + * @return the largest (closest to positive infinity) + * {@code int} value that is less than or equal to the algebraic quotient. + */ +constexpr std::signed_integral auto FloorDiv(std::signed_integral auto x, + std::signed_integral auto y) { + auto quot = x / y; + auto rem = x % y; + // if the signs are different and modulo not zero, round down + if ((x < 0) != (y < 0) && rem != 0) { + --quot; + } + return quot; +} + +/** + * Returns the floor modulus of the {@code int} arguments. + *

+ * The floor modulus is {@code r = x - (floorDiv(x, y) * y)}, + * has the same sign as the divisor {@code y} or is zero, and + * is in the range of {@code -std::abs(y) < r < +std::abs(y)}. + * + * @param x the dividend + * @param y the divisor + * @return the floor modulus {@code x - (floorDiv(x, y) * y)} + */ +constexpr std::signed_integral auto FloorMod(std::signed_integral auto x, + std::signed_integral auto y) { + return x - FloorDiv(x, y) * y; +} } // namespace frc