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