diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp index b0c3a4506a..81adc7bbcd 100644 --- a/wpilibc/src/main/native/cpp/AddressableLED.cpp +++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 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. */ @@ -94,36 +94,5 @@ void AddressableLED::Stop() { } 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; - } + SetLED(Color::FromHSV(h, s, v)); } diff --git a/wpilibc/src/main/native/include/frc/util/Color.h b/wpilibc/src/main/native/include/frc/util/Color.h index 65d5fa644c..1321380084 100644 --- a/wpilibc/src/main/native/include/frc/util/Color.h +++ b/wpilibc/src/main/native/include/frc/util/Color.h @@ -755,6 +755,42 @@ class Color { green(roundAndClamp(g)), blue(roundAndClamp(b)) {} + /** + * Creates a Color from HSV values. + * + * @param h The h value [0-180] + * @param s The s value [0-255] + * @param v The v value [0-255] + * @return The color + */ + static constexpr Color FromHSV(int h, int s, int v) { + if (s == 0) { + return {v / 255.0, v / 255.0, v / 255.0}; + } + + 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: + return Color(v / 255.0, t / 255.0, p / 255.0); + case 1: + return Color(q / 255.0, v / 255.0, p / 255.0); + case 2: + return Color(p / 255.0, v / 255.0, t / 255.0); + case 3: + return Color(p / 255.0, q / 255.0, v / 255.0); + case 4: + return Color(t / 255.0, p / 255.0, v / 255.0); + default: + return Color(v / 255.0, p / 255.0, q / 255.0); + } + } + double red = 0.0; double green = 0.0; double blue = 0.0; 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 b9b3dd9d40..a576991b62 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 @@ -48,6 +48,43 @@ public class Color { color.blue / 255.0); } + /** + * Creates a Color from HSV values. + * + * @param h The h value [0-180] + * @param s The s value [0-255] + * @param v The v value [0-255] + * @return The color + */ + @SuppressWarnings("ParameterName") + public static Color fromHSV(int h, int s, int v) { + if (s == 0) { + return new Color(v / 255.0, v / 255.0, v / 255.0); + } + + 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: + return new Color(v / 255.0, t / 255.0, p / 255.0); + case 1: + return new Color(q / 255.0, v / 255.0, p / 255.0); + case 2: + return new Color(p / 255.0, v / 255.0, t / 255.0); + case 3: + return new Color(p / 255.0, q / 255.0, v / 255.0); + case 4: + return new Color(t / 255.0, p / 255.0, v / 255.0); + default: + return new Color(v / 255.0, p / 255.0, q / 255.0); + } + } + @Override public boolean equals(Object other) { if (this == other) {