Add HSV helpers to AddressableLED (#2135)

Also change the setLED() method to setRGB() for consistency and clarity.

Add rainbow example to demonstrate HSV usage.

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

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

wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp

@@ -12,35 +12,44 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 
 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<frc::AddressableLED::LEDData, 12> m_ledBuffer;  // Reuse the buffer
-  int m_count = 0;
+  frc::AddressableLED m_led{9};
+  std::array<frc::AddressableLED::LEDData, kLength>
+      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);
   }
 };

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

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

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