diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..410e21f --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,56 @@ +{"keyboardJoysticks": [{ + "axisConfig": [{ + "decKey": 546, + "incKey": 549 + }, { + "decKey": 568, + "incKey": 564 + }, { + "decKey": 550, + "decayRate": 0, + "incKey": 563, + "keyRate": 0.01 + }], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [571, 569, 548, 567], + "povConfig": [{ + "keyDown": 614, + "keyDownLeft": 613, + "keyDownRight": 615, + "keyLeft": 616, + "keyRight": 618, + "keyUp": 620, + "keyUpLeft": 619, + "keyUpRight": 621 + }], + "povCount": 1 +}, { + "axisConfig": [{ + "decKey": 555, + "incKey": 557 + }, { + "decKey": 554, + "incKey": 556 + }], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [558, 597, 599, 600], + "povCount": 0 +}, { + "axisConfig": [{ + "decKey": 513, + "incKey": 514 + }, { + "decKey": 515, + "incKey": 516 + }], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [521, 519, 517, 522, 520, 518], + "povCount": 0 +}, { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 +}]} diff --git a/src/main/java/first/robot/Robot.java b/src/main/java/first/robot/Robot.java index 6ec426b..f82b931 100644 --- a/src/main/java/first/robot/Robot.java +++ b/src/main/java/first/robot/Robot.java @@ -10,8 +10,13 @@ import org.wpilib.epilogue.Epilogue; import org.wpilib.epilogue.EpilogueTelemetry; import org.wpilib.epilogue.Logged; import org.wpilib.framework.OpModeRobot; +import org.wpilib.tunable.TunableRegistry; +import first.robot.constants.LEDConstants; import first.robot.constants.OIConstants; +import first.robot.leds.AnimationManager; +import first.robot.leds.PhysicalLEDStrip; +import first.robot.leds.animations.FluidRainbow; import first.robot.mechanisms.Drivetrain; import first.robot.mechanisms.Pusher; import first.robot.mechanisms.Shooter; @@ -22,6 +27,12 @@ public class Robot extends OpModeRobot { public final Pusher pusher; public final Shooter shooter; public final CommandGamepad gamepad; + + private final PhysicalLEDStrip strip; + + private AnimationManager manager; + + private FluidRainbow rainbow; public Robot() { drive = new Drivetrain(true); @@ -32,6 +43,19 @@ public class Robot extends OpModeRobot { drive.setDefaultCommand(drive.idle()); pusher.setDefaultCommand(pusher.idle()); shooter.setDefaultCommand(shooter.idle()); + + strip = new PhysicalLEDStrip(LEDConstants.kLEDPWM, LEDConstants.kNumLEDs); + + manager = new AnimationManager(new PhysicalLEDStrip[] {strip}); + + rainbow = new FluidRainbow(strip, LEDConstants.kRainbowUpdateRate); + rainbow.start(); + + manager.registerAnimationForUpdate(rainbow); + + manager.start(LEDConstants.kAnimationManagerUpdateRate); + + TunableRegistry.publish("Drive Tunables", drive); } @Override diff --git a/src/main/java/first/robot/constants/LEDConstants.java b/src/main/java/first/robot/constants/LEDConstants.java new file mode 100644 index 0000000..fc5dd16 --- /dev/null +++ b/src/main/java/first/robot/constants/LEDConstants.java @@ -0,0 +1,13 @@ +package first.robot.constants; + +public class LEDConstants { + public static final int kLEDPWM = 8; + + public static final int kNumLEDs = 19 * 2 + 12 * 2; + + public static final double kRainbowUpdateRate = .02; + public static final double kBreathingUpdateRate = .03; + public static final double kBreathingMinPercentage = 25; + public static final double kBreathingMaxPercentage = 100; + public static final double kAnimationManagerUpdateRate = .01; +} diff --git a/src/main/java/first/robot/leds/Animation.java b/src/main/java/first/robot/leds/Animation.java new file mode 100644 index 0000000..fb9a132 --- /dev/null +++ b/src/main/java/first/robot/leds/Animation.java @@ -0,0 +1,44 @@ +package first.robot.leds; + +import org.wpilib.system.Timer; + +public abstract class Animation { + private boolean enabled; + private double timeDelay; + private Timer timer; + + protected LEDStrip strip; + + public Animation(LEDStrip strip, double timeDelay) { + this.strip = strip; + + timer = new Timer(); + this.timeDelay = timeDelay; + enabled = false; + } + + public boolean isEnabled() { + return enabled; + } + + public void start() { + timer.reset(); + timer.start(); + enabled = true; + } + + public void stop() { + timer.stop(); + enabled = false; + } + + public abstract boolean update(); + + protected boolean shouldUpdate() { + return enabled && timer.get() >= timeDelay; + } + + protected void resetTiming() { + timer.reset(); + } +} diff --git a/src/main/java/first/robot/leds/AnimationManager.java b/src/main/java/first/robot/leds/AnimationManager.java new file mode 100644 index 0000000..ed374a9 --- /dev/null +++ b/src/main/java/first/robot/leds/AnimationManager.java @@ -0,0 +1,85 @@ +package first.robot.leds; + +import java.util.ArrayList; + +import org.wpilib.system.Notifier; + +public class AnimationManager { + + private Notifier internalNotifier; + private ArrayList animations; + private PhysicalLEDStrip[] ledStrips; + private boolean[] updateLEDStrip; + + public AnimationManager(PhysicalLEDStrip[] ledStrips) { + this.ledStrips = ledStrips; + updateLEDStrip = new boolean[ledStrips.length]; + + for(int i = 0; i < updateLEDStrip.length; i++) { + updateLEDStrip[i] = false; + } + + internalNotifier = new Notifier(() -> { + synchronized(animations) { + for(Animation animation : animations) { + boolean shouldUpdate = animation.update(); + + if(shouldUpdate) { + int stripIndex = findStripIndex(animation.strip.getPhysicalStrip()); + + if(stripIndex != -1) { + updateLEDStrip[stripIndex] = true; + } + } + } + + for(int i = 0; i < updateLEDStrip.length; i++) { + if(updateLEDStrip[i]) { + ledStrips[i].show(); + } + + updateLEDStrip[i] = false; + } + } + }); + + animations = new ArrayList(); + } + + public void registerAnimationForUpdate(Animation animation) { + synchronized(animations) { + animations.add(animation); + } + } + + public void unregisterAnimation(Animation animation) { + synchronized(animations) { + animations.remove(animation); + } + } + + public void unregisterAnimation(int index) { + synchronized(animations) { + animations.remove(index); + } + } + + public void start(double periodSeconds) { + internalNotifier.startPeriodic(periodSeconds); + } + + public void stop() { + internalNotifier.stop(); + } + + private int findStripIndex(LEDStrip strip) { + for(int i = 0; i < ledStrips.length; i++) { + if(ledStrips[i] == strip) { + return i; + } + } + + return -1; + } + +} diff --git a/src/main/java/first/robot/leds/LEDStrip.java b/src/main/java/first/robot/leds/LEDStrip.java new file mode 100644 index 0000000..a3bf3ce --- /dev/null +++ b/src/main/java/first/robot/leds/LEDStrip.java @@ -0,0 +1,84 @@ +package first.robot.leds; + +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +/** + * An interface designed to create a common interface + * for current and future LED types. This interface can be implemented + * to wrap additional types of LEDs + */ +public interface LEDStrip { + /** + * Retrieves the number of LEDs in the strip + * + * @return An integer representing the number of LEDs in the strip + */ + public int getNumLEDs(); + + /** + * Directs the underlying physical strip to show + * the current internal buffer + */ + public void show(); + + /** + * Returns a reference to the physical LED strip + * + * @return The physical LED strip reference + */ + public LEDStrip getPhysicalStrip(); + + /** + * Sets a particular pixel using a Color object + * + * @param pixel The pixel index to set + * @param color The Color object used to specify the desired color + */ + public void setRGBColor(int pixel, Color color); + + /** + * Sets a particular pixel using a Color8Bit object + * + * @param pixel The pixel index to set + * @param color The Color8Bit object used to specify the desired color + */ + public void setRGBColor(int pixel, Color8Bit color); + + /** + * Sets a particular pixel using 8 bit r, g, and b values + * + * @param pixel The pixel index to set + * @param r The Red value of the RGB color (0 - 255) + * @param g The Green value of the RGB color (0 - 255) + * @param b The Blue value of the RGB color (0 - 255) + */ + public void setRGBColor(int pixel, int r, int g, int b); + + /** + * Sets a particular pixel using 8 bit h, s, v values + * + * @param pixel The pixel index to set + * @param h The Hue value of the HSV color (0 - 180) + * @param s The Saturation value of the HSV color (0 - 255) + * @param v The Value value of the HSV color (0 - 255) + */ + public void setHSVColor(int pixel, int h, int s, int v); + + /** + * Returns the color at a specified pixel index using a Color object + * + * @param pixel The pixel index to return the current color from + * @return The Color object representation of the specified pixel index + */ + public Color getColor(int pixel); + + /** + * Returns the color at the specified pixel index using a Color8Bit object + * + * @param pixel The pixel index to return the current color from + * @return The Color8Bit object representation of the specified pixel index + */ + public Color8Bit getColor8Bit(int pixel); + +} diff --git a/src/main/java/first/robot/leds/LEDStrip2DWrapper.java b/src/main/java/first/robot/leds/LEDStrip2DWrapper.java new file mode 100644 index 0000000..2190168 --- /dev/null +++ b/src/main/java/first/robot/leds/LEDStrip2DWrapper.java @@ -0,0 +1,104 @@ +package first.robot.leds; + +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +public class LEDStrip2DWrapper implements LEDStrip { + private LEDStrip strip; + private int width; + private int height; + + public LEDStrip2DWrapper(LEDStrip strip, int width, int height) { + this.strip = strip; + this.width = width; + this.height = height; + } + + @Override + public int getNumLEDs() { + return width * height; + } + + public int getWidth() { + return width; + } + + public int getHeight() { + return height; + } + + @Override + public void show() { + strip.show(); + + } + + @Override + public LEDStrip getPhysicalStrip() { + return strip.getPhysicalStrip(); + } + + public void setRGBColor(int x, int y, Color color) { + setRGBColor(translateXY(x, y), color); + } + + public void setRGBColor(int x, int y, Color8Bit color) { + setRGBColor(translateXY(x, y), color); + } + + public void setRGBColor(int x, int y, int r, int g, int b) { + setRGBColor(translateXY(x, y), r, g, b); + } + + public void setHSVColor(int x, int y, int h, int s, int v) { + setHSVColor(translateXY(x, y), h, s, v); + } + + @Override + public void setRGBColor(int pixel, Color color) { + strip.setRGBColor(pixel, color); + + } + + @Override + public void setRGBColor(int pixel, Color8Bit color) { + strip.setRGBColor(pixel, color); + } + + @Override + public void setRGBColor(int pixel, int r, int g, int b) { + strip.setRGBColor(pixel, r, g, b); + + } + + @Override + public void setHSVColor(int pixel, int h, int s, int v) { + strip.setHSVColor(pixel, h, s, v); + + } + + public Color getColor(int x, int y) { + return getColor(translateXY(x, y)); + } + + public Color8Bit getColor8Bit(int x, int y) { + return getColor8Bit(translateXY(x, y)); + } + + @Override + public Color getColor(int pixel) { + return strip.getColor(pixel); + } + + @Override + public Color8Bit getColor8Bit(int pixel) { + return strip.getColor8Bit(pixel); + } + + protected int translateXY(int x, int y) { + //TODO This should throw out of range when appropriate + //TODO Serpentine mode? + return y * width + x; + } + +} diff --git a/src/main/java/first/robot/leds/LogicalLEDStrip.java b/src/main/java/first/robot/leds/LogicalLEDStrip.java new file mode 100644 index 0000000..fff8be1 --- /dev/null +++ b/src/main/java/first/robot/leds/LogicalLEDStrip.java @@ -0,0 +1,67 @@ +package first.robot.leds; + +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +public class LogicalLEDStrip implements LEDStrip{ + + private LEDStrip strip; + private int startPixel; + private int endPixel; + + public LogicalLEDStrip(LEDStrip strip, int startPixel, int endPixel) { + this.strip = strip; + this.startPixel = startPixel; + this.endPixel = endPixel; + } + + @Override + public int getNumLEDs() { + return endPixel - startPixel; + } + + @Override + public void show() { + strip.show(); + } + + @Override + public void setRGBColor(int pixel, Color color) { + strip.setRGBColor(rescaleIndex(pixel), color); + } + + @Override + public void setRGBColor(int pixel, Color8Bit color) { + strip.setRGBColor(rescaleIndex(pixel), color); + } + + @Override + public void setRGBColor(int pixel, int r, int g, int b) { + strip.setRGBColor(rescaleIndex(pixel), r, g, b); + } + + @Override + public void setHSVColor(int pixel, int h, int s, int v) { + strip.setHSVColor(rescaleIndex(pixel), h, s, v); + } + + @Override + public Color getColor(int pixel) { + return strip.getColor(rescaleIndex(pixel)); + } + + @Override + public Color8Bit getColor8Bit(int pixel) { + return strip.getColor8Bit(rescaleIndex(pixel)); + } + + //TODO This should be hardened to throw an IndexOutOfBound Exception + private int rescaleIndex(int pixel) { + return startPixel + pixel; + } + + @Override + public LEDStrip getPhysicalStrip() { + return strip.getPhysicalStrip(); + } +} diff --git a/src/main/java/first/robot/leds/PhysicalLEDStrip.java b/src/main/java/first/robot/leds/PhysicalLEDStrip.java new file mode 100644 index 0000000..430d9b0 --- /dev/null +++ b/src/main/java/first/robot/leds/PhysicalLEDStrip.java @@ -0,0 +1,64 @@ +package first.robot.leds; + +import org.wpilib.hardware.led.AddressableLED; +import org.wpilib.hardware.led.AddressableLEDBuffer; +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +public class PhysicalLEDStrip implements LEDStrip { + private AddressableLED led; + private AddressableLEDBuffer buffer; + + public PhysicalLEDStrip(int portNum, int numLEDs) { + led = new AddressableLED(portNum); + led.setLength(numLEDs); + + buffer = new AddressableLEDBuffer(numLEDs); + led.setData(buffer); + } + + @Override + public void show() { + led.setData(buffer); + } + + @Override + public int getNumLEDs() { + return buffer.getLength(); + } + + @Override + public void setRGBColor(int pixel, Color color) { + buffer.setLED(pixel, color); + } + + @Override + public void setRGBColor(int pixel, Color8Bit color) { + buffer.setLED(pixel, color); + } + + @Override + public void setRGBColor(int pixel, int r, int g, int b) { + buffer.setLED(pixel, new Color8Bit(r, g, b)); + } + + @Override + public void setHSVColor(int pixel, int h, int s, int v) { + buffer.setHSV(pixel, h, s, v); + } + + @Override + public Color getColor(int pixel) { + return buffer.getLED(pixel); + } + + @Override + public Color8Bit getColor8Bit(int pixel) { + return new Color8Bit(getColor(pixel)); + } + + @Override + public LEDStrip getPhysicalStrip() { + return this; + } +} \ No newline at end of file diff --git a/src/main/java/first/robot/leds/animations/Alternate.java b/src/main/java/first/robot/leds/animations/Alternate.java new file mode 100644 index 0000000..72249fe --- /dev/null +++ b/src/main/java/first/robot/leds/animations/Alternate.java @@ -0,0 +1,46 @@ +package first.robot.leds.animations; + +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class Alternate extends Animation { + private Color8Bit[] colors; + private int alternateSize; + private int alternateInt; + + public Alternate(LEDStrip strip, double timeDelay, Color8Bit[] colors, int alternateSize) { + super(strip, timeDelay); + this.colors = colors; + this.alternateSize = alternateSize; + alternateInt = 0; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + for(int i = 0; i < strip.getNumLEDs(); i += alternateSize) { + Color8Bit targetColor = colors[((i / alternateSize) + alternateInt) % colors.length]; + + for(int j = i; j < i + alternateSize; j++) { + strip.setRGBColor(j, targetColor); + } + } + + alternateInt++; + + if(alternateInt >= colors.length) { + alternateInt = 0; + } + + resetTiming(); + + return true; + } + + return false; + } + + +} diff --git a/src/main/java/first/robot/leds/animations/Breathing.java b/src/main/java/first/robot/leds/animations/Breathing.java new file mode 100644 index 0000000..6464eb7 --- /dev/null +++ b/src/main/java/first/robot/leds/animations/Breathing.java @@ -0,0 +1,57 @@ +package first.robot.leds.animations; + +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class Breathing extends Animation { + private Color8Bit originalColor; + private double minPercentage; + private double maxPercentage; + private double currentPercentage; + private double currentAdditive; + + public Breathing(LEDStrip strip, double timeDelay, Color8Bit originalColor, double minPercentage, double maxPercentage) { + super(strip, timeDelay); + + this.originalColor = originalColor; + this.minPercentage = minPercentage; + this.maxPercentage = maxPercentage; + currentPercentage = minPercentage; + currentAdditive = 1; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + Color8Bit nextColor = new Color8Bit( + (int)(originalColor.red * currentPercentage / 100.0), + (int)(originalColor.green * currentPercentage / 100.0), + (int)(originalColor.blue * currentPercentage / 100.0)); + + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setRGBColor(i, nextColor); + } + + currentPercentage += currentAdditive; + + if(currentAdditive > 0) { + if(currentPercentage > maxPercentage) { + currentAdditive *= -1; + } + } else { + if(currentPercentage < minPercentage) { + currentAdditive *= -1; + } + } + + resetTiming(); + + return true; + } + + return false; + } + +} diff --git a/src/main/java/first/robot/leds/animations/Collision.java b/src/main/java/first/robot/leds/animations/Collision.java new file mode 100644 index 0000000..ae309fd --- /dev/null +++ b/src/main/java/first/robot/leds/animations/Collision.java @@ -0,0 +1,57 @@ +package first.robot.leds.animations; + +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class Collision extends Animation { + private Color8Bit color1; + private Color8Bit color2; + + private int collisionInt; + + public Collision(LEDStrip strip, double timeDelay, Color8Bit color1, Color8Bit color2) { + super(strip, timeDelay); + + this.color1 = color1; + this.color2 = color2; + + collisionInt = 0; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + if(collisionInt == 0) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setRGBColor(i, 0, 0, 0); + } + + strip.setRGBColor(0, color1); + strip.setRGBColor(strip.getNumLEDs() - 1, color2); + + collisionInt++; + + resetTiming(); + return true; + } else if(collisionInt < strip.getNumLEDs()) { + strip.setRGBColor(collisionInt, color1); + strip.setRGBColor(strip.getNumLEDs() - collisionInt - 1, color2); + + collisionInt++; + + resetTiming(); + return true; + } else { + collisionInt = 0; + + //Don't reset timing here. + //Forces an immediate update to clear the LEDs + return false; + } + } + return false; + } + +} diff --git a/src/main/java/first/robot/leds/animations/ColorRandomizer.java b/src/main/java/first/robot/leds/animations/ColorRandomizer.java new file mode 100644 index 0000000..10c58b6 --- /dev/null +++ b/src/main/java/first/robot/leds/animations/ColorRandomizer.java @@ -0,0 +1,116 @@ +package first.robot.leds.animations; + +import java.util.Random; + +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class ColorRandomizer extends Animation { + private Color[] colors; + private Color[] fadingValues; + private Random random; + private int colorRandomizerInt; + private int fadeScale; + private boolean shouldFade; + private boolean isFading; + + public ColorRandomizer(LEDStrip strip, double timeDelay, Color8Bit[] colors, + boolean shouldFade, int fadeScale) { + super(strip, timeDelay); + + this.colors = new Color[colors.length]; + + for(int i = 0; i < colors.length; i++) { + this.colors[i] = new Color(colors[i]); + } + + random = new Random(); + + fadingValues = new Color[colors.length]; + + this.shouldFade = shouldFade; + this.fadeScale = fadeScale; + colorRandomizerInt = 0; + isFading = false; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + if(shouldFade && isFading) { + if(colorRandomizerInt == 0) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + Color currentColor = strip.getColor(i); + + fadingValues[i] = new Color( + currentColor.red / fadeScale, + currentColor.green / fadeScale, + currentColor.blue / fadeScale + ); + } + + colorRandomizerInt++; + } else if(colorRandomizerInt > 0 && colorRandomizerInt < fadeScale + 1) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + Color currentColor = strip.getColor(i); + double nextRed = 0; + double nextGreen = 0; + double nextBlue = 0; + + if(currentColor.red == 0) { + nextRed = 0; + } else if(currentColor.red - fadingValues[i].red < 0) { + nextRed = 0; + } else { + nextRed = currentColor.red - fadingValues[i].red; + } + + if(currentColor.green == 0) { + nextGreen = 0; + } else if(currentColor.green - fadingValues[i].green < 0) { + nextGreen = 0; + } else { + nextGreen = currentColor.green - fadingValues[i].green; + } + + if(currentColor.blue == 0) { + nextBlue = 0; + } else if(currentColor.blue - fadingValues[i].blue < 0) { + nextBlue = 0; + } else { + nextBlue = currentColor.blue - fadingValues[i].blue; + } + + strip.setRGBColor(i, new Color(nextRed, nextGreen, nextBlue)); + } + + colorRandomizerInt++; + + resetTiming(); + + return true; + } else { + colorRandomizerInt = 0; + isFading = false; + } + } else { + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setRGBColor(i, colors[random.nextInt(colors.length)]); + } + + if(shouldFade) { + isFading = true; + } + + resetTiming(); + return true; + } + } + + return false; + } + +} \ No newline at end of file diff --git a/src/main/java/first/robot/leds/animations/CycleLight.java b/src/main/java/first/robot/leds/animations/CycleLight.java new file mode 100644 index 0000000..ffc0207 --- /dev/null +++ b/src/main/java/first/robot/leds/animations/CycleLight.java @@ -0,0 +1,58 @@ +package first.robot.leds.animations; + +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class CycleLight extends Animation { + private Color8Bit[] colors; + private int cycleLightInt; + private int currentColorIndex; + + public CycleLight(LEDStrip strip, double timeDelay, Color8Bit[] colors) { + super(strip, timeDelay); + + this.colors = colors; + + cycleLightInt = 0; + currentColorIndex = 0; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + if(cycleLightInt == 0) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setRGBColor(i, 0, 0, 0); + } + + strip.setRGBColor(cycleLightInt, colors[currentColorIndex]); + cycleLightInt++; + + resetTiming(); + + return true; + } else if(cycleLightInt < strip.getNumLEDs()) { + strip.setRGBColor(cycleLightInt, colors[currentColorIndex]); + cycleLightInt++; + + resetTiming(); + + return true; + } else { + cycleLightInt = 0; + + currentColorIndex++; + + if(currentColorIndex >= colors.length) { + currentColorIndex = 0; + } + } + } + + return false; + } + + +} diff --git a/src/main/java/first/robot/leds/animations/FluidRainbow.java b/src/main/java/first/robot/leds/animations/FluidRainbow.java new file mode 100644 index 0000000..a147d75 --- /dev/null +++ b/src/main/java/first/robot/leds/animations/FluidRainbow.java @@ -0,0 +1,31 @@ +package first.robot.leds.animations; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class FluidRainbow extends Animation { + + private int hsvShift; + + public FluidRainbow(LEDStrip strip, double timeDelay) { + super(strip, timeDelay); + } + + @Override + public boolean update() { + if(shouldUpdate()) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setHSVColor(i, i + (hsvShift % 180), 255, 255); + } + + hsvShift++; + + resetTiming(); + + return true; + } + + return false; + } + +} diff --git a/src/main/java/first/robot/leds/animations/IndicatorLight.java b/src/main/java/first/robot/leds/animations/IndicatorLight.java new file mode 100644 index 0000000..30b93d4 --- /dev/null +++ b/src/main/java/first/robot/leds/animations/IndicatorLight.java @@ -0,0 +1,35 @@ +package first.robot.leds.animations; + +import java.util.function.BooleanSupplier; + +import org.wpilib.util.Color; +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class IndicatorLight extends Animation { + private BooleanSupplier indicatorSupplier; + private Color8Bit color; + + public IndicatorLight(LEDStrip strip, double timeDelay, BooleanSupplier indicatorSupplier, Color8Bit color) { + super(strip, timeDelay); + + this.indicatorSupplier = indicatorSupplier; + this.color = color; + } + + @Override + public boolean update() { + if(shouldUpdate()) { + for(int i = 0; i < strip.getNumLEDs(); i++) { + strip.setRGBColor(i, indicatorSupplier.getAsBoolean() ? new Color(color) : Color.BLACK); + } + + resetTiming(); + + return true; + } + return false; + } +} diff --git a/src/main/java/first/robot/leds/animations/InputMeter.java b/src/main/java/first/robot/leds/animations/InputMeter.java new file mode 100644 index 0000000..1227f3b --- /dev/null +++ b/src/main/java/first/robot/leds/animations/InputMeter.java @@ -0,0 +1,71 @@ +package first.robot.leds.animations; + +import java.util.function.DoubleSupplier; + +import org.wpilib.util.Color; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip; + +public class InputMeter extends Animation { + private DoubleSupplier inputSupplier; + private double minValue; + private double maxValue; + + private int yellowStartPixel; + private int redStartPixel; + + public InputMeter(LEDStrip strip, double timeDelay, DoubleSupplier inputSupplier, + double minValue, double maxValue) { + super(strip, timeDelay); + + this.inputSupplier = inputSupplier; + this.minValue = minValue; + this.maxValue = maxValue; + + redStartPixel = (int)(strip.getNumLEDs() - Math.ceil(strip.getNumLEDs() * .1)); + yellowStartPixel = (int)(strip.getNumLEDs() - Math.ceil(strip.getNumLEDs() * .1) - Math.ceil(strip.getNumLEDs() * .2)); + } + + @Override + public boolean update() { + if(shouldUpdate()) { + int remappedInput = (int)remapInput(inputSupplier.getAsDouble(), minValue, maxValue, 0, strip.getNumLEDs()-1); + + if(remappedInput > yellowStartPixel) { + for(int i = 0; i < yellowStartPixel; i++) { + strip.setRGBColor(i, Color.GREEN); + } + + if(remappedInput > redStartPixel) { + for(int i = yellowStartPixel; i < redStartPixel; i++) { + strip.setRGBColor(i, Color.YELLOW); + } + + for(int i = redStartPixel; i < remappedInput; i++) { + strip.setRGBColor(i, Color.RED); + } + } else { + for(int i = yellowStartPixel; i < remappedInput; i++) { + strip.setRGBColor(i, Color.YELLOW); + } + } + } else { + for(int i = 0; i < remappedInput; i++) { + strip.setRGBColor(i, Color.GREEN); + } + } + + resetTiming(); + + return true; + } + + return false; + } + + private double remapInput(double x, double inMin, double inMax, double outMin, double outMax) { + return (x - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; + } + +} \ No newline at end of file diff --git a/src/main/java/first/robot/leds/animations2D/Ricochet2D.java b/src/main/java/first/robot/leds/animations2D/Ricochet2D.java new file mode 100644 index 0000000..a23b635 --- /dev/null +++ b/src/main/java/first/robot/leds/animations2D/Ricochet2D.java @@ -0,0 +1,56 @@ +package first.robot.leds.animations2D; + +import java.util.Random; + +import org.wpilib.util.Color8Bit; + +import first.robot.leds.Animation; +import first.robot.leds.LEDStrip2DWrapper; + +public class Ricochet2D extends Animation { + private RicochetHelper[] helpers; + private Random random; + private LEDStrip2DWrapper wrapper; + + public Ricochet2D(LEDStrip2DWrapper wrapper, double timeDelay, + int numBalls, Color8Bit[] colors) { + + super(wrapper, timeDelay); + this.wrapper = wrapper; + + helpers = new RicochetHelper[numBalls]; + random = new Random(); + + for(int i = 0; i < numBalls; i++) { + helpers[i] = new RicochetHelper(random.nextInt(wrapper.getWidth()), + random.nextInt(wrapper.getHeight()), wrapper.getWidth(), + wrapper.getHeight(), colors[random.nextInt(colors.length)]); + } + } + + @Override + public boolean update() { + if(shouldUpdate()) { + for(int i = 0; i < wrapper.getWidth(); i++) { + for(int j = 0; j < wrapper.getHeight(); j++) { + wrapper.setRGBColor(i, j, 0, 0, 0); + } + } + + for(int i = 0; i < helpers.length; i++) { + helpers[i].updatePosition(); + + wrapper.setRGBColor(helpers[i].getCurrentXPosition(), + helpers[i].getCurrentYPosition(), + helpers[i].getCurrentColor()); + } + + resetTiming(); + + return true; + } + + return false; + } + +} diff --git a/src/main/java/first/robot/leds/animations2D/RicochetHelper.java b/src/main/java/first/robot/leds/animations2D/RicochetHelper.java new file mode 100644 index 0000000..ed6ec15 --- /dev/null +++ b/src/main/java/first/robot/leds/animations2D/RicochetHelper.java @@ -0,0 +1,68 @@ +package first.robot.leds.animations2D; + +import java.util.Random; + +import org.wpilib.util.Color8Bit; + +public class RicochetHelper { + private int currentX; + private int currentY; + private int width; + private int height; + private int xDirection; + private int yDirection; + private Color8Bit color; + private Random random; + + public RicochetHelper(int currentX, int currentY, int width, + int height, Color8Bit color) { + this.currentX = currentX; + this.currentY = currentY; + this.width = width; + this.height = height; + this.color = color; + + random = new Random(); + + if(currentX == 0) { + xDirection = 1; + } else if(currentX == width - 1) { + xDirection = -1; + } else { + xDirection = random.nextBoolean() ? -1 : 1; + } + + if(currentY == 0) { + yDirection = 1; + } else if(currentY == height - 1) { + yDirection = -1; + } else { + yDirection = random.nextBoolean() ? -1 : 1; + } + } + + public int getCurrentXPosition() { + return currentX; + } + + public int getCurrentYPosition() { + return currentY; + } + + public Color8Bit getCurrentColor() { + return color; + } + + public void updatePosition() { + if(currentX + xDirection >= width || currentX + xDirection < 0) { + xDirection *= -1; + } + + if(currentY + yDirection >= height || currentY + yDirection < 0) { + yDirection *= -1; + } + + currentX += xDirection; + currentY += yDirection; + } +} diff --git a/src/main/java/first/robot/mechanisms/Drivetrain.java b/src/main/java/first/robot/mechanisms/Drivetrain.java index ec36aed..ef32e6f 100644 --- a/src/main/java/first/robot/mechanisms/Drivetrain.java +++ b/src/main/java/first/robot/mechanisms/Drivetrain.java @@ -6,11 +6,13 @@ import org.wpilib.command3.button.CommandGamepad; import org.wpilib.drive.DifferentialDrive; import org.wpilib.drivers.motor.VictorSP; import org.wpilib.epilogue.Logged; +import org.wpilib.tunable.ComplexTunable; +import org.wpilib.tunable.TunableTable; import first.robot.constants.DrivetrainConstants; @Logged -public class Drivetrain implements Mechanism { +public class Drivetrain implements Mechanism,ComplexTunable { private VictorSP frontLeft; private VictorSP rearLeft; private VictorSP frontRight; @@ -72,4 +74,9 @@ public class Drivetrain implements Mechanism { }) .named("Drivetrain.Stop"); } + + @Override + public void publishTunable(TunableTable table) { + table.publishBoolean("Slow Mode", () -> slowMode, (b) -> slowMode = b); + } } diff --git a/src/main/java/first/robot/mechanisms/Shooter.java b/src/main/java/first/robot/mechanisms/Shooter.java index 051c761..d8303cf 100644 --- a/src/main/java/first/robot/mechanisms/Shooter.java +++ b/src/main/java/first/robot/mechanisms/Shooter.java @@ -5,6 +5,7 @@ import java.util.function.DoubleSupplier; import org.wpilib.command3.Command; import org.wpilib.command3.Mechanism; import org.wpilib.epilogue.Logged; +import org.wpilib.math.controller.PIDController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkLowLevel.MotorType;