Skip to content

Commit 56be586

Browse files
Nummun14Nummun14
authored
LEDs (#16)
* implemented AddressableLed and most of CANdleLed * finished CANdleLEDStrip and added LEDCommands * added colorFLowCommand * made code more efficient * cleaned * added jDocs for the patterns, and fixed colorFlow inverted * added requirements * made CANdleLEDStrip use AddressableLEDStrip in sim * cleaned a bunch * cleaned stuff * added jDocs and improved simulation * improved setLED method * cleaned up colorFlow * implementing this was soooooooooooooooooo annoying * made rainbow command work with params, and fixed var name * removed extra new line * fixed commands, and removed shouldLoop (We don't need it Ezra) * cleaned code and jDocs * reverted Command stuff, will figure out a solution tommorow * finally fixed critical error!!!!!!!!!!!!!!!!!!!!!!!! 🎱 * fixed CANdle LED sim bug and added more jdocs * jDoc changes, and replace intervalSeconds with speed. (CANdle animations are so annoying! * Forgot to do this earlier * cleaned a bit, optimized, and stuff * fixed method name and cleaned a bit * simple is good * minor optimization thing * cleaning. jdocs and rainbow inverted * sim cleaning thing. Still working on annoying LED_STRIPS array bug * cleaned a bit, finished sim logic and added initiateAddressableLED * cleaning
1 parent dd22b53 commit 56be586

File tree

5 files changed

+669
-1
lines changed

5 files changed

+669
-1
lines changed

‎src/main/java/org/trigon/hardware/misc/leds/AddressableLEDStrip.java‎

Lines changed: 209 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,209 @@
1+
package org.trigon.hardware.misc.leds;
2+
3+
import com.ctre.phoenix.led.LarsonAnimation;
4+
import edu.wpi.first.wpilibj.AddressableLED;
5+
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
6+
import edu.wpi.first.wpilibj.Timer;
7+
import edu.wpi.first.wpilibj.util.Color;
8+
9+
import java.util.function.Supplier;
10+
11+
/**
12+
* A LED strip that is controlled by an AddressableLED.
13+
*/
14+
public class AddressableLEDStrip extends LEDStrip {
15+
private static AddressableLED LED;
16+
private static AddressableLEDBuffer LED_BUFFER;
17+
18+
private int lastBreatheLED;
19+
private double lastLEDAnimationChangeTime = 0;
20+
private double rainbowFirstPixelHue = 0;
21+
private boolean isLEDAnimationChanged = false;
22+
private int amountOfColorFlowLEDs = 0;
23+
24+
/**
25+
* Sets and configures the AddressableLED and AddressableLEDBuffer instances to be used for controlling the LED strip.
26+
* Must be set before using any LED strips. Should only be called once.
27+
*
28+
* @param port the port of the LED strip
29+
* @param totalAmountOfLEDs the total amount of LEDs in all LED strips
30+
*/
31+
public static void initiateAddressableLED(int port, int totalAmountOfLEDs) {
32+
if (LED_BUFFER == null)
33+
LED_BUFFER = new AddressableLEDBuffer(totalAmountOfLEDs);
34+
35+
if (LED == null) {
36+
LED = new AddressableLED(port);
37+
LED.setLength(totalAmountOfLEDs);
38+
LED.start();
39+
}
40+
}
41+
42+
/**
43+
* Constructs a new AddressableLEDStrip. Before any commands are sent to the LED strip, the setAddressableLED and setAddressableLEDBuffer methods must be called.
44+
*
45+
* @param inverted whether the LED strip is inverted
46+
* @param numberOfLEDs the amount of LEDs in the strip
47+
* @param indexOffset the offset of the first LED in the strip
48+
*/
49+
AddressableLEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) {
50+
super(inverted, numberOfLEDs, indexOffset);
51+
resetLEDSettings();
52+
}
53+
54+
@Override
55+
public void periodic() {
56+
currentAnimation.run();
57+
LED.setData(LED_BUFFER);
58+
}
59+
60+
@Override
61+
void clearLEDColors() {
62+
staticColor(Color.kBlack);
63+
}
64+
65+
@Override
66+
void blink(Color firstColor, double speed) {
67+
final double correctedSpeed = 1 - speed;
68+
final double currentTime = Timer.getFPGATimestamp();
69+
70+
if (currentTime - lastLEDAnimationChangeTime > correctedSpeed) {
71+
lastLEDAnimationChangeTime = currentTime;
72+
isLEDAnimationChanged = !isLEDAnimationChanged;
73+
}
74+
75+
if (isLEDAnimationChanged) {
76+
staticColor(firstColor);
77+
return;
78+
}
79+
clearLEDColors();
80+
}
81+
82+
@Override
83+
void staticColor(Color color) {
84+
setLEDColors(color, 0, numberOfLEDs - 1);
85+
}
86+
87+
@Override
88+
void breathe(Color color, int breathingLEDs, double speed, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
89+
clearLEDColors();
90+
final boolean correctedInverted = this.inverted != inverted;
91+
final double moveLEDTimeSeconds = 1 - speed;
92+
final double currentTime = Timer.getFPGATimestamp();
93+
94+
if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
95+
lastLEDAnimationChangeTime = currentTime;
96+
if (correctedInverted)
97+
lastBreatheLED--;
98+
else
99+
lastBreatheLED++;
100+
}
101+
102+
checkIfBreathingHasHitEnd(breathingLEDs, correctedInverted, bounceMode);
103+
setBreathingLEDs(color, breathingLEDs, bounceMode);
104+
}
105+
106+
@Override
107+
void colorFlow(Color color, double speed, boolean inverted) {
108+
clearLEDColors();
109+
final boolean correctedInverted = this.inverted != inverted;
110+
final double moveLEDTimeSeconds = 1 - speed;
111+
final double currentTime = Timer.getFPGATimestamp();
112+
113+
if (currentTime - lastLEDAnimationChangeTime > moveLEDTimeSeconds) {
114+
lastLEDAnimationChangeTime = currentTime;
115+
if (isLEDAnimationChanged)
116+
amountOfColorFlowLEDs--;
117+
else
118+
amountOfColorFlowLEDs++;
119+
}
120+
121+
checkIfColorFlowHasHitEnd();
122+
setLEDColors(color, correctedInverted ? numberOfLEDs - amountOfColorFlowLEDs - 1 : 0, correctedInverted ? numberOfLEDs - 1 : amountOfColorFlowLEDs);
123+
}
124+
125+
@Override
126+
void alternateColor(Color firstColor, Color secondColor) {
127+
for (int i = 0; i < numberOfLEDs; i++)
128+
LED_BUFFER.setLED(i + indexOffset, i % 2 == 0 ? firstColor : secondColor);
129+
}
130+
131+
@Override
132+
void rainbow(double brightness, double speed, boolean inverted) {
133+
final boolean correctedInverted = this.inverted != inverted;
134+
final int adjustedBrightness = (int) (brightness * 255);
135+
final int hueIncrement = (int) (speed * 8);
136+
137+
for (int led = 0; led < numberOfLEDs; led++) {
138+
final int hue = (int) (rainbowFirstPixelHue + (led * 180 / numberOfLEDs) % 180);
139+
LED_BUFFER.setHSV(led + indexOffset, hue, 255, adjustedBrightness);
140+
}
141+
142+
if (correctedInverted) {
143+
rainbowFirstPixelHue -= hueIncrement;
144+
if (rainbowFirstPixelHue < 0)
145+
rainbowFirstPixelHue += 180;
146+
return;
147+
}
148+
rainbowFirstPixelHue += hueIncrement;
149+
rainbowFirstPixelHue %= 180;
150+
}
151+
152+
@Override
153+
void sectionColor(Supplier<Color>[] colors) {
154+
final int amountOfSections = colors.length;
155+
final int LEDsPerSection = (int) Math.floor(numberOfLEDs / amountOfSections);
156+
157+
for (int i = 0; i < amountOfSections; i++)
158+
setLEDColors(
159+
inverted ? colors[amountOfSections - i - 1].get() : colors[i].get(),
160+
LEDsPerSection * i,
161+
i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDsPerSection * (i + 1) - 1
162+
);
163+
}
164+
165+
@Override
166+
void resetLEDSettings() {
167+
lastBreatheLED = indexOffset;
168+
lastLEDAnimationChangeTime = Timer.getFPGATimestamp();
169+
rainbowFirstPixelHue = 0;
170+
isLEDAnimationChanged = false;
171+
amountOfColorFlowLEDs = 0;
172+
}
173+
174+
private void checkIfBreathingHasHitEnd(int amountOfBreathingLEDs, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
175+
final int bounceModeAddition = switch (bounceMode) {
176+
case Back -> amountOfBreathingLEDs;
177+
case Center -> amountOfBreathingLEDs / 2;
178+
default -> 0;
179+
};
180+
181+
if (inverted ? (lastBreatheLED < indexOffset + bounceModeAddition) : (lastBreatheLED >= numberOfLEDs + indexOffset + bounceModeAddition))
182+
lastBreatheLED = inverted ? indexOffset + numberOfLEDs : indexOffset;
183+
}
184+
185+
private void setBreathingLEDs(Color color, int breathingLEDs, LarsonAnimation.BounceMode bounceMode) {
186+
for (int i = 0; i < breathingLEDs; i++) {
187+
if (lastBreatheLED - i >= indexOffset && lastBreatheLED - i < indexOffset + numberOfLEDs)
188+
LED_BUFFER.setLED(lastBreatheLED - i, color);
189+
190+
else if (lastBreatheLED - i < indexOffset + numberOfLEDs) {
191+
if (bounceMode.equals(LarsonAnimation.BounceMode.Back) || bounceMode.equals(LarsonAnimation.BounceMode.Center) && i > breathingLEDs / 2)
192+
return;
193+
LED_BUFFER.setLED(lastBreatheLED - i + numberOfLEDs, color);
194+
}
195+
}
196+
}
197+
198+
private void checkIfColorFlowHasHitEnd() {
199+
if (amountOfColorFlowLEDs >= numberOfLEDs || amountOfColorFlowLEDs < 0) {
200+
amountOfColorFlowLEDs = amountOfColorFlowLEDs < 0 ? amountOfColorFlowLEDs + 1 : amountOfColorFlowLEDs - 1;
201+
isLEDAnimationChanged = !isLEDAnimationChanged;
202+
}
203+
}
204+
205+
private void setLEDColors(Color color, int startIndex, int endIndex) {
206+
for (int i = 0; i <= endIndex - startIndex; i++)
207+
LED_BUFFER.setLED(startIndex + indexOffset + i, color);
208+
}
209+
}

