diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp index 0209484672..b0c3a4506a 100644 --- a/wpilibc/src/main/native/cpp/AddressableLED.cpp +++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp @@ -92,3 +92,38 @@ void AddressableLED::Stop() { HAL_StopAddressableLEDOutput(m_handle, &status); wpi_setHALError(status); } + +void AddressableLED::LEDData::SetHSV(int h, int s, int v) { + if (s == 0) { + SetRGB(v, v, v); + return; + } + + int region = h / 30; + int remainder = (h - (region * 30)) * 6; + + int p = (v * (255 - s)) >> 8; + int q = (v * (255 - ((s * remainder) >> 8))) >> 8; + int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8; + + switch (region) { + case 0: + SetRGB(v, t, p); + break; + case 1: + SetRGB(q, v, p); + break; + case 2: + SetRGB(p, v, t); + break; + case 3: + SetRGB(p, q, v); + break; + case 4: + SetRGB(t, p, v); + break; + default: + SetRGB(v, p, q); + break; + } +} diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h index 3b7409914a..2ffe0ea382 100644 --- a/wpilibc/src/main/native/include/frc/AddressableLED.h +++ b/wpilibc/src/main/native/include/frc/AddressableLED.h @@ -35,12 +35,25 @@ class AddressableLED : public ErrorBase { /** * A helper method to set all values of the LED. + * + * @param r the r value [0-255] + * @param g the g value [0-255] + * @param b the b value [0-255] */ - void SetLED(int r, int g, int b) { + void SetRGB(int r, int g, int b) { this->r = r; this->g = g; this->b = b; } + + /** + * A helper method to set all values of the LED. + * + * @param h the h value [0-180] + * @param s the s value [0-255] + * @param v the v value [0-255] + */ + void SetHSV(int h, int s, int v); }; /** diff --git a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp index aa758e31a4..3cf84e7eb6 100644 --- a/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp @@ -12,35 +12,44 @@ #include class Robot : public frc::TimedRobot { + static constexpr int kLength = 60; + // PWM port 0 // Must be a PWM header, not MXP or DIO - frc::AddressableLED m_led{0}; - std::array m_ledBuffer; // Reuse the buffer - int m_count = 0; + 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; public: + void 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; + } + void RobotInit() override { - // Default to a length of 12, start empty output + // Default to a length of 60, start empty output // Length is expensive to set, so only set it once, then just update data - m_led.SetLength(12); + m_led.SetLength(kLength); m_led.SetData(m_ledBuffer); m_led.Start(); } void RobotPeriodic() override { - // Zero out all LEDs - for (auto& ledData : m_ledBuffer) { - ledData.SetLED(0, 0, 0); - } - - // Set 1 single LED to red - m_ledBuffer[m_count].SetLED(50, 0, 0); - - // Continue moving LED - m_count++; - if (m_count >= 12) m_count = 0; - - // Buffer must be written to update. + // Fill the buffer with a rainbow + Rainbow(); + // Set the LEDs m_led.SetData(m_ledBuffer); } }; 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 e7586f3bdd..8f9c1b4f12 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java @@ -26,18 +26,62 @@ public class AddressableLEDBuffer { * Sets a specific led in the buffer. * * @param index the index to write - * @param r the r value - * @param g the g value - * @param b the b value + * @param r the r value [0-255] + * @param g the g value [0-255] + * @param b the b value [0-255] */ @SuppressWarnings("ParameterName") - public void setLED(int index, int r, int g, int b) { + public void setRGB(int index, int r, int g, int b) { m_buffer[index * 4] = (byte) b; m_buffer[(index * 4) + 1] = (byte) g; m_buffer[(index * 4) + 2] = (byte) r; 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] + */ + @SuppressWarnings("ParameterName") + public void setHSV(final int index, final int h, final int s, final int v) { + if (s == 0) { + setRGB(index, v, v, v); + return; + } + + final int region = h / 30; + final int remainder = (h - (region * 30)) * 6; + + final int p = (v * (255 - s)) >> 8; + final int q = (v * (255 - ((s * remainder) >> 8))) >> 8; + final int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8; + + switch (region) { + case 0: + setRGB(index, v, t, p); + break; + case 1: + setRGB(index, q, v, p); + break; + case 2: + setRGB(index, p, v, t); + break; + case 3: + setRGB(index, p, q, v); + break; + case 4: + setRGB(index, t, p, v); + break; + default: + setRGB(index, v, p, q); + break; + } + } + /** * Gets the buffer length. * diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java new file mode 100644 index 0000000000..13f1651088 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import java.util.stream.Stream; + +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; +import org.junit.jupiter.params.provider.MethodSource; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.params.provider.Arguments.arguments; + +class AddressableLEDBufferTest { + @ParameterizedTest + @MethodSource("hsvToRgbProvider") + @SuppressWarnings("ParameterName") + void hsvConvertTest(int h, int s, int v, int r, int g, int b) { + var buffer = new AddressableLEDBuffer(1); + buffer.setHSV(0, h, s, v); + assertAll( + () -> assertEquals((byte) r, buffer.m_buffer[2], "R value didn't match"), + () -> assertEquals((byte) g, buffer.m_buffer[1], "G value didn't match"), + () -> assertEquals((byte) b, buffer.m_buffer[0], "B value didn't match") + ); + } + + 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, 254, 255, 0), // Yellow (ish) + arguments(90, 255, 255, 0, 254, 255), // Cyan (ish) + arguments(150, 255, 255, 255, 0, 254), // Magenta (ish) + 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, 127, 128, 0), // Olive (ish) + arguments(60, 255, 128, 0, 128, 0), // Green + arguments(150, 255, 128, 128, 0, 127), // Purple (ish) + arguments(90, 255, 128, 0, 127, 128), // Teal (ish) + 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 7e0bb7f54c..b89365b802 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 @@ -14,7 +14,8 @@ import edu.wpi.first.wpilibj.TimedRobot; public class Robot extends TimedRobot { private AddressableLED m_led; private AddressableLEDBuffer m_ledBuffer; - private int m_count; + // Store what the last hue of the first pixel is + private int m_rainbowFirstPixelHue; @Override public void robotInit() { @@ -22,36 +23,37 @@ public class Robot extends TimedRobot { // Must be a PWM header, not MXP or DIO m_led = new AddressableLED(0); - // Default to a length of 12, start empty output - // Length is expensive to set, so only set it once, then just update data - m_led.setLength(12); - // Reuse buffer - m_ledBuffer = new AddressableLEDBuffer(12); + // Default to a length of 60, start empty output + // Length is expensive to set, so only set it once, then just update data + m_ledBuffer = new AddressableLEDBuffer(60); + m_led.setLength(m_ledBuffer.getLength()); + // Set the data m_led.setData(m_ledBuffer); m_led.start(); - } @Override public void robotPeriodic() { - // Zero out all LEDS - for (int i = 0; i < m_ledBuffer.getLength(); i++) { - m_ledBuffer.setLED(i, 0, 0, 0); - } - - // Set 1 single LED to red - m_ledBuffer.setLED(m_count, 50, 0, 0); - - // Continue moving LED - m_count++; - if (m_count >= 12) { - m_count = 0; - } - - // Buffer must be written to update. + // Fill the buffer with a rainbow + rainbow(); + // 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; + } }