Skip to content

Commit

Permalink
refined led animation code
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu committed Dec 26, 2024
1 parent bc8df12 commit 41bf2f4
Show file tree
Hide file tree
Showing 4 changed files with 80 additions and 83 deletions.
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

package frc.robot;

import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import frc.robot.constants.RobotMode;
Expand Down Expand Up @@ -82,7 +83,7 @@ public void robotPeriodic() {
if (robotContainer.drive.hardwareFaultsDetected.getAsBoolean())
robotContainer
.ledStatusLight
.playAnimationPeriodically(new LEDAnimation.Breathe(255, 0, 0), 2)
.playAnimationPeriodically(new LEDAnimation.Breathe(new Color(255, 0, 0)), 2)
.until(robotContainer.drive.hardwareFaultsDetected.negate())
.withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
.schedule();
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
Expand Down Expand Up @@ -326,6 +327,8 @@ public void configureButtonBindings() {

public void configureLEDEffects() {
ledStatusLight.setDefaultCommand(ledStatusLight.showEnableDisableState());

driverXBox.a().onTrue(ledStatusLight.playAnimation(new LEDAnimation.Charging(Color.kOrange), 1));
}

/**
Expand Down
121 changes: 54 additions & 67 deletions src/main/java/frc/robot/subsystems/led/LEDAnimation.java
View file
Original file line number Diff line number Diff line change
@@ -1,123 +1,110 @@
package frc.robot.subsystems.led;

import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import org.ironmaple.utils.mathutils.MapleCommonMath;
import edu.wpi.first.wpilibj.util.Color;
import java.util.Arrays;

public sealed interface LEDAnimation {
/**
* Plays one frame the animation
*
* @param buffer the {@link AddressableLEDBuffer} to play on
* @param colors an array containing a series of {@link Color} that this function will modify
* @param t the time of the frame. 0~1 represents a full animation cycle
*/
void play(AddressableLEDBuffer buffer, double t);
void play(Color[] colors, double t);

static double oscillate(double t) {
return 0.5 + 0.5 * Math.sin(2 * Math.PI * t);
}

final class Breathe implements LEDAnimation {
private final int colorR, colorG, colorB;
private final Color color;

public Breathe(int colorR, int colorG, int colorB) {
this.colorR = colorR;
this.colorG = colorG;
this.colorB = colorB;
public Breathe(Color color) {
this.color = color;
}

@Override
public void play(AddressableLEDBuffer buffer, double t) {
final double brightness = 0.5 + 0.5 * Math.sin(t * Math.PI);
for (int i = 0; i < buffer.getLength(); i++)
buffer.setRGB(i, (int) (colorR * brightness), (int) (colorG * brightness), (int) (colorB * brightness));
public void play(Color[] colors, double t) {
final double brightness = oscillate(t);
for (int i = 0; i < colors.length; i++)
colors[i] = new Color(color.red * brightness, color.green * brightness, color.blue * brightness);
}
}

final class ShowColor implements LEDAnimation {
private final int colorR, colorG, colorB;
private final Color color;

public ShowColor(int colorR, int colorG, int colorB) {
this.colorR = colorR;
this.colorG = colorG;
this.colorB = colorB;
public ShowColor(Color color) {
this.color = color;
}

@Override
public void play(AddressableLEDBuffer buffer, double t) {
for (int i = 0; i < buffer.getLength(); i++) buffer.setRGB(i, colorR, colorG, colorB);
public void play(Color[] colors, double t) {
Arrays.fill(colors, color);
}
}

final class SlideBackAndForth extends Slide {
public SlideBackAndForth(int colorR, int colorG, int colorB, double slideLength) {
super(colorR, colorG, colorB, slideLength);
public SlideBackAndForth(Color color) {
super(color);
}

@Override
public void play(AddressableLEDBuffer buffer, double t) {
super.play(buffer, 0.5 + 0.5 * Math.sin(t * Math.PI));
public void play(Color[] colors, double t) {
super.play(colors, oscillate(t));
}
}

sealed class Slide implements LEDAnimation {
private final int colorR, colorG, colorB;
private final double slideLength;

public Slide(int colorR, int colorG, int colorB, double slideLength) {
this.colorR = colorR;
this.colorG = colorG;
this.colorB = colorB;
this.slideLength = slideLength;
private final Color color;

public Slide(Color color) {
this.color = color;
}

@Override
public void play(AddressableLEDBuffer buffer, double t) {
int halfLength = buffer.getLength() / 2;
double lowerEdge = MapleCommonMath.linearInterpretation(0, -slideLength, 1, 1, t);
double upperEdge = lowerEdge + slideLength;
/* strip is half the entire led */
for (int i = 0; i < halfLength; i++) {
int r = colorR, g = colorG, b = colorB;
if (i >= (int) (upperEdge * halfLength) || i <= (int) (lowerEdge * halfLength)) r = g = b = 0;
buffer.setRGB(halfLength + i, r, g, b);
buffer.setRGB(halfLength - i - 1, r, g, b);
public void play(Color[] colors, double t) {
if (t < 0.5) playSlideForward(colors, t * 2);
else playSlideBackwards(colors, (t - 0.5) * 2);
}

private void playSlideForward(Color[] colors, double t) {
for (int i = 0; i < colors.length; i++) {
colors[i] = i < t * colors.length ? color : new Color();
}
}

private void playSlideBackwards(Color[] colors, double t) {
for (int i = 0; i < colors.length; i++) {
colors[i] = i > t * colors.length ? color : new Color();
}
}
}

final class Charging implements LEDAnimation {
private final int colorR, colorG, colorB;
private final Color color;

public Charging(int colorR, int colorG, int colorB) {
this.colorR = colorR;
this.colorG = colorG;
this.colorB = colorB;
public Charging(Color color) {
this.color = color;
}

@Override
public void play(AddressableLEDBuffer buffer, double t) {
final int edge = (int) (t * buffer.getLength() / 2);
final double coolDownTime = 0.2;

t *= 1 + coolDownTime;
for (int i = 0; i < buffer.getLength() / 2; i++) {
int r = colorR, g = colorG, b = colorB;
if (t > 1) {
double brightness = (1 + coolDownTime - t) / coolDownTime;
r = (int) (r * brightness);
g = (int) (g * brightness);
b = (int) (b * brightness);
} else if (i > edge) r = g = b = 0;
buffer.setRGB(buffer.getLength() / 2 + 1 - i, r, g, b);
buffer.setRGB(buffer.getLength() / 2 - 1 + i, r, g, b);
public void play(Color[] colors, double t) {
for (int i = 0; i < colors.length; i++) {
colors[i] = i < t * colors.length
? new Color(color.red * t + 0.2, color.green * t + 0.2, color.blue * t + 0.2)
: new Color();
}
}
}

final class Rainbow implements LEDAnimation {
@Override
public void play(AddressableLEDBuffer buffer, double t) {
public void play(Color[] colors, double t) {
final int firstPixelHue = (int) (t * 180), v = 128;
for (var i = 0; i < buffer.getLength() / 2; i++) {
final int colorH = (firstPixelHue + (i * 180 / buffer.getLength())) % 180;
buffer.setHSV(buffer.getLength() / 2 + i, colorH, 255, v);
buffer.setHSV(buffer.getLength() / 2 - i, colorH, 255, v);
for (var i = 0; i < colors.length; i++) {
int colorH = (firstPixelHue + (i * 180 / colors.length)) % 180;
colors[i] = Color.fromHSV(colorH, 255, 128);
}
}
}
Expand Down
36 changes: 21 additions & 15 deletions src/main/java/frc/robot/subsystems/led/LEDStatusLight.java
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,26 @@
package frc.robot.subsystems.led;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import frc.robot.Robot;
import frc.robot.subsystems.MapleSubsystem;
import java.util.Arrays;
import org.littletonrobotics.junction.Logger;

public class LEDStatusLight extends MapleSubsystem {
private static final int DASHBOARD_DISPLAY_LENGTH = 32;
private static AddressableLED led = null;
private final Color[] ledColors;
private final AddressableLEDBuffer buffer;
private final AddressableLEDBuffer bufferForDashboard;

public LEDStatusLight(int port, int length) {
super("LED");
// make sure length is even
length = length / 2 * 2;
this.ledColors = new Color[length / 2];
Arrays.fill(ledColors, new Color());
this.buffer = new AddressableLEDBuffer(length);
this.bufferForDashboard = new AddressableLEDBuffer(DASHBOARD_DISPLAY_LENGTH);

if (led != null) led.close();
led = new AddressableLED(port);
Expand All @@ -25,25 +29,26 @@ public LEDStatusLight(int port, int length) {
led.start();
}

final String[] dashboardColors = new String[DASHBOARD_DISPLAY_LENGTH];

@Override
public void periodic(double dt, boolean enabled) {
for (int i = 0; i < DASHBOARD_DISPLAY_LENGTH; i++)
dashboardColors[i] = bufferForDashboard.getLED(i).toHexString();
for (int i = 0; i < ledColors.length; i++) {
int index1 = ledColors.length - 1 - i;
buffer.setLED(index1, ledColors[i]);
int index2 = ledColors.length + i;
buffer.setLED(index2, ledColors[i]);
}

led.setData(buffer);
if (!Robot.isReal()) Logger.recordOutput("Status Light", dashboardColors);
if (!Robot.isReal())
Logger.recordOutput(
"Status Light",
Arrays.stream(ledColors).map(Color::toHexString).toArray(String[]::new));
}

public Command playAnimation(LEDAnimation animation, double timeSeconds) {
Timer timer = new Timer();
timer.start();
return this.run(() -> {
double t = timer.get() / timeSeconds;
animation.play(buffer, t);
animation.play(bufferForDashboard, t);
})
return this.run(() -> animation.play(ledColors, timer.get() / timeSeconds))
.beforeStarting(timer::reset)
.withTimeout(timeSeconds)
.ignoringDisable(true);
Expand All @@ -60,9 +65,10 @@ public Command playAnimationPeriodically(LEDAnimation animation, double hz) {

public Command showEnableDisableState() {
return new ConditionalCommand(
playAnimation(new LEDAnimation.SlideBackAndForth(0, 200, 255, 0.8), 0.8)
playAnimation(new LEDAnimation.SlideBackAndForth(new Color(0, 200, 255)), 3.5)
.until(RobotState::isDisabled),
playAnimation(new LEDAnimation.Breathe(0, 200, 255), 2).until(RobotState::isEnabled),
playAnimation(new LEDAnimation.Breathe(new Color(0, 200, 255)), 3)
.until(RobotState::isEnabled),
RobotState::isEnabled)
.repeatedly()
.ignoringDisable(true);
Expand Down

0 comments on commit 41bf2f4

Please sign in to comment.