‎src/main/java/org/trigon/hardware/misc/leds/CANdleLEDStrip.java‎

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
package org.trigon.hardware.misc.leds;
2+
3+
import com.ctre.phoenix.led.*;
4+
import edu.wpi.first.wpilibj.util.Color;
5+
import org.trigon.hardware.RobotHardwareStats;
6+
7+
import java.util.function.Supplier;
8+
9+
/**
10+
* A LED strip that is controlled by a CANdle, and uses AddressableLED for simulation.
11+
*/
12+
public class CANdleLEDStrip extends LEDStrip {
13+
private static CANdle CANDLE;
14+
private static int LAST_CREATED_LED_STRIP_ANIMATION_SLOT = 0;
15+
private final int animationSlot;
16+
17+
/**
18+
* Sets the CANdle instance to be used for controlling the LED strips. Must be set before using any LED strips. Should only be called once
19+
*
20+
* @param candle the CANdle instance to be used
21+
*/
22+
public static void setCANdle(CANdle candle) {
23+
if (CANDLE == null)
24+
CANDLE = candle;
25+
}
26+
27+
/**
28+
* Sets the total amount of LEDs in all LED strips for simulation.
29+
* Must be set before using any LED strips in simulation. Should only be called once.
30+
*
31+
* @param totalAmountOfLEDs the total amount of LEDs in all LED strips
32+
*/
33+
public static void setTotalAmountOfLEDs(int totalAmountOfLEDs) {
34+
if (RobotHardwareStats.isSimulation() || RobotHardwareStats.isReplay())
35+
AddressableLEDStrip.initiateAddressableLED(0, totalAmountOfLEDs);
36+
}
37+
38+
/**
39+
* Constructs a new CANdleLEDStrip. Before any commands are sent to the LED strip, the setCANdle method must be called.
40+
*
41+
* @param inverted whether the LED strip is inverted
42+
* @param numberOfLEDs the amount of LEDs in the strip
43+
* @param indexOffset the offset of the first LED in the strip
44+
*/
45+
CANdleLEDStrip(boolean inverted, int numberOfLEDs, int indexOffset) {
46+
super(inverted, numberOfLEDs, indexOffset);
47+
animationSlot = LAST_CREATED_LED_STRIP_ANIMATION_SLOT;
48+
LAST_CREATED_LED_STRIP_ANIMATION_SLOT++;
49+
}
50+
51+
@Override
52+
void clearLEDColors() {
53+
CANDLE.clearAnimation(animationSlot);
54+
}
55+
56+
@Override
57+
void blink(Color firstColor, double speed) {
58+
CANDLE.animate(
59+
new SingleFadeAnimation(
60+
(int) firstColor.red,
61+
(int) firstColor.green,
62+
(int) firstColor.blue,
63+
0,
64+
speed,
65+
this.numberOfLEDs,
66+
indexOffset
67+
),
68+
animationSlot
69+
);
70+
}
71+
72+
@Override
73+
void staticColor(Color color) {
74+
CANDLE.setLEDs((int) color.red, (int) color.green, (int) color.blue, 0, indexOffset, numberOfLEDs);
75+
}
76+
77+
@Override
78+
void breathe(Color color, int amountOfBreathingLEDs, double speed, boolean inverted, LarsonAnimation.BounceMode bounceMode) {
79+
CANDLE.animate(
80+
new LarsonAnimation(
81+
(int) color.red,
82+
(int) color.green,
83+
(int) color.blue,
84+
0,
85+
speed,
86+
this.numberOfLEDs,
87+
bounceMode,
88+
amountOfBreathingLEDs,
89+
indexOffset
90+
),
91+
animationSlot
92+
);
93+
}
94+
95+
@Override
96+
void alternateColor(Color firstColor, Color secondColor) {
97+
for (int i = 0; i < numberOfLEDs; i++)
98+
CANDLE.setLEDs(
99+
(int) (i % 2 == 0 ? firstColor.red : secondColor.red),
100+
(int) (i % 2 == 0 ? firstColor.green : secondColor.green),
101+
(int) (i % 2 == 0 ? firstColor.blue : secondColor.blue),
102+
0,
103+
i + indexOffset,
104+
1
105+
);
106+
}
107+
108+
@Override
109+
void colorFlow(Color color, double speed, boolean inverted) {
110+
final boolean correctedInverted = this.inverted != inverted;
111+
CANDLE.animate(
112+
new ColorFlowAnimation(
113+
(int) color.red,
114+
(int) color.green,
115+
(int) color.blue,
116+
0,
117+
speed,
118+
this.numberOfLEDs,
119+
correctedInverted ? ColorFlowAnimation.Direction.Backward : ColorFlowAnimation.Direction.Forward,
120+
indexOffset
121+
),
122+
animationSlot
123+
);
124+
}
125+
126+
@Override
127+
void rainbow(double brightness, double speed, boolean inverted) {
128+
final boolean correctedInverted = this.inverted != inverted;
129+
CANDLE.animate(
130+
new RainbowAnimation(
131+
brightness,
132+
speed,
133+
this.numberOfLEDs,
134+
correctedInverted,
135+
indexOffset
136+
),
137+
animationSlot
138+
);
139+
}
140+
141+
@Override
142+
void sectionColor(Supplier<Color>[] colors) {
143+
final int LEDSPerSection = (int) Math.floor(numberOfLEDs / colors.length);
144+
setSectionColor(colors.length, LEDSPerSection, colors);
145+
}
146+
147+
private void setSectionColor(int amountOfSections, int LEDSPerSection, Supplier<Color>[] colors) {
148+
for (int i = 0; i < amountOfSections; i++) {
149+
CANDLE.setLEDs(
150+
(int) (inverted ? colors[amountOfSections - i - 1].get().red : colors[i].get().red),
151+
(int) (inverted ? colors[amountOfSections - i - 1].get().green : colors[i].get().green),
152+
(int) (inverted ? colors[amountOfSections - i - 1].get().blue : colors[i].get().blue),
153+
0,
154+
LEDSPerSection * i + indexOffset,
155+
i == amountOfSections - 1 ? numberOfLEDs - 1 : LEDSPerSection * (i + 1) - 1
156+
);
157+
}
158+
}
159+
}

0 commit comments

Comments
 (0)