presets = new HashMap<>();
- private Spark controller;
-
- public LEDController(int port) {
- controller = new Spark(port);
- }
-
- public void switchPreset(String name) {
- LEDPreset targetPreset = presets.get(name);
- if (targetPreset != null) {
- controller.set(targetPreset.value());
- } else {
- System.err.println("Invalid preset name, did you register it?");
- }
- }
-
- public void registerPreset(LEDPreset preset) {
- presets.put(preset.name(), preset);
- }
-}
diff --git a/src/main/java/frc/lib/led/LEDStrip.java b/src/main/java/frc/lib/led/LEDStrip.java
new file mode 100644
index 0000000..6e502ac
--- /dev/null
+++ b/src/main/java/frc/lib/led/LEDStrip.java
@@ -0,0 +1,118 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led;
+
+import frc.lib.led.presets.addressable.LEDPattern;
+import frc.lib.led.presets.addressable.RainbowPattern;
+
+/**
+ * Class to represent an LED strip with a fixed LED length and offset from the start.
+ * A physical LED strip can be divided into multiple LED strip objects.
+ * This allows for multiple patterns to be displayed on the same strip.
+ * Each strip can have its own color, pattern, and duration.
+ * @see Color
+ */
+public class LEDStrip {
+ private final int length;
+ private final int offset;
+
+ private double patternDuration = 5;
+ private Color primaryColor = new Color(12, 255, 255);
+ private Color secondaryColor = new Color(102, 255, 255);
+ private LEDPattern pattern = new RainbowPattern();
+
+ /**
+ * Creates a new LED strip.
+ * @param length The length of the strip in LEDs.
+ * @param offset The offset of the strip in LEDs.
+ */
+ public LEDStrip(int length, int offset) {
+ this.length = length;
+ this.offset = offset;
+ }
+
+ /**
+ * Sets the color of the strip.
+ * @param color The color to set the strip to.
+ */
+ @SuppressWarnings("unused")
+ public void setPrimaryColor(Color color) {
+ this.primaryColor = color;
+ }
+
+ /**
+ * Sets the secondary of the strip.
+ * @param color The color to set the strip to.
+ */
+ @SuppressWarnings("unused")
+ public void setSecondaryColor(Color color) {
+ this.secondaryColor = color;
+ }
+
+
+ /**
+ * Sets the pattern of the strip.
+ * @param pattern The pattern to set the strip to.
+ */
+ @SuppressWarnings("unused")
+ public void setPattern(LEDPattern pattern) {
+ this.pattern = pattern;
+ }
+
+ /**
+ * Sets the time it takes for the pattern to loop.
+ * @param patternDuration The pattern duration to set the strip to (in s).
+ */
+ @SuppressWarnings("unused")
+ public void setPatternDuration(double patternDuration) {
+ this.patternDuration = patternDuration;
+ }
+
+ /**
+ * Gets the length of the strip.
+ * @return The amount of LEDs on the strip.
+ */
+ public int getLength() {
+ return length;
+ }
+
+ /**
+ * Gets the offset of the strip.
+ * @return The amount of LEDs before this strip starts.
+ */
+ public int getOffset() {
+ return offset;
+ }
+
+ /**
+ * Gets the primary color of the strip.
+ * @return The color of the strip.
+ */
+ public Color getPrimaryColor() {
+ return primaryColor;
+ }
+
+ /**
+ * Gets the secondary color of the strip.
+ * @return The color of the strip.
+ */
+ public Color getSecondaryColor() {
+ return secondaryColor;
+ }
+
+ /**
+ * Gets the pattern of the strip.
+ * @return The pattern of the strip.
+ */
+ public LEDPattern getPattern() {
+ return pattern;
+ }
+
+ /**
+ * Gets the time it takes for the pattern to loop.
+ * @return The pattern duration in s.
+ */
+ public double getPatternDuration() {
+ return patternDuration;
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/PWMLEDController.java b/src/main/java/frc/lib/led/PWMLEDController.java
new file mode 100644
index 0000000..7931fd8
--- /dev/null
+++ b/src/main/java/frc/lib/led/PWMLEDController.java
@@ -0,0 +1,383 @@
+package frc.lib.led;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.function.Consumer;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+
+/**
+ * Unified RGB LED controller that can drive either a fixed 12V strip or an
+ * individually addressable 5V strip.
+ *
+ * Use {@link Mode#FIXED} for Spark/PWM-based controllers and
+ * {@link Mode#ADDRESSABLE} for addressable LED buffers.
+ */
+public class PWMLEDController {
+ /** Selects which hardware model the controller should use. */
+ public enum Mode {
+ FIXED,
+ ADDRESSABLE
+ }
+
+ /**
+ * Defines one logical preset name with a fixed implementation and an
+ * addressable implementation.
+ *
+ *
The addressable mutator receives a read-only strip list. It can mutate
+ * strip state (pattern/colors/duration) but should not modify strip topology.
+ */
+ public record ModePreset(
+ String name,
+ LEDPreset fixedPreset,
+ Consumer> addressableMutator
+ ) {}
+
+ private final Mode mode;
+ private final LEDBackend backend;
+
+ /**
+ * Creates a new RGB controller for either fixed or addressable hardware.
+ *
+ * @param port The PWM port the controller is connected to.
+ * @param mode The hardware mode to use.
+ */
+ public PWMLEDController(int port, Mode mode) {
+ this.mode = mode;
+ this.backend = switch (mode) {
+ case FIXED -> new FixedBackend(port);
+ case ADDRESSABLE -> new AddressableBackend(port);
+ };
+ }
+
+ /** Returns the configured hardware mode. */
+ public Mode getMode() {
+ return mode;
+ }
+
+ /** Returns true when this controller is using addressable LED hardware. */
+ public boolean isAddressable() {
+ return mode == Mode.ADDRESSABLE;
+ }
+
+ /** Returns true when this controller is using fixed LED hardware. */
+ public boolean isFixed() {
+ return mode == Mode.FIXED;
+ }
+
+ /** Starts the output of the controller. */
+ public void start() {
+ backend.start();
+ }
+
+ /** Stops the output of the controller. */
+ public void stop() {
+ backend.stop();
+ }
+
+ /**
+ * Updates the controller.
+ *
+ * For fixed hardware this is a no-op, while addressable hardware pushes
+ * the current strip buffers to the RoboRIO output.
+ */
+ public void update() {
+ backend.update();
+ }
+
+ /** Registers a preset for fixed LED hardware. */
+ public void registerPreset(LEDPreset preset) {
+ backend.registerPreset(preset);
+ }
+
+ /** Registers a mode-aware preset for either fixed or addressable hardware. */
+ public void registerPreset(ModePreset preset) {
+ backend.registerPreset(preset);
+ }
+
+ /** Switches the active preset for the currently selected hardware mode. */
+ public void switchPreset(String name) {
+ backend.switchPreset(name);
+ }
+
+ /** Adds a strip for addressable LED hardware. */
+ public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
+ backend.addStrip(strip);
+ }
+
+ /** Adds a strip for addressable LED hardware. */
+ public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
+ backend.addStrip(length, offset);
+ }
+
+ /** Gets an addressable strip by index. */
+ public LEDStrip getStrip(int index) {
+ return backend.getStrip(index);
+ }
+
+ /** Removes an addressable strip by index. */
+ public void removeStrip(int index) {
+ backend.removeStrip(index);
+ }
+
+ /** Returns the number of configured strips. */
+ public int getStripCount() {
+ return backend.getStripCount();
+ }
+
+ private interface LEDBackend {
+ void start();
+
+ void stop();
+
+ void update();
+
+ void registerPreset(LEDPreset preset);
+
+ void registerPreset(ModePreset preset);
+
+ void switchPreset(String name);
+
+ void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException;
+
+ void addStrip(int length, int offset) throws DuplicateLEDAssignmentException;
+
+ LEDStrip getStrip(int index);
+
+ void removeStrip(int index);
+
+ int getStripCount();
+ }
+
+ private static abstract class BaseBackend implements LEDBackend {
+ @Override
+ public void registerPreset(LEDPreset preset) {
+ throw unsupported("registerPreset");
+ }
+
+ @Override
+ public void registerPreset(ModePreset preset) {
+ throw unsupported("registerPreset");
+ }
+
+ @Override
+ public void switchPreset(String name) {
+ throw unsupported("switchPreset");
+ }
+
+ @Override
+ public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
+ throw unsupported("addStrip");
+ }
+
+ @Override
+ public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
+ throw unsupported("addStrip");
+ }
+
+ @Override
+ public LEDStrip getStrip(int index) {
+ throw unsupported("getStrip");
+ }
+
+ @Override
+ public void removeStrip(int index) {
+ throw unsupported("removeStrip");
+ }
+
+ @Override
+ public int getStripCount() {
+ return 0;
+ }
+
+ protected UnsupportedOperationException unsupported(String operation) {
+ return new UnsupportedOperationException(
+ operation + " is only supported for addressable or fixed hardware as applicable"
+ );
+ }
+ }
+
+ private static final class FixedBackend extends BaseBackend {
+ private final Spark controller;
+ private final Map presets = new HashMap<>();
+
+ private FixedBackend(int port) {
+ controller = new Spark(port);
+ }
+
+ @Override
+ public void start() {
+ // Fixed PWM strips are driven directly by setting the controller output.
+ }
+
+ @Override
+ public void stop() {
+ controller.set(0);
+ }
+
+ @Override
+ public void update() {
+ // Fixed strips do not need a periodic buffer push.
+ }
+
+ @Override
+ public void registerPreset(LEDPreset preset) {
+ presets.put(preset.name(), preset);
+ }
+
+ @Override
+ public void registerPreset(ModePreset preset) {
+ if (preset.fixedPreset() != null) {
+ registerPreset(preset.fixedPreset());
+ }
+ }
+
+ @Override
+ public void switchPreset(String name) {
+ LEDPreset targetPreset = presets.get(name);
+ if (targetPreset != null) {
+ controller.set(targetPreset.value());
+ } else {
+ System.err.println("Invalid preset name, did you register it?");
+ }
+ }
+ }
+
+ private static final class AddressableBackend extends BaseBackend {
+ private final AddressableLED controller;
+ private AddressableLEDBuffer buffer;
+ private final ArrayList strips = new ArrayList<>();
+ private final Map>> presets = new HashMap<>();
+
+ private AddressableBackend(int port) {
+ controller = new AddressableLED(port);
+ buffer = new AddressableLEDBuffer(0);
+ controller.setLength(0);
+ controller.setData(buffer);
+ }
+
+ @Override
+ public void start() {
+ controller.start();
+ }
+
+ @Override
+ public void stop() {
+ controller.stop();
+ }
+
+ @Override
+ public void update() {
+ for (LEDStrip strip : strips) {
+ strip.getPattern().updateBuffer(buffer, strip);
+ }
+
+ controller.setData(buffer);
+ }
+
+ @Override
+ public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
+ int index = getNewStripIndex(strip.getLength(), strip.getOffset());
+
+ if (index == strips.size()) {
+ strips.add(strip);
+ } else {
+ strips.add(index, strip);
+ }
+
+ updateTotalStripLength();
+ }
+
+ @Override
+ public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
+ addStrip(new LEDStrip(length, offset));
+ }
+
+ @Override
+ public void registerPreset(ModePreset preset) {
+ if (preset.addressableMutator() != null) {
+ presets.put(preset.name(), preset.addressableMutator());
+ }
+ }
+
+ @Override
+ public void registerPreset(LEDPreset preset) {
+ // Fixed presets are ignored in addressable mode.
+ }
+
+ @Override
+ public void switchPreset(String name) {
+ Consumer> stripMutator = presets.get(name);
+ if (stripMutator == null) {
+ System.err.println("Invalid preset name, did you register it?");
+ return;
+ }
+
+ try {
+ stripMutator.accept(Collections.unmodifiableList(strips));
+ } catch (RuntimeException ex) {
+ throw new IllegalStateException(
+ "Failed to apply addressable preset '" + name + "'",
+ ex
+ );
+ }
+ }
+
+ @Override
+ public LEDStrip getStrip(int index) {
+ return strips.get(index);
+ }
+
+ @Override
+ public void removeStrip(int index) {
+ strips.remove(index);
+ updateTotalStripLength();
+ }
+
+ @Override
+ public int getStripCount() {
+ return strips.size();
+ }
+
+ private void updateTotalStripLength() {
+ int totalLength = 0;
+ if (!strips.isEmpty()) {
+ LEDStrip lastStrip = strips.get(strips.size() - 1);
+ totalLength = lastStrip.getOffset() + lastStrip.getLength();
+ }
+
+ buffer = new AddressableLEDBuffer(totalLength);
+ controller.setLength(totalLength);
+ controller.setData(buffer);
+ }
+
+ private int getNewStripIndex(int length, int offset) {
+ int newStripEnd = offset + length;
+
+ for (int i = 0; i < strips.size(); i++) {
+ LEDStrip strip = strips.get(i);
+ int stripStart = strip.getOffset();
+ int stripEnd = stripStart + strip.getLength();
+
+ if (offset < stripEnd && newStripEnd > stripStart) {
+ throw new DuplicateLEDAssignmentException(
+ "LED strip overlaps with existing strip at index " + i
+ );
+ }
+ }
+
+ for (int i = 0; i < strips.size(); i++) {
+ if (strips.get(i).getOffset() > offset) {
+ return i;
+ }
+ }
+
+ return strips.size();
+ }
+ }
+}
+
diff --git a/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java b/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java
new file mode 100644
index 0000000..283e246
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java
@@ -0,0 +1,51 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+import frc.lib.CountingDelay;
+
+/**
+ * A blinking pattern that can be applied to an LED strip.
+ * This pattern will alternate between the primary and secondary colors.
+ * @see LEDStrip
+ */
+public class BlinkPattern implements LEDPattern {
+ private final CountingDelay blinkDelay = new CountingDelay();
+ private boolean blinkPrimary = true;
+
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ Color.HSV primaryColor = strip.getPrimaryColor().getHSV();
+ Color.HSV secondaryColor = strip.getSecondaryColor().getHSV();
+ double blinkDuration = strip.getPatternDuration();
+
+ // Switching states should be done twice per duration
+ if (blinkDelay.delay(blinkDuration / 2)) {
+ blinkPrimary = !blinkPrimary;
+ blinkDelay.reset();
+ }
+
+ Color.HSV color = blinkPrimary ? primaryColor : secondaryColor;
+
+ // Set the LEDs to the color if blinkOn is true, otherwise set them to black
+ for (int i = 0; i < length; i++) {
+ buffer.setHSV(
+ offset + i,
+ color.hue(),
+ color.saturation(),
+ color.value()
+ );
+ }
+ }
+}
diff --git a/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java b/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java
new file mode 100644
index 0000000..ccc7861
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java
@@ -0,0 +1,65 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+
+/**
+ * A chase pattern that can be applied to an LED strip.
+ * This pattern will override the value of the primary color.
+ * The hue and saturation of the primary color are applied to the chase.
+ * The secondary color is not used.
+ * @see LEDStrip
+ */
+public class ChasePattern implements LEDPattern{
+ private double valueShiftBuffer = 0;
+ private double previousTime = Timer.getFPGATimestamp();
+ private int firstPixelValue = 255;
+
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ double patternSeconds = strip.getPatternDuration();
+ Color.HSV color = strip.getPrimaryColor().getHSV();
+
+ // Calculate delta time
+ double newTime = Timer.getFPGATimestamp();
+ double deltaTime = newTime - previousTime;
+ previousTime = newTime;
+
+ // For every pixel
+ for (int i = 0; i < length; i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ int value = (firstPixelValue - (i * color.value() / length)) % color.value();
+ if (value < 0) value += color.value();
+
+ // Set the value
+ buffer.setHSV(i + offset, color.hue(), color.saturation(), value);
+ }
+
+ // Increase by to make the rainbow "move"
+ valueShiftBuffer += ((deltaTime / patternSeconds) * color.value());
+ int valueShift = (int) valueShiftBuffer;
+ valueShiftBuffer -= valueShift;
+ firstPixelValue -= valueShift;
+
+ // Check bounds
+ if (color.value() > 0){
+ firstPixelValue %= color.value();
+ if(firstPixelValue < 0) firstPixelValue += color.value();
+ } else {
+ firstPixelValue = 0;
+ }
+ }
+}
diff --git a/src/main/java/frc/lib/led/presets/addressable/FadePattern.java b/src/main/java/frc/lib/led/presets/addressable/FadePattern.java
new file mode 100644
index 0000000..a7f3a2c
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/FadePattern.java
@@ -0,0 +1,46 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+
+/**
+ * A fading sine wave pattern that can be applied to an LED strip.
+ * This pattern will interpolate between the primary and secondary colors.
+ * @see LEDStrip
+ */
+public class FadePattern implements LEDPattern {
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ Color.RGB primary = strip.getPrimaryColor().getRGB();
+ Color.RGB secondary = strip.getSecondaryColor().getRGB();
+
+ // Fading is done by changing the value of the color over a sine wave
+ double wavelength = (2 * Math.PI) / strip.getPatternDuration();
+ double interpolation = Math.sin(wavelength * Timer.getFPGATimestamp()) * 0.5 + 0.5;
+
+ for (int i = 0; i < length; i++) {
+ buffer.setRGB(
+ offset + i,
+ interpolate(primary.red(), secondary.red(), interpolation),
+ interpolate(primary.green(), secondary.green(), interpolation),
+ interpolate(primary.blue(), secondary.blue(), interpolation)
+ );
+ }
+ }
+
+ private int interpolate(int start, int end, double interpolationValue) {
+ return (int) (start + (end - start) * interpolationValue);
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java b/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java
new file mode 100644
index 0000000..685cd2b
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java
@@ -0,0 +1,9 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import frc.lib.led.LEDStrip;
+public interface LEDPattern {
+ void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip);
+}
diff --git a/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java b/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java
new file mode 100644
index 0000000..c325620
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java
@@ -0,0 +1,179 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+import java.util.ArrayList;
+import java.util.Random;
+
+/**
+ * A pattern that can be applied to an LED strip.
+ * This pattern will mimic the merge sort algorithm.
+ * It will override the hue of the primary color.
+ * The saturation and value of the primary color remain the same.
+ * The secondary color is not used.
+ * The pattern duration is approximated but might not be exact.
+ * @see LEDStrip
+ */
+public class MergeSortPattern implements LEDPattern {
+ private int[] sortArray = new int[0];
+ private final ArrayList sortSteps = new ArrayList<>();
+
+ private double stepShiftBuffer = 0;
+ private double previousTime = Timer.getFPGATimestamp();
+ private int stepIndex = 0;
+
+ private boolean sorted = false;
+ private boolean shuffled = false;
+
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ final int HUE_RANGE = 180;
+
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ Color.HSV color = strip.getPrimaryColor().getHSV();
+ double patternSeconds = strip.getPatternDuration();
+
+ // Calculate delta time
+ double newTime = Timer.getFPGATimestamp();
+ double deltaTime = newTime - previousTime;
+ previousTime = newTime;
+
+ // Create new array if needed
+ if (sortArray.length != length) {
+ sortArray = new int[length];
+
+ for (int i = 0; i < length; i++) {
+ sortArray[i] = i * HUE_RANGE / length % HUE_RANGE;
+ }
+
+ shuffled = false;
+ }
+
+ // Shuffle array if needed
+ if (!shuffled) {
+ sortSteps.clear();
+
+ shuffleArray(sortArray);
+
+ sortSteps.add(sortArray.clone());
+
+ shuffled = true;
+ sorted = false;
+ stepIndex = 0;
+ }
+
+ // Sort array if needed
+ if (!sorted) {
+ mergeSort(sortArray, 0, sortArray.length - 1);
+ sorted = true;
+ }
+
+ // Calculate delta time
+ stepShiftBuffer += ((deltaTime / patternSeconds) * sortSteps.size());
+ int stepShift = (int) stepShiftBuffer;
+ stepShiftBuffer -= stepShift;
+ stepIndex += stepShift;
+
+
+ // Check bounds
+ if (stepIndex >= sortSteps.size()) {
+ shuffled = false;
+ } else {
+ int[] step = sortSteps.get(stepIndex);
+
+ // Set colors
+ for (int i = 0; i < length; i++) {
+ int hue = step[i];
+ buffer.setHSV(i + offset, hue, color.saturation(), color.value());
+ }
+ }
+ }
+
+ private void mergeSort(int[] array, int left, int right) {
+ if (left < right) {
+ int mid = left + (right - left) / 2;
+
+ // Recursively sort the two halves
+ mergeSort(array, left, mid);
+ mergeSort(array, mid + 1, right);
+
+ // Merge the sorted halves
+ merge(array, left, mid, right);
+
+ // Add the entire array after each merge
+ sortSteps.add(array.clone());
+ }
+ }
+
+ /**
+ * Sorts an array using the merge sort algorithm.
+ * @param array The array to be sorted.
+ * @param left The left index of the array.
+ * @param right The right index of the array.
+ * @param mid The middle index of the array.
+ */
+ public void merge(int[] array, int left, int mid, int right) {
+ int n1 = mid - left + 1;
+ int n2 = right - mid;
+
+ int[] leftArray = new int[n1];
+ int[] rightArray = new int[n2];
+
+ // Copy data to temporary arrays
+ for (int i = 0; i < n1; ++i) {
+ leftArray[i] = array[left + i];
+ }
+ for (int j = 0; j < n2; ++j) {
+ rightArray[j] = array[mid + 1 + j];
+ }
+
+ // Merge the temporary arrays back into the original array
+ int i = 0, j = 0, k = left;
+ while (i < n1 && j < n2) {
+ if (leftArray[i] <= rightArray[j]) {
+ array[k++] = leftArray[i++];
+ } else {
+ array[k++] = rightArray[j++];
+ }
+ sortSteps.add(array.clone());
+ }
+
+ // Copy the remaining elements of leftArray[], if there are any
+ while (i < n1) {
+ array[k++] = leftArray[i++];
+ sortSteps.add(array.clone());
+ }
+
+ // Copy the remaining elements of rightArray[], if there are any
+ while (j < n2) {
+ array[k++] = rightArray[j++];
+ sortSteps.add(array.clone());
+ }
+ }
+
+ /**
+ * Shuffles an array.
+ * @param array The array to be shuffled.
+ */
+ private static void shuffleArray(int[] array) {
+ Random rand = new Random();
+
+ for (int i = 0; i < array.length; i++) {
+ int randomIndexToSwap = rand.nextInt(array.length);
+ int temp = array[randomIndexToSwap];
+ array[randomIndexToSwap] = array[i];
+ array[i] = temp;
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java b/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java
new file mode 100644
index 0000000..8546a32
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java
@@ -0,0 +1,62 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+
+/**
+ * A rainbow pattern that can be applied to an LED strip.
+ * This pattern will override the hue of the primary color.
+ * The saturation and value of the primary color are applied to the rainbow.
+ * The secondary color is not used.
+ * @see LEDStrip
+ */
+public class RainbowPattern implements LEDPattern {
+ // Constants
+ private static final int HUE_RANGE = 180;
+
+ private int rainbowFirstPixelHue = 0;
+ private double hueShiftBuffer = 0;
+ private double previousTime = Timer.getFPGATimestamp();
+
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ double patternSeconds = strip.getPatternDuration();
+ Color.HSV color = strip.getPrimaryColor().getHSV();
+
+ // Calculate delta time
+ double newTime = Timer.getFPGATimestamp();
+ double deltaTime = newTime - previousTime;
+ previousTime = newTime;
+
+ // For every pixel
+ for (int i = 0; i < length; i++) {
+ // Calculate the hue - hue is easier for rainbows because the color
+ // shape is a circle so only one value needs to precess
+ final int hue = (rainbowFirstPixelHue + (i * HUE_RANGE / length)) % HUE_RANGE;
+ // Set the value
+ buffer.setHSV(i + offset, hue, color.saturation(), color.value());
+ }
+
+ // Increase by to make the rainbow "move"
+ hueShiftBuffer += ((deltaTime / patternSeconds) * HUE_RANGE);
+ int hueShift = (int) hueShiftBuffer;
+ hueShiftBuffer -= hueShift;
+ rainbowFirstPixelHue += hueShift;
+
+ // Check bounds
+ rainbowFirstPixelHue %= HUE_RANGE;
+ if(rainbowFirstPixelHue < 0) rainbowFirstPixelHue += HUE_RANGE;
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java b/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java
new file mode 100644
index 0000000..05dc965
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java
@@ -0,0 +1,30 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+
+/**
+ * A solid color pattern that can be applied to an LED strip.
+ * @see LEDStrip
+ */
+public class SolidColorPattern implements LEDPattern {
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ Color.HSV color = strip.getPrimaryColor().getHSV();
+
+ for (int i = 0; i < length; i++) {
+ buffer.setHSV(offset + i, color.hue(), color.saturation(), color.value());
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/WavePattern.java b/src/main/java/frc/lib/led/presets/addressable/WavePattern.java
new file mode 100644
index 0000000..9282d3c
--- /dev/null
+++ b/src/main/java/frc/lib/led/presets/addressable/WavePattern.java
@@ -0,0 +1,52 @@
+// Implementation based on Team 4481's LED Controller
+// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
+package frc.lib.led.presets.addressable;
+
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.Timer;
+import frc.lib.led.Color;
+import frc.lib.led.LEDStrip;
+
+/**
+ * A wave pattern that can be applied to an LED strip.
+ * This pattern will interpolate between the primary and secondary colors.
+ * These colors will shift over time.
+ * @see LEDStrip
+ */
+public class WavePattern implements LEDPattern {
+ /**
+ * Updates the LED buffer with the pattern.
+ *
+ * @param buffer The LED buffer to update.
+ * @param strip The strip to update the buffer with.
+ */
+ @Override
+ public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
+ int offset = strip.getOffset();
+ int length = strip.getLength();
+ Color.RGB primary = strip.getPrimaryColor().getRGB();
+ Color.RGB secondary = strip.getSecondaryColor().getRGB();
+
+ // Fading is done by changing the value of the color over a sine wave
+ double ledWavelength = (2 * Math.PI) / length;
+ double timeWavelength = (2 * Math.PI) / strip.getPatternDuration();
+
+ // Calculate time offset
+ double timeOffset = Timer.getFPGATimestamp() * timeWavelength;
+
+ for (int i = 0; i < length; i++) {
+ double interpolation = Math.sin(ledWavelength * i + timeOffset) * 0.5 + 0.5;
+
+ buffer.setRGB(
+ offset + i,
+ interpolate(primary.red(), secondary.red(), interpolation),
+ interpolate(primary.green(), secondary.green(), interpolation),
+ interpolate(primary.blue(), secondary.blue(), interpolation)
+ );
+ }
+ }
+
+ private int interpolate(int start, int end, double interpolationValue) {
+ return (int) (start + (end - start) * interpolationValue);
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3d4389b..fae1107 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -13,8 +13,6 @@
package frc.robot;
-import edu.wpi.first.net.PortForwarder;
-import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -156,6 +154,7 @@ void SetupLog() {
@Override
public void robotPeriodic() {
double startTime = Timer.getFPGATimestamp();
+ robotContainer.updateLEDs(); // Updates all LEDs for dynamic patterns.
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled commands, running already-scheduled commands, removing
// finished or interrupted commands, and running subsystem periodic() methods.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b745bd8..3d2a480 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,7 +7,6 @@
import static edu.wpi.first.units.Units.*;
import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
-import com.pathplanner.lib.auto.NamedCommands;
import com.ctre.phoenix6.swerve.SwerveRequest;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -19,8 +18,14 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.lib.SpikeController;
-import frc.lib.led.LEDController;
+import frc.lib.led.Color;
import frc.lib.led.LEDPreset;
+import frc.lib.led.LEDStrip;
+import frc.lib.led.PWMLEDController;
+import frc.lib.led.PWMLEDController.ModePreset;
+import frc.lib.led.presets.addressable.ChasePattern;
+import frc.lib.led.presets.addressable.RainbowPattern;
+import frc.lib.led.presets.addressable.SolidColorPattern;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
import frc.robot.subsystems.findexer.Findexer;
@@ -65,10 +70,17 @@ public class RobotContainer {
private final Targeting targeting;
private final Findexer findexer;
- private static LEDController ledController;
+ private static final PWMLEDController.Mode LED_MODE = PWMLEDController.Mode.FIXED;
+ private static final int ADDRESSABLE_LED_LENGTH = 60;
+
+ private static PWMLEDController ledController;
public RobotContainer() {
- ledController = new LEDController(0);
+ ledController = new PWMLEDController(0, LED_MODE);
+ configureAddressableStrips();
+ configureLEDPresets();
+ ledController.start();
+
drive = TunerConstants.createDrivetrain();
this.turret = new Turret();
this.vision = new Vision(drive);
@@ -82,29 +94,85 @@ public RobotContainer() {
SmartDashboard.putData("Auto Path", autoChooser);
configureBindings();
- configureLEDPresets();
ledController.switchPreset("hub");
}
private void configureLEDPresets() {
- ledController.registerPreset(
- new LEDPreset("hub", -0.25)
+ ledController.registerPreset(createHubPreset());
+ ledController.registerPreset(createShuttlePreset());
+ ledController.registerPreset(createFixedPreset());
+ }
+
+ private void configureAddressableStrips() {
+ if (!ledController.isAddressable()) {
+ return;
+ }
+
+ // Configure physical strip layout once. Presets only mutate strip state.
+ ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH, 0));
+ }
+
+ private ModePreset createHubPreset() {
+ return new ModePreset(
+ "hub",
+ new LEDPreset("hub", -0.25),
+ strips -> {
+ LEDStrip strip = getRequiredStrip(strips, 0, "hub");
+ strip.setPrimaryColor(new Color(100, 255, 255));
+ strip.setSecondaryColor(new Color(100, 255, 255));
+ strip.setPattern(new RainbowPattern());
+ strip.setPatternDuration(2.5);
+ }
);
+ }
- ledController.registerPreset(
- new LEDPreset("shuttle", -0.23)
+ private ModePreset createShuttlePreset() {
+ return new ModePreset(
+ "shuttle",
+ new LEDPreset("shuttle", -0.23),
+ strips -> {
+ LEDStrip strip = getRequiredStrip(strips, 0, "shuttle");
+ strip.setPrimaryColor(new Color(35, 255, 255));
+ strip.setSecondaryColor(new Color(0, 0, 0));
+ strip.setPattern(new ChasePattern());
+ strip.setPatternDuration(1.5);
+ }
);
+ }
- ledController.registerPreset(
- new LEDPreset("fixed", -0.21)
+ private ModePreset createFixedPreset() {
+ return new ModePreset(
+ "fixed",
+ new LEDPreset("fixed", -0.21),
+ strips -> {
+ LEDStrip strip = getRequiredStrip(strips, 0, "fixed");
+ strip.setPrimaryColor(new Color(0, 255, 255));
+ strip.setSecondaryColor(new Color(0, 0, 0));
+ strip.setPattern(new SolidColorPattern());
+ strip.setPatternDuration(1.0);
+ }
);
}
- public static LEDController getLEDController() {
+ private LEDStrip getRequiredStrip(java.util.List strips, int index, String presetName) {
+ if (index < 0 || index >= strips.size()) {
+ throw new IllegalStateException(
+ "Addressable preset '" + presetName + "' requires strip index " + index
+ );
+ }
+
+ return strips.get(index);
+ }
+
+ public static PWMLEDController getLEDController() {
return ledController;
}
+ public void updateLEDs() {
+ ledController.update();
+ }
+
public static CommandSwerveDrivetrain getDrive() {
return drive;
}
From f9e3b04a0346d92c52135d720c0b13e6c96332f8 Mon Sep 17 00:00:00 2001
From: Maxim Kudryashov <74438588+maxim-kudryashov@users.noreply.github.com>
Date: Thu, 9 Apr 2026 08:56:36 -0400
Subject: [PATCH 02/14] fix: Added access modifiers to addStrip method and
CountingDelay variables.
---
src/main/java/frc/lib/CountingDelay.java | 4 ++--
src/main/java/frc/lib/led/AddressableLEDController.java | 2 +-
2 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/src/main/java/frc/lib/CountingDelay.java b/src/main/java/frc/lib/CountingDelay.java
index 7d83cc1..e4c31bd 100644
--- a/src/main/java/frc/lib/CountingDelay.java
+++ b/src/main/java/frc/lib/CountingDelay.java
@@ -3,8 +3,8 @@
import edu.wpi.first.wpilibj.Timer;
public class CountingDelay {
- boolean lock;
- double startTimeStamp;
+ private boolean lock;
+ private double startTimeStamp;
public CountingDelay() {
reset();
diff --git a/src/main/java/frc/lib/led/AddressableLEDController.java b/src/main/java/frc/lib/led/AddressableLEDController.java
index b76ba98..878e2a1 100644
--- a/src/main/java/frc/lib/led/AddressableLEDController.java
+++ b/src/main/java/frc/lib/led/AddressableLEDController.java
@@ -40,7 +40,7 @@ public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
* @param offset The offset to the start of the strip in LEDs.
* @throws DuplicateLEDAssignmentException If the new strip overlaps with an existing strip.
*/
- void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
+ public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
controller.addStrip(length, offset);
}
From c99bba419188009328e58cbc8fb20bc0610b269a Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 9 Apr 2026 17:18:28 -0400
Subject: [PATCH 03/14] fixed channels and patterns
---
src/main/java/frc/robot/RobotContainer.java | 37 +++++++++++++++++----
1 file changed, 30 insertions(+), 7 deletions(-)
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3d2a480..827a256 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -12,6 +12,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color8Bit;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
@@ -24,8 +25,11 @@
import frc.lib.led.PWMLEDController;
import frc.lib.led.PWMLEDController.ModePreset;
import frc.lib.led.presets.addressable.ChasePattern;
+import frc.lib.led.presets.addressable.FadePattern;
+import frc.lib.led.presets.addressable.MergeSortPattern;
import frc.lib.led.presets.addressable.RainbowPattern;
import frc.lib.led.presets.addressable.SolidColorPattern;
+import frc.lib.led.presets.addressable.WavePattern;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
import frc.robot.subsystems.findexer.Findexer;
@@ -70,8 +74,8 @@ public class RobotContainer {
private final Targeting targeting;
private final Findexer findexer;
- private static final PWMLEDController.Mode LED_MODE = PWMLEDController.Mode.FIXED;
- private static final int ADDRESSABLE_LED_LENGTH = 60;
+ private static final PWMLEDController.Mode LED_MODE = PWMLEDController.Mode.ADDRESSABLE;
+ private static final int ADDRESSABLE_LED_LENGTH = 160;
private static PWMLEDController ledController;
@@ -110,7 +114,8 @@ private void configureAddressableStrips() {
}
// Configure physical strip layout once. Presets only mutate strip state.
- ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH, 0));
+ ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH / 2, 0));
+ ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH / 2, ADDRESSABLE_LED_LENGTH / 2));
}
private ModePreset createHubPreset() {
@@ -119,10 +124,16 @@ private ModePreset createHubPreset() {
new LEDPreset("hub", -0.25),
strips -> {
LEDStrip strip = getRequiredStrip(strips, 0, "hub");
- strip.setPrimaryColor(new Color(100, 255, 255));
- strip.setSecondaryColor(new Color(100, 255, 255));
+ strip.setPrimaryColor(Color.fromHex("00F5FF"));
+ strip.setSecondaryColor(Color.fromHex("FF007F"));
strip.setPattern(new RainbowPattern());
strip.setPatternDuration(2.5);
+
+ LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "hub");
+ secondaryStrip.setPrimaryColor(Color.fromHex("00F5FF"));
+ secondaryStrip.setSecondaryColor(Color.fromHex("FF007F"));
+ secondaryStrip.setPattern(new RainbowPattern());
+ secondaryStrip.setPatternDuration(2.5);
}
);
}
@@ -135,8 +146,14 @@ private ModePreset createShuttlePreset() {
LEDStrip strip = getRequiredStrip(strips, 0, "shuttle");
strip.setPrimaryColor(new Color(35, 255, 255));
strip.setSecondaryColor(new Color(0, 0, 0));
- strip.setPattern(new ChasePattern());
+ strip.setPattern(new MergeSortPattern());
strip.setPatternDuration(1.5);
+
+ LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "shuttle");
+ secondaryStrip.setPrimaryColor(new Color(35, 255, 255));
+ secondaryStrip.setSecondaryColor(new Color(0, 0, 0));
+ secondaryStrip.setPattern(new MergeSortPattern());
+ secondaryStrip.setPatternDuration(1.5);
}
);
}
@@ -149,8 +166,14 @@ private ModePreset createFixedPreset() {
LEDStrip strip = getRequiredStrip(strips, 0, "fixed");
strip.setPrimaryColor(new Color(0, 255, 255));
strip.setSecondaryColor(new Color(0, 0, 0));
- strip.setPattern(new SolidColorPattern());
+ strip.setPattern(new ChasePattern());
strip.setPatternDuration(1.0);
+
+ LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "fixed");
+ secondaryStrip.setPrimaryColor(new Color(0, 255, 255));
+ secondaryStrip.setSecondaryColor(new Color(0, 0, 0));
+ secondaryStrip.setPattern(new ChasePattern());
+ secondaryStrip.setPatternDuration(1.0);
}
);
}
From fb0e1b4df45d3806d1b44ab37c6f03c78e94366c Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Sun, 12 Apr 2026 09:05:02 -0400
Subject: [PATCH 04/14] Change auto paths and fix camera issues, add logging of
position
---
...er Score.auto => 1 Pass Depot to Mid.auto} | 0
.../autos/1 Pass Outpost to Mid.auto | 49 +++++++
.../autos/2 Pass Outpost to Mid.auto | 61 ++++++++
.../autos/Outpost Center Score.auto | 4 +-
...turn.path => Pass 1 - Outpost to Mid.path} | 99 +++++--------
.../paths/Pass 2 - Outpost to Mid.path | 137 ++++++++++++++++++
src/main/java/frc/robot/Robot.java | 2 +-
src/main/java/frc/robot/RobotContainer.java | 7 +
.../commands/RequestShootingForSeconds.java | 4 +-
.../drive/CommandSwerveDrivetrain.java | 6 +
.../vision/photon/CameraManager.java | 30 ++--
11 files changed, 316 insertions(+), 83 deletions(-)
rename src/main/deploy/pathplanner/autos/{Depot Center Score.auto => 1 Pass Depot to Mid.auto} (100%)
create mode 100644 src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto
create mode 100644 src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto
rename src/main/deploy/pathplanner/paths/{Outpost Center Path + Return.path => Pass 1 - Outpost to Mid.path} (55%)
create mode 100644 src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path
diff --git a/src/main/deploy/pathplanner/autos/Depot Center Score.auto b/src/main/deploy/pathplanner/autos/1 Pass Depot to Mid.auto
similarity index 100%
rename from src/main/deploy/pathplanner/autos/Depot Center Score.auto
rename to src/main/deploy/pathplanner/autos/1 Pass Depot to Mid.auto
diff --git a/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto b/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto
new file mode 100644
index 0000000..5dfeacf
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/1 Pass Outpost to Mid.auto
@@ -0,0 +1,49 @@
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "deployIntake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "enableIntake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "startFlywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Pass 1 - Outpost to Mid"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "requestShooting20S"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "stopFlywheel"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto b/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto
new file mode 100644
index 0000000..14425a4
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/2 Pass Outpost to Mid.auto
@@ -0,0 +1,61 @@
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "deployIntake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "enableIntake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "startFlywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Pass 1 - Outpost to Mid"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "requestShooting5S"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Pass 2 - Outpost to Mid"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "requestShooting20S"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "stopFlywheel"
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Outpost Center Score.auto b/src/main/deploy/pathplanner/autos/Outpost Center Score.auto
index 0c1833e..ae9cb19 100644
--- a/src/main/deploy/pathplanner/autos/Outpost Center Score.auto
+++ b/src/main/deploy/pathplanner/autos/Outpost Center Score.auto
@@ -13,13 +13,13 @@
{
"type": "path",
"data": {
- "pathName": "Outpost Center Path + Return"
+ "pathName": "Pass 1 - Outpost to Mid"
}
},
{
"type": "named",
"data": {
- "name": "requestShooting15S"
+ "name": "requestShooting10S"
}
}
]
diff --git a/src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path b/src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path
similarity index 55%
rename from src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path
rename to src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path
index c2bddf9..76a69e7 100644
--- a/src/main/deploy/pathplanner/paths/Outpost Center Path + Return.path
+++ b/src/main/deploy/pathplanner/paths/Pass 1 - Outpost to Mid.path
@@ -3,12 +3,12 @@
"waypoints": [
{
"anchor": {
- "x": 4.320596026490066,
+ "x": 3.9938990066225166,
"y": 0.666390728476822
},
"prevControl": null,
"nextControl": {
- "x": 5.320596026490067,
+ "x": 4.9938990066225175,
"y": 0.666390728476822
},
"isLocked": false,
@@ -32,44 +32,44 @@
},
{
"anchor": {
- "x": 7.391548013245033,
- "y": 2.9823096026490052
+ "x": 7.093890728476822,
+ "y": 3.301746688741722
},
"prevControl": {
- "x": 8.052817700960428,
- "y": 2.9877298459909336
+ "x": 7.755160416192217,
+ "y": 3.3071669320836503
},
"nextControl": {
- "x": 6.5058360927152314,
- "y": 2.9750496688741723
+ "x": 6.208178807947021,
+ "y": 3.294486754966889
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 6.498576158940397,
- "y": 1.3415645695364242
+ "x": 6.476796357615894,
+ "y": 1.101986754966888
},
"prevControl": {
- "x": 6.9269122516556285,
- "y": 2.2127566225165562
+ "x": 6.905132450331125,
+ "y": 1.9731788079470203
},
"nextControl": {
- "x": 6.214810484806516,
- "y": 0.7644140458742933
+ "x": 6.193030683482013,
+ "y": 0.5248362313047571
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 4.320596026490066,
+ "x": 3.7035016556291387,
"y": 0.666390728476822
},
"prevControl": {
- "x": 5.796368851244969,
- "y": 0.8582092889088078
+ "x": 5.206307947019867,
+ "y": 0.7172102649006615
},
"nextControl": null,
"isLocked": false,
@@ -77,22 +77,34 @@
}
],
"rotationTargets": [
+ {
+ "waypointRelativePos": 0.3099273607748189,
+ "rotationDegrees": 0.0
+ },
{
"waypointRelativePos": 0.9539951573849952,
- "rotationDegrees": 117.40360625563348
+ "rotationDegrees": 98.80231741603863
+ },
+ {
+ "waypointRelativePos": 1.8983050847457779,
+ "rotationDegrees": 106.53481995772754
},
{
"waypointRelativePos": 2.6963635575589464,
- "rotationDegrees": 89.95862437106403
+ "rotationDegrees": 87.79015138447025
+ },
+ {
+ "waypointRelativePos": 3.675544794188865,
+ "rotationDegrees": 2.2136983625309083
}
],
"constraintZones": [
{
"name": "Constraints Zone",
- "minWaypointRelativePos": 1.2071651090342688,
- "maxWaypointRelativePos": 1.8885319314641746,
+ "minWaypointRelativePos": 1.1646643109540615,
+ "maxWaypointRelativePos": 2.131448763250887,
"constraints": {
- "maxVelocity": 1.5,
+ "maxVelocity": 1.2,
"maxAcceleration": 1.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
@@ -102,48 +114,7 @@
}
],
"pointTowardsZones": [],
- "eventMarkers": [
- {
- "name": "startFlywheel",
- "waypointRelativePos": 0.3279151943462853,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "startFlywheel"
- }
- }
- },
- {
- "name": "enableIntake",
- "waypointRelativePos": 0.38445229681977794,
- "endWaypointRelativePos": null,
- "command": {
- "type": "named",
- "data": {
- "name": "enableIntake"
- }
- }
- },
- {
- "name": "disableIntake",
- "waypointRelativePos": 2.137102473498207,
- "endWaypointRelativePos": null,
- "command": {
- "type": "sequential",
- "data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "disableIntake"
- }
- }
- ]
- }
- }
- }
- ],
+ "eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
diff --git a/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path b/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path
new file mode 100644
index 0000000..4caa88c
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Pass 2 - Outpost to Mid.path
@@ -0,0 +1,137 @@
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.805140728476821,
+ "y": 0.666390728476822
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 4.805140728476822,
+ "y": 0.666390728476822
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.1573592715231795,
+ "y": 1.0802069536423846
+ },
+ "prevControl": {
+ "x": 5.9890401792156975,
+ "y": 0.635363638258319
+ },
+ "nextControl": {
+ "x": 6.258998344370861,
+ "y": 1.3488245033112594
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.794362582781457,
+ "y": 2.270836092715232
+ },
+ "prevControl": {
+ "x": 5.935691178086882,
+ "y": 2.047536912132661
+ },
+ "nextControl": {
+ "x": 5.431365894039736,
+ "y": 2.8443708609271505
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 6.1573592715231795,
+ "y": 1.2181456953642373
+ },
+ "prevControl": {
+ "x": 6.309817880794703,
+ "y": 1.4577235099337735
+ },
+ "nextControl": {
+ "x": 5.81207457608011,
+ "y": 0.6755554596679854
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 3.805140728476821,
+ "y": 0.666390728476822
+ },
+ "prevControl": {
+ "x": 5.409586092715232,
+ "y": 0.7099503311258286
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 0.4503631961259079,
+ "rotationDegrees": 0.0
+ },
+ {
+ "waypointRelativePos": 1.2493946731235022,
+ "rotationDegrees": 106.73738567187311
+ },
+ {
+ "waypointRelativePos": 1.961259079903153,
+ "rotationDegrees": 106.53481995772754
+ },
+ {
+ "waypointRelativePos": 2.6246973365617308,
+ "rotationDegrees": 87.79015138447025
+ },
+ {
+ "waypointRelativePos": 3.767554479418897,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 1.1646643109540615,
+ "maxWaypointRelativePos": 2.131448763250887,
+ "constraints": {
+ "maxVelocity": 1.2,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -0.24014922069523068
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 0.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 07feafe..a9c2711 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -16,6 +16,7 @@
import edu.wpi.first.net.PortForwarder;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -39,7 +40,6 @@
public class Robot extends LoggedRobot {
private Command autonomousCommand;
private RobotContainer robotContainer;
-
private static final boolean IS_PRACTICE = true;
private static final String LOG_DIRECTORY = "/home/lvuser/logs";
private static final long MIN_FREE_SPACE =
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9d25049..2c92ac2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -88,7 +88,14 @@ public RobotContainer() {
NamedCommands.registerCommand("disableIntake", new SetIntakeState(intake, false));
NamedCommands.registerCommand("startFlywheel", new SetFlywheelState(shooter, true));
NamedCommands.registerCommand("stopFlywheel", new SetFlywheelState(shooter, false));
+ NamedCommands.registerCommand("requestShooting5S", new RequestShootingForSeconds(shooter, true, 5.0));
+ NamedCommands.registerCommand("requestShooting6S", new RequestShootingForSeconds(shooter, true, 6.0));
+ NamedCommands.registerCommand("requestShooting7S", new RequestShootingForSeconds(shooter, true, 7.0));
+ NamedCommands.registerCommand("requestShooting8S", new RequestShootingForSeconds(shooter, true, 8.0));
+ NamedCommands.registerCommand("requestShooting9S", new RequestShootingForSeconds(shooter, true, 9.0));
NamedCommands.registerCommand("requestShooting10S", new RequestShootingForSeconds(shooter, true, 10.0));
+ NamedCommands.registerCommand("requestShooting11S", new RequestShootingForSeconds(shooter, true, 11.0));
+ NamedCommands.registerCommand("requestShooting12S", new RequestShootingForSeconds(shooter, true, 12.0));
NamedCommands.registerCommand("requestShooting15S", new RequestShootingForSeconds(shooter, true, 15.0));
NamedCommands.registerCommand("requestShooting20S", new RequestShootingForSeconds(shooter, true, 20.0));
NamedCommands.registerCommand("stopShooting", new RequestShootingForSeconds(shooter, false, 0.0));
diff --git a/src/main/java/frc/robot/commands/RequestShootingForSeconds.java b/src/main/java/frc/robot/commands/RequestShootingForSeconds.java
index 436710c..c9eaf87 100644
--- a/src/main/java/frc/robot/commands/RequestShootingForSeconds.java
+++ b/src/main/java/frc/robot/commands/RequestShootingForSeconds.java
@@ -41,6 +41,8 @@ public boolean isFinished() {
@Override
public void end(boolean interrupted) {
this.shooter.setActuateHoodAndLaunch(false);
- this.shooter.setDriverSpinUpFlywheel(false);
+ if (interrupted) {
+ this.shooter.setDriverSpinUpFlywheel(false);
+ }
}
}
diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
index 29fec7d..b4d8ecd 100644
--- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
@@ -26,7 +26,9 @@
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
@@ -65,6 +67,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
private double robotOmegaDegPerSec = 0.0;
+ private Field2d field = new Field2d();
/* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
@@ -268,6 +271,9 @@ public void periodic() {
this.robotOmegaDegPerSec = this.getState().Speeds.omegaRadiansPerSecond
* 180.0 / Math.PI;
+
+ this.field.setRobotPose(getPose());
+ SmartDashboard.putData(field);
}
private void startSimThread() {
diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
index 4faa1f2..9c55703 100644
--- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
+++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
@@ -39,21 +39,21 @@ public static List getCameras() {
)
);
- // registerCamera(
- // new Camera(
- // "right",
- // new Transform3d(
- // Inches.of(-1.5),
- // Inches.of(-13.75),
- // Inches.of(8.625),
- // new Rotation3d(
- // Degrees.of(0),
- // Degrees.of(-65), // negative pitch = tilted upward
- // Degrees.of(-90)
- // )
- // )
- // )
- // );
+ registerCamera(
+ new Camera(
+ "right",
+ new Transform3d(
+ Inches.of(-1.5),
+ Inches.of(-13.75),
+ Inches.of(8.625),
+ new Rotation3d(
+ Degrees.of(0),
+ Degrees.of(-65), // negative pitch = tilted upward
+ Degrees.of(-90)
+ )
+ )
+ )
+ );
registerCamera(
new Camera(
From 1308c8e8f93ea63d391840620a0d3146aed30bde Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Mon, 13 Apr 2026 16:10:59 -0400
Subject: [PATCH 05/14] auto path flipping
---
src/main/java/frc/robot/Robot.java | 11 +++++++++++
1 file changed, 11 insertions(+)
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a9c2711..d61dd9e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -24,6 +24,7 @@
import java.io.IOException;
import java.nio.file.Files;
import java.util.Arrays;
+
import org.littletonrobotics.junction.LogFileUtil;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
@@ -31,6 +32,8 @@
import org.littletonrobotics.junction.wpilog.WPILOGReader;
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
/**
* The VM is configured to automatically run this class, and to call the functions corresponding to
* each mode, as described in the TimedRobot documentation. If you change the name of this class or
@@ -105,6 +108,7 @@ public void robotInit() {
// Start AdvantageKit logger
Logger.start();
+ SmartDashboard.putBoolean("Auto/PathFlipped", false);
// Instantiate our RobotContainer. This will perform all our button bindings,
// and put our autonomous chooser on the dashboard.
@@ -184,6 +188,13 @@ public void disabledPeriodic() {}
@Override
public void autonomousInit() {
autonomousCommand = robotContainer.getAutonomousCommand();
+ boolean flipped = SmartDashboard.getBoolean("Auto/PathFlipped", false);
+
+ if (flipped) {
+ PathPlannerAuto auto = new PathPlannerAuto(autonomousCommand);
+ PathPlannerAuto flippedAuto = new PathPlannerAuto(auto.getName(), true);
+ autonomousCommand = flippedAuto;
+ }
// schedule the autonomous command (example)
if (autonomousCommand != null) {
From 6aeafaae5a30a0270687b3989e77ecdcd9c07e82 Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Tue, 14 Apr 2026 16:13:08 -0400
Subject: [PATCH 06/14] Clean up code
---
src/main/java/frc/lib/Elastic.java | 390 ------------------
src/main/java/frc/lib/FieldConstants.java | 3 +-
src/main/java/frc/lib/led/Color.java | 7 -
src/main/java/frc/lib/led/LEDStrip.java | 4 -
src/main/java/frc/robot/Robot.java | 2 +-
src/main/java/frc/robot/RobotContainer.java | 25 +-
.../java/frc/robot/commands/EmptyHopper.java | 9 -
.../frc/robot/commands/SetFlywheelState.java | 1 -
.../drive/CommandSwerveDrivetrain.java | 54 +--
.../robot/subsystems/findexer/Findexer.java | 1 -
.../frc/robot/subsystems/intake/Intake.java | 59 +--
.../frc/robot/subsystems/intake/IntakeIO.java | 4 -
.../subsystems/intake/IntakeIOTalonFX.java | 25 +-
.../frc/robot/subsystems/shooter/Shooter.java | 4 -
.../subsystems/shooter/ShooterIOTalonFX.java | 5 +-
.../robot/subsystems/targeting/Targeting.java | 15 +-
.../frc/robot/subsystems/trigger/Trigger.java | 18 -
.../subsystems/trigger/TriggerIOTalonFX.java | 3 +-
.../frc/robot/subsystems/turret/Turret.java | 10 -
.../subsystems/turret/TurretIOTalonFX.java | 27 +-
.../frc/robot/subsystems/vision/Vision.java | 1 -
.../frc/robot/subsystems/vision/VisionIO.java | 1 -
.../vision/VisionIOPhotonCamera.java | 5 +-
23 files changed, 21 insertions(+), 652 deletions(-)
delete mode 100644 src/main/java/frc/lib/Elastic.java
diff --git a/src/main/java/frc/lib/Elastic.java b/src/main/java/frc/lib/Elastic.java
deleted file mode 100644
index 8d0cc95..0000000
--- a/src/main/java/frc/lib/Elastic.java
+++ /dev/null
@@ -1,390 +0,0 @@
-// Copyright (c) 2023-2026 Gold87 and other Elastic contributors
-// This software can be modified and/or shared under the terms
-// defined by the Elastic license:
-// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE
-
-package frc.lib;
-
-import com.fasterxml.jackson.annotation.JsonProperty;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.PubSubOption;
-import edu.wpi.first.networktables.StringPublisher;
-import edu.wpi.first.networktables.StringTopic;
-
-public final class Elastic {
- private static final StringTopic notificationTopic =
- NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications");
- private static final StringPublisher notificationPublisher =
- notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true));
- private static final StringTopic selectedTabTopic =
- NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab");
- private static final StringPublisher selectedTabPublisher =
- selectedTabTopic.publish(PubSubOption.keepDuplicates(true));
- private static final ObjectMapper objectMapper = new ObjectMapper();
-
- /**
- * Represents the possible levels of notifications for the Elastic dashboard. These levels are
- * used to indicate the severity or type of notification.
- */
- public enum NotificationLevel {
- /** Informational Message */
- INFO,
- /** Warning message */
- WARNING,
- /** Error message */
- ERROR
- }
-
- /**
- * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string
- * before being published.
- *
- * @param notification the {@link Notification} object containing notification details
- */
- public static void sendNotification(Notification notification) {
- try {
- notificationPublisher.set(objectMapper.writeValueAsString(notification));
- } catch (JsonProcessingException e) {
- e.printStackTrace();
- }
- }
-
- /**
- * Selects the tab of the dashboard with the given name. If no tab matches the name, this will
- * have no effect on the widgets or tabs in view.
- *
- * If the given name is a number, Elastic will select the tab whose index equals the number
- * provided.
- *
- * @param tabName the name of the tab to select
- */
- public static void selectTab(String tabName) {
- selectedTabPublisher.set(tabName);
- }
-
- /**
- * Selects the tab of the dashboard at the given index. If this index is greater than or equal to
- * the number of tabs, this will have no effect.
- *
- * @param tabIndex the index of the tab to select.
- */
- public static void selectTab(int tabIndex) {
- selectTab(Integer.toString(tabIndex));
- }
-
- /**
- * Represents an notification object to be sent to the Elastic dashboard. This object holds
- * properties such as level, title, description, display time, and dimensions to control how the
- * notification is displayed on the dashboard.
- */
- public static class Notification {
- @JsonProperty("level")
- private NotificationLevel level;
-
- @JsonProperty("title")
- private String title;
-
- @JsonProperty("description")
- private String description;
-
- @JsonProperty("displayTime")
- private int displayTimeMillis;
-
- @JsonProperty("width")
- private double width;
-
- @JsonProperty("height")
- private double height;
-
- /**
- * Creates a new Notification with all default parameters. This constructor is intended to be
- * used with the chainable decorator methods
- *
- *
Title and description fields are empty.
- */
- public Notification() {
- this(NotificationLevel.INFO, "", "");
- }
-
- /**
- * Creates a new Notification with all properties specified.
- *
- * @param level the level of the notification (e.g., INFO, WARNING, ERROR)
- * @param title the title text of the notification
- * @param description the descriptive text of the notification
- * @param displayTimeMillis the time in milliseconds for which the notification is displayed
- * @param width the width of the notification display area
- * @param height the height of the notification display area, inferred if below zero
- */
- public Notification(
- NotificationLevel level,
- String title,
- String description,
- int displayTimeMillis,
- double width,
- double height) {
- this.level = level;
- this.title = title;
- this.displayTimeMillis = displayTimeMillis;
- this.description = description;
- this.height = height;
- this.width = width;
- }
-
- /**
- * Creates a new Notification with default display time and dimensions.
- *
- * @param level the level of the notification
- * @param title the title text of the notification
- * @param description the descriptive text of the notification
- */
- public Notification(NotificationLevel level, String title, String description) {
- this(level, title, description, 3000, 350, -1);
- }
-
- /**
- * Creates a new Notification with a specified display time and default dimensions.
- *
- * @param level the level of the notification
- * @param title the title text of the notification
- * @param description the descriptive text of the notification
- * @param displayTimeMillis the display time in milliseconds
- */
- public Notification(
- NotificationLevel level, String title, String description, int displayTimeMillis) {
- this(level, title, description, displayTimeMillis, 350, -1);
- }
-
- /**
- * Creates a new Notification with specified dimensions and default display time. If the height
- * is below zero, it is automatically inferred based on screen size.
- *
- * @param level the level of the notification
- * @param title the title text of the notification
- * @param description the descriptive text of the notification
- * @param width the width of the notification display area
- * @param height the height of the notification display area, inferred if below zero
- */
- public Notification(
- NotificationLevel level, String title, String description, double width, double height) {
- this(level, title, description, 3000, width, height);
- }
-
- /**
- * Updates the level of this notification
- *
- * @param level the level to set the notification to
- */
- public void setLevel(NotificationLevel level) {
- this.level = level;
- }
-
- /**
- * @return the level of this notification
- */
- public NotificationLevel getLevel() {
- return level;
- }
-
- /**
- * Updates the title of this notification
- *
- * @param title the title to set the notification to
- */
- public void setTitle(String title) {
- this.title = title;
- }
-
- /**
- * Gets the title of this notification
- *
- * @return the title of this notification
- */
- public String getTitle() {
- return title;
- }
-
- /**
- * Updates the description of this notification
- *
- * @param description the description to set the notification to
- */
- public void setDescription(String description) {
- this.description = description;
- }
-
- public String getDescription() {
- return description;
- }
-
- /**
- * Updates the display time of the notification
- *
- * @param seconds the number of seconds to display the notification for
- */
- public void setDisplayTimeSeconds(double seconds) {
- setDisplayTimeMillis((int) Math.round(seconds * 1000));
- }
-
- /**
- * Updates the display time of the notification in milliseconds
- *
- * @param displayTimeMillis the number of milliseconds to display the notification for
- */
- public void setDisplayTimeMillis(int displayTimeMillis) {
- this.displayTimeMillis = displayTimeMillis;
- }
-
- /**
- * Gets the display time of the notification in milliseconds
- *
- * @return the number of milliseconds the notification is displayed for
- */
- public int getDisplayTimeMillis() {
- return displayTimeMillis;
- }
-
- /**
- * Updates the width of the notification
- *
- * @param width the width to set the notification to
- */
- public void setWidth(double width) {
- this.width = width;
- }
-
- /**
- * Gets the width of the notification
- *
- * @return the width of the notification
- */
- public double getWidth() {
- return width;
- }
-
- /**
- * Updates the height of the notification
- *
- *
If the height is set to -1, the height will be determined automatically by the dashboard
- *
- * @param height the height to set the notification to
- */
- public void setHeight(double height) {
- this.height = height;
- }
-
- /**
- * Gets the height of the notification
- *
- * @return the height of the notification
- */
- public double getHeight() {
- return height;
- }
-
- /**
- * Modifies the notification's level and returns itself to allow for method chaining
- *
- * @param level the level to set the notification to
- * @return the current notification
- */
- public Notification withLevel(NotificationLevel level) {
- this.level = level;
- return this;
- }
-
- /**
- * Modifies the notification's title and returns itself to allow for method chaining
- *
- * @param title the title to set the notification to
- * @return the current notification
- */
- public Notification withTitle(String title) {
- setTitle(title);
- return this;
- }
-
- /**
- * Modifies the notification's description and returns itself to allow for method chaining
- *
- * @param description the description to set the notification to
- * @return the current notification
- */
- public Notification withDescription(String description) {
- setDescription(description);
- return this;
- }
-
- /**
- * Modifies the notification's display time and returns itself to allow for method chaining
- *
- * @param seconds the number of seconds to display the notification for
- * @return the current notification
- */
- public Notification withDisplaySeconds(double seconds) {
- return withDisplayMilliseconds((int) Math.round(seconds * 1000));
- }
-
- /**
- * Modifies the notification's display time and returns itself to allow for method chaining
- *
- * @param displayTimeMillis the number of milliseconds to display the notification for
- * @return the current notification
- */
- public Notification withDisplayMilliseconds(int displayTimeMillis) {
- setDisplayTimeMillis(displayTimeMillis);
- return this;
- }
-
- /**
- * Modifies the notification's width and returns itself to allow for method chaining
- *
- * @param width the width to set the notification to
- * @return the current notification
- */
- public Notification withWidth(double width) {
- setWidth(width);
- return this;
- }
-
- /**
- * Modifies the notification's height and returns itself to allow for method chaining
- *
- * @param height the height to set the notification to
- * @return the current notification
- */
- public Notification withHeight(double height) {
- setHeight(height);
- return this;
- }
-
- /**
- * Modifies the notification's height and returns itself to allow for method chaining
- *
- *
This will set the height to -1 to have it automatically determined by the dashboard
- *
- * @return the current notification
- */
- public Notification withAutomaticHeight() {
- setHeight(-1);
- return this;
- }
-
- /**
- * Modifies the notification to disable the auto dismiss behavior
- *
- *
This sets the display time to 0 milliseconds
- *
- *
The auto dismiss behavior can be re-enabled by setting the display time to a number
- * greater than 0
- *
- * @return the current notification
- */
- public Notification withNoAutoDismiss() {
- setDisplayTimeMillis(0);
- return this;
- }
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/FieldConstants.java b/src/main/java/frc/lib/FieldConstants.java
index 37158be..55dc7f5 100644
--- a/src/main/java/frc/lib/FieldConstants.java
+++ b/src/main/java/frc/lib/FieldConstants.java
@@ -14,7 +14,6 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.Filesystem;
import java.io.IOException;
/**
@@ -305,6 +304,7 @@ public static class Outpost {
new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY());
}
+ @SuppressWarnings("unused")
public enum FieldType {
ANDYMARK("andymark"),
WELDED("welded");
@@ -316,6 +316,7 @@ public enum FieldType {
}
}
+ @SuppressWarnings("unused")
public enum AprilTagLayoutType {
OFFICIAL("2026-official"),
NONE("2026-none");
diff --git a/src/main/java/frc/lib/led/Color.java b/src/main/java/frc/lib/led/Color.java
index b5adba8..05d3fb4 100644
--- a/src/main/java/frc/lib/led/Color.java
+++ b/src/main/java/frc/lib/led/Color.java
@@ -36,7 +36,6 @@ public Color(int hue, int saturation, int value) {
* @param green The green value of the color [0-255].
* @param blue The blue value of the color [0-255].
*/
- @SuppressWarnings("unused")
public static Color fromRGB(int red, int green, int blue) {
HSV hsv = RGBtoHSV(red, green, blue);
return new Color(hsv.hue(), hsv.saturation(), hsv.value());
@@ -46,7 +45,6 @@ public static Color fromRGB(int red, int green, int blue) {
* Creates a new color from a hex string.
* @param hex The hex string to convert (without #).
*/
- @SuppressWarnings("unused")
public static Color fromHex(String hex) throws InvalidColorException {
if (hex.length() != 6) {
throw new InvalidColorException("Hex string must be 6 characters long.");
@@ -63,7 +61,6 @@ public static Color fromHex(String hex) throws InvalidColorException {
* Gets the RGB values of the color.
* @return The RGB values of the color.
*/
- @SuppressWarnings("unused")
public RGB getRGB() {
return rgb;
}
@@ -72,7 +69,6 @@ public RGB getRGB() {
* Gets the HSV values of the color.
* @return The HSV values of the color.
*/
- @SuppressWarnings("unused")
public HSV getHSV() {
return hsv;
}
@@ -187,7 +183,6 @@ public RGB(int red, int green, int blue) {
* Gets the red value of the color.
* @return The red value of the color [0-255].
*/
- @SuppressWarnings("unused")
public int red() {
return red;
}
@@ -196,7 +191,6 @@ public int red() {
* Gets the green value of the color.
* @return The green value of the color [0-255].
*/
- @SuppressWarnings("unused")
public int green() {
return green;
}
@@ -205,7 +199,6 @@ public int green() {
* Gets the blue value of the color.
* @return The blue value of the color [0-255].
*/
- @SuppressWarnings("unused")
public int blue() {
return blue;
}
diff --git a/src/main/java/frc/lib/led/LEDStrip.java b/src/main/java/frc/lib/led/LEDStrip.java
index 6e502ac..dc2edee 100644
--- a/src/main/java/frc/lib/led/LEDStrip.java
+++ b/src/main/java/frc/lib/led/LEDStrip.java
@@ -35,7 +35,6 @@ public LEDStrip(int length, int offset) {
* Sets the color of the strip.
* @param color The color to set the strip to.
*/
- @SuppressWarnings("unused")
public void setPrimaryColor(Color color) {
this.primaryColor = color;
}
@@ -44,7 +43,6 @@ public void setPrimaryColor(Color color) {
* Sets the secondary of the strip.
* @param color The color to set the strip to.
*/
- @SuppressWarnings("unused")
public void setSecondaryColor(Color color) {
this.secondaryColor = color;
}
@@ -54,7 +52,6 @@ public void setSecondaryColor(Color color) {
* Sets the pattern of the strip.
* @param pattern The pattern to set the strip to.
*/
- @SuppressWarnings("unused")
public void setPattern(LEDPattern pattern) {
this.pattern = pattern;
}
@@ -63,7 +60,6 @@ public void setPattern(LEDPattern pattern) {
* Sets the time it takes for the pattern to loop.
* @param patternDuration The pattern duration to set the strip to (in s).
*/
- @SuppressWarnings("unused")
public void setPatternDuration(double patternDuration) {
this.patternDuration = patternDuration;
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d800e44..f9a22f6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -83,7 +83,7 @@ public void robotInit() {
switch (Constants.currentMode) {
case REAL:
// Running on a real robot, log to a USB stick ("/U/logs")
- // Logger.addDataReceiver(new WPILOGWriter(LOG_DIRECTORY));
+ Logger.addDataReceiver(new WPILOGWriter(LOG_DIRECTORY));
Logger.addDataReceiver(new NT4Publisher());
break;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ffdb6de..49a5ae3 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -80,7 +80,7 @@ public class RobotContainer {
private static PWMLEDController ledController;
public RobotContainer() {
- ledController = new PWMLEDController(0, LED_MODE);
+ ledController = new PWMLEDController(1, LED_MODE);
configureAddressableStrips();
configureLEDPresets();
ledController.start();
@@ -88,9 +88,9 @@ public RobotContainer() {
drive = TunerConstants.createDrivetrain();
this.turret = new Turret();
this.vision = new Vision(drive);
- this.intake = new Intake(drive);
+ this.intake = new Intake();
this.shooter = new Shooter();
- this.targeting = new Targeting(drive);
+ this.targeting = new Targeting();
this.trigger = new Trigger(shooter, turret);
this.findexer = new Findexer(trigger);
@@ -279,25 +279,6 @@ else if (driverController.x().getAsBoolean()) { // Left
drive.applyRequest(() -> idle).ignoringDisable(true));
driverController.leftTrigger().whileTrue(drive.applyRequest(() -> brake));
- // driverController.rightTrigger().whileTrue(drive.applyRequest(() -> point
- // .withModuleDirection(new Rotation2d(-driverController.getLeftY(), -driverController.getLeftX()))));
-
- // Run SysId routines when holding back/start and X/Y.
- // Note that each routine should be run exactly once in a single log.
- driverController.back().and(driverController.y()).whileTrue(drive.sysIdDynamic(Direction.kForward));
- driverController.back().and(driverController.x()).whileTrue(drive.sysIdDynamic(Direction.kReverse));
- driverController.start().and(driverController.y()).whileTrue(drive.sysIdQuasistatic(Direction.kForward));
- driverController.start().and(driverController.x()).whileTrue(drive.sysIdQuasistatic(Direction.kReverse));
-
- // reset the field-centric heading on left bumper press
-
- // operatorController.y().onTrue(vision.runOnce(() -> {
- // var estimatedPose = vision.getEstimatedPositionFromCameras();
- // if (estimatedPose != null) {
- // Logger.recordOutput("Vision/SnapshotEstimate", estimatedPose);
- // drive.resetPose(estimatedPose);
- // }
- // }));
}
private void setupIntakeBindings() {
diff --git a/src/main/java/frc/robot/commands/EmptyHopper.java b/src/main/java/frc/robot/commands/EmptyHopper.java
index 591bf50..aeb9b02 100644
--- a/src/main/java/frc/robot/commands/EmptyHopper.java
+++ b/src/main/java/frc/robot/commands/EmptyHopper.java
@@ -1,10 +1,7 @@
package frc.robot.commands;
-import org.littletonrobotics.junction.Logger;
-
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.targeting.Targeting;
import frc.robot.subsystems.targeting.Targeting.Target;
@@ -22,11 +19,6 @@ public EmptyHopper(Shooter shooter, Targeting targeting, double forTime, boolean
this.scoringTime = forTime;
- // if (targetHub) {
- // this.targeting.setTargetingHub();
- // } else {
- // this.targeting.setTargetingShuttleRight(); // TODO: Allow for left selection as well
- // }
this.targeting.setTarget(Target.HUB);
addRequirements(shooter, targeting);
@@ -56,7 +48,6 @@ public boolean isFinished() {
@Override
public void end(boolean interrupted) {
- // TODO Auto-generated method stub
this.shooter.setDriverSpinUpFlywheel(false);
this.shooter.setActuateHoodAndLaunch(false);
}
diff --git a/src/main/java/frc/robot/commands/SetFlywheelState.java b/src/main/java/frc/robot/commands/SetFlywheelState.java
index a1cef2b..b166b3e 100644
--- a/src/main/java/frc/robot/commands/SetFlywheelState.java
+++ b/src/main/java/frc/robot/commands/SetFlywheelState.java
@@ -1,7 +1,6 @@
package frc.robot.commands;
import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.intake.Intake;
import frc.robot.subsystems.shooter.Shooter;
public class SetFlywheelState extends Command {
diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
index b4d8ecd..a814f3b 100644
--- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
@@ -63,8 +63,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
/* Swerve requests to apply during SysId characterization */
private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
- private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
- private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
private double robotOmegaDegPerSec = 0.0;
private Field2d field = new Field2d();
@@ -85,49 +83,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
)
);
- /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
- private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
- new SysIdRoutine.Config(
- null, // Use default ramp rate (1 V/s)
- Volts.of(7), // Use dynamic voltage of 7 V
- null, // Use default timeout (10 s)
- // Log state with SignalLogger class
- state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
- ),
- new SysIdRoutine.Mechanism(
- volts -> setControl(m_steerCharacterization.withVolts(volts)),
- null,
- this
- )
- );
-
- /*
- * SysId routine for characterizing rotation.
- * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
- * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
- */
- private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
- new SysIdRoutine.Config(
- /* This is in radians per second^2, but SysId only supports "volts per second" */
- Volts.of(Math.PI / 6).per(Second),
- /* This is in radians per second, but SysId only supports "volts" */
- Volts.of(Math.PI),
- null, // Use default timeout (10 s)
- // Log state with SignalLogger class
- state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
- ),
- new SysIdRoutine.Mechanism(
- output -> {
- /* output is actually radians per second, but SysId only supports "volts" */
- setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
- /* also log the requested output for SysId */
- SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
- },
- null,
- this
- )
- );
-
/* The SysId routine to test */
private final SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
@@ -279,8 +234,8 @@ public void periodic() {
private void startSimThread() {
m_lastSimTime = Utils.getCurrentTimeSeconds();
- /* Run simulation at a faster rate so PID gains behave more reasonably */
- /* use the measured time delta, get battery voltage from WPILib */
+ try (/* Run simulation at a faster rate so PID gains behave more reasonably */
+ /* use the measured time delta, get battery voltage from WPILib */
Notifier m_simNotifier = new Notifier(() -> {
final double currentTime = Utils.getCurrentTimeSeconds();
double deltaTime = currentTime - m_lastSimTime;
@@ -288,8 +243,9 @@ private void startSimThread() {
/* use the measured time delta, get battery voltage from WPILib */
updateSimState(deltaTime, RobotController.getBatteryVoltage());
- });
- m_simNotifier.startPeriodic(kSimLoopPeriod);
+ })) {
+ m_simNotifier.startPeriodic(kSimLoopPeriod);
+ }
}
/**
diff --git a/src/main/java/frc/robot/subsystems/findexer/Findexer.java b/src/main/java/frc/robot/subsystems/findexer/Findexer.java
index 7f4f485..7197e4d 100644
--- a/src/main/java/frc/robot/subsystems/findexer/Findexer.java
+++ b/src/main/java/frc/robot/subsystems/findexer/Findexer.java
@@ -1,6 +1,5 @@
package frc.robot.subsystems.findexer;
-import edu.wpi.first.wpilibj.Timer;
import frc.lib.subsystem.SpikeSystem;
import frc.robot.subsystems.trigger.Trigger;
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
index ca40a3a..f603365 100644
--- a/src/main/java/frc/robot/subsystems/intake/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -1,33 +1,17 @@
package frc.robot.subsystems.intake;
-import edu.wpi.first.wpilibj.DriverStation;
import frc.lib.subsystem.SpikeSystem;
-import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
public class Intake extends SpikeSystem {
- private static final double DEPLOY_SPEED = 1.0; // Speed in Rotations Per Second to deploy the intake
- private static final double RETRACT_SPEED = -1.0; // Speed in Rotations Per Second to retract the intake
- private static final double DEPLOY_CURRENT_THRESHOLD = 10.0; // Amp limit for the deploy motor. Watches
- // for a resistance to the motor to see when it's
- // deployed
- private static final double BASE_SPEED_INTAKE = 1; // Rotation Per Second
- private static final double SPEED_PER_MPS = 0.05; // Speed added to the base speed per m/s of drive velocity
- private static final double MAX_SPEED = 5.0; // speed cap, max speed of the intake in RPS
-
- public enum IntakeState {
- DEPLOYED, RETRACTED, DEPLOYING, RETRACTING
- }
private IntakeIO intakeIO;
- private final CommandSwerveDrivetrain drivetrain;
private boolean running = false; // True if the intake is running, False otherwise
private boolean forward = true;
// Intake constructor
- public Intake(CommandSwerveDrivetrain drivetrain) {
+ public Intake() {
super("Intake", new IntakeIOInputsAutoLogged());
- this.drivetrain = drivetrain;
}
// Intake periodic function
@@ -44,17 +28,6 @@ public void onPeriodic() {
// Run the DEPLOYED state periodic actions
private void doDeployedState() {
- // // Get the absolute velocity of the entire robot
- // double robotAbsoluteVelocity = Math.abs(drivetrain.getState().Speeds.vxMetersPerSecond);
-
- // // Adjust the intake speed, increase the intake as the robot moves faster
- // // Cap at MAX_SPEED
- // double speed = Math.min(BASE_SPEED_INTAKE + robotAbsoluteVelocity * SPEED_PER_MPS, MAX_SPEED);
-
- // if (forward == false) {
- // speed *= -1;
- // }
-
// Update the intakeIO on speed
if (forward == false) {
intakeIO.setIntakeSpeed(-70);
@@ -79,10 +52,6 @@ public void switchDirection() {
forward = !forward;
}
- public void toggle() {
- running = !running;
- }
-
// Disables the intake
public void disable() {
running = false;
@@ -91,25 +60,6 @@ public void disable() {
intakeIO.setIntakeSpeed(0.0);
}
- // Deploys the over the bumper intake
- public void deploy() {
- if (intakeIO.getIntakeState() != IntakeState.DEPLOYED) { // Only try to deploy if we aren't already deployed
- intakeIO.setIntakeState(IntakeState.DEPLOYING); // Mark as deploying
- }
- }
-
- // Retracts the intake back to its stored position
- public void retract() {
- if (intakeIO.getIntakeState() != IntakeState.RETRACTED) { // Only try to retract if we aren't already retracted
- intakeIO.setIntakeState(IntakeState.RETRACTING); // Mark as retracting
- }
- }
-
- // Returns true if the intake is fully deployed, false otherwise
- public boolean isDeployed() {
- return intakeIO.getIntakeState() == IntakeState.DEPLOYED;
- }
-
@Override
protected Runnable setupDataRefresher() {
this.intakeIO = new IntakeIOTalonFX();
@@ -117,13 +67,8 @@ protected Runnable setupDataRefresher() {
}
// Toggles the intake between deployed and retracted states
- public void toggleIntake() {
+ public void toggle() {
this.running = !this.running;
- // if (intakeIO.getIntakeState() == IntakeState.DEPLOYED || intakeIO.getIntakeState() == IntakeState.DEPLOYING) {
- // retract();
- // } else {
- // deploy();
- // }
}
public void setDeployServo(double position) {
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index fd82ad0..113ec5a 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -3,7 +3,6 @@
import frc.lib.subsystem.BaseIO;
import frc.lib.subsystem.BaseInputClass;
import frc.lib.subsystem.IORefresher;
-import frc.robot.subsystems.intake.Intake.IntakeState;
import org.littletonrobotics.junction.AutoLog;
@@ -14,11 +13,8 @@ class IntakeIOInputs extends BaseInputClass {
public double intakeCurrentAmps = 0.0; // Intake Current in Amps
public double deployVelocityRPS = 0.0; // Intake deploy motor Rotations Per Second
public double deployCurrentAmps = 0.0; // Intake deploy motor Current in Amps
- public IntakeState intakeState = IntakeState.RETRACTED; // current intake state
}
void setIntakeSpeed(double speed);
- IntakeState getIntakeState();
- void setIntakeState(IntakeState newState);
void setDeployServo(double position);
}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
index c370dad..47114d0 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFX.java
@@ -1,6 +1,7 @@
package frc.robot.subsystems.intake;
import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.VelocityVoltage;
@@ -8,16 +9,12 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.wpilibj.Servo;
-import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
-import frc.lib.subsystem.IORefresher;
import frc.robot.CanID;
-import frc.robot.subsystems.intake.Intake.IntakeState;
-public class IntakeIOTalonFX implements IntakeIO, IORefresher {
+public class IntakeIOTalonFX implements IntakeIO {
// TalonFX Motors
private final TalonFX intakeMotor;
private final Servo deployServo;
@@ -28,12 +25,9 @@ public class IntakeIOTalonFX implements IntakeIO, IORefresher {
private final StatusSignal intakeVelocity;
private final StatusSignal intakeCurrent;
- // Inputs for logging
- private IntakeIOInputs intakeIO;
-
// IntakeIOTalonFX constructor
public IntakeIOTalonFX() {
- intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), "Canivore_Drivetrain"); // Setup the intake motor with the CAN ID
+ intakeMotor = new TalonFX(CanID.INTAKE_MOTOR.getID(), new CANBus("Canivore_Drivetrain")); // Setup the intake motor with the CAN ID
deployServo = new Servo(CanID.INTAKE_DEPLOY_SERVO.getID());
// Configure motors
@@ -56,7 +50,6 @@ public void refreshData() {
public void updateInputs(IntakeIOInputs inputs) {
inputs.intakeVelocityRPS = intakeVelocity.getValueAsDouble();
inputs.intakeCurrentAmps = intakeCurrent.getValueAsDouble();
- intakeIO = inputs;
}
// Set the speed of the intake motor
@@ -73,18 +66,6 @@ public void setIntakeSpeed(double speed) {
intakeMotor.setControl(this.velocityControl);
}
- // Return the state of the Intake
- @Override
- public IntakeState getIntakeState() {
- return intakeIO.intakeState;
- }
-
- // Set the state of the Intake
- @Override
- public void setIntakeState(IntakeState newState) {
- intakeIO.intakeState = newState;
- }
-
public static TalonFXConfiguration getIntakeMotorConfig() {
var intakeConfig = new TalonFXConfiguration();
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index ad23e0e..67685e6 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,7 +4,6 @@
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.subsystem.SpikeSystem;
import frc.robot.subsystems.targeting.ShotData;
@@ -32,11 +31,8 @@ public Shooter() {
*/
@Override
public void onPeriodic() {
-
double distToTarget = getDistanceToTarget(); // distance in meters
readFromData = SmartDashboard.getBoolean("ReadFromData", true);
- // double targetRPM = ShotData.distanceToRPM.get(distToTarget);
- // double hoodAngle = ShotData.distanceToHoodAngle.get(distToTarget);
double targetRPM = 0;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java
index 0e58939..1966fe4 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java
@@ -22,7 +22,6 @@
public class ShooterIOTalonFX implements ShooterIO {
private static final double HOOD_ZERO_CURRENT = 1.75; // amps at which we consider the hood to have hit a limit
- private static final double FLYWHEEL_SETPOINT_UPDATE_DEADBAND_RPS = 0.35; // ignore tiny target changes
private final TalonFX flywheelMotor;
private final TalonFXS hoodMotor;
@@ -41,7 +40,6 @@ public class ShooterIOTalonFX implements ShooterIO {
private double hoodAngleSetPoint = 0.0;
private double flywheelRPSSetPoint = 0.0;
- private double lastAppliedFlywheelRPSSetPoint = Double.NaN;
private double hoodTargetEncoder = 0.0;
private boolean isZeroing = true;
@@ -190,9 +188,8 @@ public void setFlywheelVelocity(double rps) {
this.flywheelControl.withVelocity(rps);
flywheelMotor.setControl(this.flywheelControl);
}
-
- this.lastAppliedFlywheelRPSSetPoint = rps;
}
+
/**
* Set the target hood angle.
* @param angle target angle
diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
index 9ad74d7..f4f5d55 100644
--- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
@@ -13,15 +13,12 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.lib.FieldConstants;
import frc.robot.RobotContainer;
-import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
import frc.robot.subsystems.turret.Turret;
public class Targeting extends SubsystemBase {
- private static final double NOMINAL_SHOT_TIME_S = 0.3; // see github issue #23 (https://github.com/Team293/Rebuilt/issues/23)
private static ShotCompensation.AdjustedShot shotData = new ShotCompensation.AdjustedShot(0.0, 0.0, 0.0, 0.0, 0.0);
private static Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d();
- private final CommandSwerveDrivetrain drive;
private static final double FIELD_WIDTH = 8.07; // meters
private static final double FIELD_LENGTH = 16.54; // meters
@@ -46,8 +43,7 @@ public static enum Target {
private boolean overrideRedAlliance = false;
private boolean overrideBlueAlliance = false;
- public Targeting(CommandSwerveDrivetrain drive) {
- this.drive = drive;
+ public Targeting() {
Logger.recordOutput("HubTarget", FieldConstants.Hub.oppTopCenterPoint);
Logger.recordOutput("ShuttleTarget", new Pose2d(0, 0, new Rotation2d()));
@@ -142,15 +138,6 @@ private void setPoseTargetingHub() {
overrideBlueAlliance = SmartDashboard.getBoolean("OverrideBlueAlliance", overrideBlueAlliance);
overrideRedAlliance = SmartDashboard.getBoolean("OverrideRedAlliance", overrideRedAlliance);
- // if (!DriverStation.getAlliance().isPresent() && isRedAlliance) {
- // if (isRedAlliance) {
- // targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d();
- // } else {
- // targetPos = FieldConstants.Hub.innerCenterPoint.toTranslation2d();
- // }
- // return;
- // }
-
if (overrideRedAlliance) {
targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d();
return;
diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java
index 7baeea7..8949e1d 100644
--- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java
+++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java
@@ -1,6 +1,5 @@
package frc.robot.subsystems.trigger;
-import edu.wpi.first.wpilibj.DriverStation;
import frc.lib.subsystem.SpikeSystem;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.turret.Turret;
@@ -33,24 +32,12 @@ public void onPeriodic() {
// run the indexer if the mechanisms are ready for balls
// run it regardless of ball in indexer, so that it can feed a ball in if there is one queued up
triggerIO.setSpeed(TRIGGER_SPEED);
- // } else if (needsFeeding()) {
- // // bring the ball to the indexer and stop once we see a ball
- // triggerIO.setSpeed(TRIGGER_SPEED);
} else {
// stop the indexer if the mechanisms aren't ready and we have a ball queued
triggerIO.setSpeed(0.0);
}
}
- /**
- * Checks the proximity sensor to see if there is a ball currently queued up in the indexer.
- * @return true if there is a ball in the indexer, false otherwise
- */
- private boolean hasBallQueued() {
- // return super.io.proximitySensor;
- return false;
- }
-
public void setReverseTrigger(boolean reverse) {
this.reverseTrigger = reverse;
}
@@ -68,11 +55,6 @@ private boolean mechanismReadyForBalls() {
// check if turret is at target angle
boolean turretReady = turret.isAtTargetAngle();
-
- // if (DriverStation.isAutonomous() && turretReady) {
- // turretReady = shooter.isAtTargetRPS();
- // }
-
if (!turretReady) {
return false;
}
diff --git a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java
index 5ce7990..06c8470 100644
--- a/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/trigger/TriggerIOTalonFX.java
@@ -3,10 +3,9 @@
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj.DigitalInput;
-import frc.lib.subsystem.IORefresher;
import frc.robot.CanID;
-public class TriggerIOTalonFX implements IORefresher, TriggerIO {
+public class TriggerIOTalonFX implements TriggerIO {
private final TalonFX motor; // Motor object
private final BaseStatusSignal motorRps; // Rotations per second
private final DigitalInput proximitySensor;
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8306c15..b60436a 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -4,14 +4,12 @@
import frc.lib.subsystem.SpikeSystem;
import frc.robot.RobotContainer;
import frc.robot.subsystems.targeting.Targeting;
-import frc.robot.subsystems.targeting.ShotCompensation;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.DriverStation;
public class Turret extends SpikeSystem {
public static final double TURRET_AIMING_TOLERANCE_DEGREES = 5.0; // degrees within which we consider the turret to be aimed at the target (+-)
@@ -31,7 +29,6 @@ public class Turret extends SpikeSystem {
public static final double ENCODER_COMBINED_PERIOD_TURRET_REV =
ENCODER_COMBINED_PERIOD_REV * (PINION_ENCODER_TEETH / TURRET_GEAR_TEETH);
-
public static final double TURRET_CENTER_OFFSET_DEG = -88.1; // subtracted from robot relative heading
public static final double TURRET_ROBOT_OFFSET_DEG = 51.8; // subtracted from robot relative heading to get turret relative heading
@@ -48,13 +45,6 @@ public Turret() {
@Override
public void onPeriodic() {
Logger.recordOutput("Turret/TurretCenterOffset", new Pose2d(TURRET_OFFSET_FROM_CENTER, RobotContainer.getDrive().getRotation()));
- // turretIO.setTurretAngleFieldRelativeDegrees(0);
-
- // if (shotData != null) {
- // double newTargetAngleDeg = shotData.turretAngleDeg();
-
- // this.turretIO.setTurretAngleFieldRelativeDegrees(newTargetAngleDeg);
- // }
if (this.overrideAutomaticAiming) {
this.turretIO.setTurretAngleRobotRelativeDegrees(0);
diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
index 522072a..b070052 100644
--- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
@@ -1,14 +1,11 @@
package frc.robot.subsystems.turret;
import org.littletonrobotics.junction.AutoLogOutput;
-import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.*;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
@@ -16,17 +13,11 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.measure.Angle;
-import frc.lib.LowPassFilter;
import frc.robot.CanID;
import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
public class TurretIOTalonFX implements TurretIO {
- // KS KV CONSTANTS
- private static final double kS = 0.35; // volts needed to overcome static friction
- private static final double kV = 0.20; // volts per (rotation per second) to maintain motion
-
// SUBSYSTEMS
private final CommandSwerveDrivetrain drive;
@@ -48,7 +39,6 @@ public class TurretIOTalonFX implements TurretIO {
private double lastTurretAngleDegrees = 0.0; // last calculated angle of the turret in degrees, used for calculating angular velocity
// VALUES
- private double targetTurretDegreesFieldRelative; // target angle of the turret in degrees, relative to the field
private double processedTargetTurretDegreesFieldRelative; // processed target angle of the turret in degrees, relative to the field
private double targetTurretAngleMotorRevs; // target angle of the turret in motor rotations
private double calculatedMotorOffsetRevs; // calculated offset in motor rotations based on the current position of the turret and the pinion encoder reading
@@ -56,7 +46,6 @@ public class TurretIOTalonFX implements TurretIO {
// COMMANDS
private final PositionTorqueCurrentFOC mmRequest = new PositionTorqueCurrentFOC(0.0);
- private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(kS, kV); // ks, kv
private double turretTrimDegrees = 0.0;
@@ -103,7 +92,6 @@ public TurretIOTalonFX(CommandSwerveDrivetrain drive) {
@Override
public void setTurretAngleFieldRelativeDegrees(double fieldRelativeAngleDegrees) {
- this.targetTurretDegreesFieldRelative = fieldRelativeAngleDegrees;
double currentRobotHeading = this.drive.getPose().getRotation().getDegrees();
// absolute robot-relative target, in motor rotations
@@ -128,7 +116,7 @@ private void setTurretAngleTurretRelativeDegrees(double angleDegrees) {
mmRequest.Position = targetMotorRotations;
this.turretMotor.setControl(
- mmRequest
+ mmRequest // .withFeedForward(drive.getState().Speeds.omegaRadiansPerSecond)
);
}
@@ -143,19 +131,6 @@ public void recalculateTurretMotorZeroPosition() {
turretMotor.setPosition(this.calculatedMotorOffsetRevs);
}
- /**
- * Calculates the feedforward voltage to apply to the turret motor to counteract the rotation of the robot, based on the current angular velocity of the robot.
- * @return the feedforward value to apply to the turret rotation
- */
- private double calculateFeedforward() {
- // get the current angular velocity of the robot in radians per second
- double gyroOmegaRadPerSecond = drive.getState().Speeds.omegaRadiansPerSecond;
-
- double mechanismRotationsPerSecond = gyroOmegaRadPerSecond / Math.PI;
- return feedforward.calculate(-mechanismRotationsPerSecond);
- }
-
-
@Override
public void refreshData() {
StatusSignal.refreshAll(this.turretMotorPosition, this.pinionEncoderSignal, this.followerEncoderSignal);
diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java
index 403fcae..e362896 100644
--- a/src/main/java/frc/robot/subsystems/vision/Vision.java
+++ b/src/main/java/frc/robot/subsystems/vision/Vision.java
@@ -5,7 +5,6 @@
import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
import frc.robot.subsystems.vision.VisionIO.VisionIOInputs;
-import org.littletonrobotics.junction.Logger;
import org.photonvision.EstimatedRobotPose;
import edu.wpi.first.math.geometry.Pose2d;
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
index 2d6f9f2..8e4c347 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java
@@ -1,6 +1,5 @@
package frc.robot.subsystems.vision;
-import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import frc.lib.subsystem.BaseIO;
import frc.lib.subsystem.BaseInputClass;
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java
index d95cc97..62f61bf 100644
--- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java
+++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonCamera.java
@@ -2,18 +2,15 @@
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
-import frc.lib.subsystem.IORefresher;
-import frc.robot.subsystems.vision.photon.Camera;
import frc.robot.subsystems.vision.photon.CameraManager;
import org.photonvision.EstimatedRobotPose;
import java.util.ArrayList;
-import java.util.Collections;
import java.util.List;
import java.util.Objects;
import java.util.function.Supplier;
-public class VisionIOPhotonCamera implements VisionIO, IORefresher {
+public class VisionIOPhotonCamera implements VisionIO {
private final List estimatedRobotPoses;
private final Supplier odometryPoseSupplier;
From af7da908e1fd65870d52be3d0260fbc18e183dda Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Tue, 14 Apr 2026 16:47:20 -0400
Subject: [PATCH 07/14] New PIDs for turret
---
.../frc/robot/subsystems/turret/TurretIOTalonFX.java | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
index b070052..dd26676 100644
--- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
@@ -181,13 +181,13 @@ private double getAngularVelocityDegreesPerSecond() {
private Pair getTurretMotionConfigs() {
Slot0Configs configs = new Slot0Configs();
- configs.kP = 200;
- configs.kI = 180;
- configs.kD = 15;
+ configs.kP = 150;
+ configs.kI = 10;
+ configs.kD = 25;
- configs.kS = 10; //kS;
- configs.kV = 0.8; //kV;
- configs.kA = 20;
+ configs.kS = 0; //kS;
+ configs.kV = 0; //kV;
+ configs.kA = 0;
MotionMagicConfigs mmConfigs = new MotionMagicConfigs();
From 3435903446a1209266e2647068efb70a3132b53b Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Tue, 14 Apr 2026 21:57:34 -0400
Subject: [PATCH 08/14] Add flipping auto paths, update rotational
compensation, tune shot map
---
src/main/java/frc/robot/Robot.java | 9 +++------
.../frc/robot/subsystems/shooter/Shooter.java | 10 +++++-----
.../robot/subsystems/targeting/ShotData.java | 6 ++++++
.../robot/subsystems/targeting/Targeting.java | 10 ++++++----
.../frc/robot/subsystems/trigger/Trigger.java | 7 ++-----
.../frc/robot/subsystems/turret/Turret.java | 18 +++++++++---------
.../frc/robot/subsystems/turret/TurretIO.java | 1 +
7 files changed, 32 insertions(+), 29 deletions(-)
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f9a22f6..856a82a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -190,13 +190,10 @@ public void autonomousInit() {
boolean flipped = SmartDashboard.getBoolean("Auto/PathFlipped", false);
if (flipped) {
- PathPlannerAuto auto = new PathPlannerAuto(autonomousCommand);
+ PathPlannerAuto auto = new PathPlannerAuto(autonomousCommand.getName());
PathPlannerAuto flippedAuto = new PathPlannerAuto(auto.getName(), true);
- autonomousCommand = flippedAuto;
- }
-
- // schedule the autonomous command (example)
- if (autonomousCommand != null) {
+ CommandScheduler.getInstance().schedule(flippedAuto);
+ } else if (autonomousCommand != null) {
CommandScheduler.getInstance().schedule(autonomousCommand);
}
}
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 67685e6..1b0f67d 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -3,14 +3,14 @@
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
-import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.subsystem.SpikeSystem;
import frc.robot.subsystems.targeting.ShotData;
import frc.robot.subsystems.targeting.Targeting;
public class Shooter extends SpikeSystem {
- private static final double SHOOTER_READY_THRESHOLD_RPS = 3.0; // RPS threshold to consider the shooter ready
+ private static final double SHOOTER_READY_THRESHOLD_RPS = 6.0; // RPS threshold to consider the shooter ready
private boolean readFromData = true;
@@ -92,9 +92,9 @@ public boolean isAtTargetRPS() {
* @return
*/
public double getDistanceToTarget() {
- Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget();
- Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm());
- return toGoal.getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input
+ Pose2d toGoal = Targeting.differenceBetweenRobotAndTarget();
+ Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getTranslation().getNorm());
+ return toGoal.getTranslation().getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input
}
/**
diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java
index 8331331..4221700 100644
--- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java
+++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java
@@ -37,6 +37,12 @@ public class ShotData {
distanceToRPM.put(4.9, 2700.0);
distanceToHoodAngle.put(4.9, 29.0);
+ distanceToRPM.put(4.9, 2700.0);
+ distanceToHoodAngle.put(4.9, 29.0);
+
+ distanceToRPM.put(5.88, 3000.0);
+ distanceToHoodAngle.put(5.88, 35.0);
+
distanceToRPM.put(17.069, 5000.0);
distanceToHoodAngle.put(17.069, 45.0);
}
diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
index f4f5d55..25d9c41 100644
--- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
@@ -20,6 +20,7 @@ public class Targeting extends SubsystemBase {
private static Translation2d targetPos = FieldConstants.Hub.oppTopCenterPoint.toTranslation2d();
+ private static final double ROTATION_TOF_MULTIPLIER = 0.5;
private static final double FIELD_WIDTH = 8.07; // meters
private static final double FIELD_LENGTH = 16.54; // meters
@@ -51,7 +52,7 @@ public Targeting() {
SmartDashboard.putBoolean("OverrideRedAlliance", overrideRedAlliance);
}
- public static Translation2d differenceBetweenRobotAndTarget() {
+ public static Pose2d differenceBetweenRobotAndTarget() {
// calculate field-relative angle of the turret based on the turret motor position and the robot's heading
// get pose of robo
Pose2d robotPose = RobotContainer.getDrive().getPose();
@@ -87,7 +88,7 @@ public static Translation2d differenceBetweenRobotAndTarget() {
Rotation2d predictedHeading =
robotPose.getRotation().plus(
- Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * tof)
+ Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * tof * ROTATION_TOF_MULTIPLIER)
);
Translation2d predictedTurretPivot = Turret.TURRET_OFFSET_FROM_CENTER
@@ -102,9 +103,10 @@ public static Translation2d differenceBetweenRobotAndTarget() {
Logger.recordOutput("Targeting/ToGoalCompensation", toGoalComp);
Logger.recordOutput("Targeting/TimeOfFlight", tof);
- return new Translation2d(
+ return new Pose2d(
toGoalXFilter.calculate(toGoalComp.getX()),
- toGoalYFilter.calculate(toGoalComp.getY())
+ toGoalYFilter.calculate(toGoalComp.getY()),
+ predictedHeading
);
}
diff --git a/src/main/java/frc/robot/subsystems/trigger/Trigger.java b/src/main/java/frc/robot/subsystems/trigger/Trigger.java
index 8949e1d..afa28ab 100644
--- a/src/main/java/frc/robot/subsystems/trigger/Trigger.java
+++ b/src/main/java/frc/robot/subsystems/trigger/Trigger.java
@@ -53,12 +53,9 @@ public boolean getReverseTrigger() {
private boolean mechanismReadyForBalls() {
if (shooter.isActuatingHoodAndLaunching()) {
// check if turret is at target angle
- boolean turretReady = turret.isAtTargetAngle();
+ boolean turretReady = turret.isAtTargetAngle() && shooter.isAtTargetRPS();
- if (!turretReady) {
- return false;
- }
- return true;
+ return turretReady;
}
return false;
}
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index b60436a..ad6b616 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -12,7 +12,7 @@
import edu.wpi.first.math.geometry.Translation2d;
public class Turret extends SpikeSystem {
- public static final double TURRET_AIMING_TOLERANCE_DEGREES = 5.0; // degrees within which we consider the turret to be aimed at the target (+-)
+ public static final double TURRET_AIMING_TOLERANCE_DEGREES = 40.0; // degrees within which we consider the turret to be aimed at the target (+-)
// HARDWARE CONSTANTS
// gearing
@@ -49,17 +49,17 @@ public void onPeriodic() {
if (this.overrideAutomaticAiming) {
this.turretIO.setTurretAngleRobotRelativeDegrees(0);
} else {
- this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative());
+ this.turretIO.setTurretAngleRobotRelativeDegrees(getTurretAngleDegreesRobotRelative());
}
}
- public double getTurretAngleDegreesFieldRelative() {
- Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget();
+ public double getTurretAngleDegreesRobotRelative() {
+ Pose2d toGoal = Targeting.differenceBetweenRobotAndTarget();
- double angleToTarget = toGoal.getAngle().getDegrees();
- Logger.recordOutput("Targeting/AngleToTargetDeg", angleToTarget);
- return angleToTarget;
+ double robotRelativeAngleToTarget = toGoal.getTranslation().getAngle().minus(toGoal.getRotation()).getDegrees();
+ Logger.recordOutput("Targeting/PredictedTargetAngleRobotRelative", robotRelativeAngleToTarget);
+ return robotRelativeAngleToTarget;
}
@@ -81,8 +81,8 @@ public void toggleAimingOverride() {
@AutoLogOutput(key="Turret/IsAtTargetAngle")
public boolean isAtTargetAngle() {
- return Math.abs(io.turretAngularVelocityDegreesPerSecond) < 200.0;
- // return Math.abs(io.targetTurretDegrees - io.turretAngleDegreesTurretRelative) < TURRET_AIMING_TOLERANCE_DEGREES;
+ // return Math.abs(io.turretAngularVelocityDegreesPerSecond) < 200.0;
+ return Math.abs(io.targetTurretMotorRotations - io.turretMotorPositionRotations) < TURRET_AIMING_TOLERANCE_DEGREES / 180.0;
}
public void changeTrim(double deltaDegrees) {
diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIO.java b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
index d9f267f..076a4d3 100644
--- a/src/main/java/frc/robot/subsystems/turret/TurretIO.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIO.java
@@ -22,6 +22,7 @@ public static class TurretIOInputs extends BaseInputClass {
public double rawTurretMechanismRotations = 0.0; // raw rotations of the entire turret mechanism
public double turretTrimDegrees = 0.0; // minor adjustment to the turret angle based on operator controller input, in degrees
public double turretAngularVelocityDegreesPerSecond = 0.0; // current angular velocity of the turret, in degrees per second
+ public double targetTurretDegreesTurretRelative = 0;
}
/**
From 6ddbbd45356ee4c38cb7aa7bc3e95b6668905a6d Mon Sep 17 00:00:00 2001
From: Justin E
Date: Wed, 15 Apr 2026 09:46:04 -0400
Subject: [PATCH 09/14] Create outpost and outpost center autos
---
.../autos/Center Depot + Middle Score.auto | 44 ++++
.../pathplanner/autos/Center Depot Score.auto | 32 +++
.../paths/Center Depot + Middle.path | 199 ++++++++++++++++++
.../pathplanner/paths/Center Depot Path.path | 80 +++++++
4 files changed, 355 insertions(+)
create mode 100644 src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
create mode 100644 src/main/deploy/pathplanner/autos/Center Depot Score.auto
create mode 100644 src/main/deploy/pathplanner/paths/Center Depot + Middle.path
create mode 100644 src/main/deploy/pathplanner/paths/Center Depot Path.path
diff --git a/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
new file mode 100644
index 0000000..32e7fae
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
@@ -0,0 +1,44 @@
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "deployIntake"
+ }
+ },
+ {
+ "type": "named",
+ "data": {
+ "name": "enableIntake"
+ }
+ },
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "startFlywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Center Depot + Middle"
+ }
+ }
+ ]
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Center Depot Score.auto b/src/main/deploy/pathplanner/autos/Center Depot Score.auto
new file mode 100644
index 0000000..73e1970
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Center Depot Score.auto
@@ -0,0 +1,32 @@
+{
+ "version": "2025.0",
+ "command": {
+ "type": "sequential",
+ "data": {
+ "commands": [
+ {
+ "type": "parallel",
+ "data": {
+ "commands": [
+ {
+ "type": "named",
+ "data": {
+ "name": "startFlywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Center Depot Path"
+ }
+ }
+ ]
+ }
+ }
+ ]
+ }
+ },
+ "resetOdom": true,
+ "folder": null,
+ "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Center Depot + Middle.path b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path
new file mode 100644
index 0000000..b0e248d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path
@@ -0,0 +1,199 @@
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.660794487847223,
+ "y": 6.020440321180556
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.6595376190544933,
+ "y": 5.979182559231182
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.5849304607865122,
+ "y": 5.88581945961034
+ },
+ "prevControl": {
+ "x": 0.5689476128826817,
+ "y": 6.135308033643065
+ },
+ "nextControl": {
+ "x": 0.6009133086903428,
+ "y": 5.636330885577614
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.859219135488436,
+ "y": 5.378003914818528
+ },
+ "prevControl": {
+ "x": 5.831043102583499,
+ "y": 5.7761905789879595
+ },
+ "nextControl": {
+ "x": 5.948829073271375,
+ "y": 4.1116268374419915
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 5.859219135488436,
+ "y": 2.749355383088813
+ },
+ "prevControl": {
+ "x": 6.147259069885608,
+ "y": 3.1473902857579765
+ },
+ "nextControl": {
+ "x": 5.595641065232987,
+ "y": 2.3851236950090944
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 2.8313093218881566,
+ "y": 2.8686942759753267
+ },
+ "prevControl": {
+ "x": 3.815959554021979,
+ "y": 2.8579860032932682
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [
+ {
+ "waypointRelativePos": 1,
+ "rotationDegrees": 160.0
+ },
+ {
+ "waypointRelativePos": 1.9488748034591195,
+ "rotationDegrees": 180.0
+ },
+ {
+ "waypointRelativePos": 2.2073751965408794,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 2.5366548742138373,
+ "rotationDegrees": -90.0
+ },
+ {
+ "waypointRelativePos": 3.3280267295597485,
+ "rotationDegrees": 0.0
+ }
+ ],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.6697612732095494,
+ "maxWaypointRelativePos": 1.1274867374005306,
+ "constraints": {
+ "maxVelocity": 0.25,
+ "maxAcceleration": 0.25,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 2.133247679045094,
+ "maxWaypointRelativePos": 2.8504263913824066,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 1.6001740716180366,
+ "maxWaypointRelativePos": 1.7901193633952235,
+ "constraints": {
+ "maxVelocity": 1.5,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 3.3256382625994703,
+ "maxWaypointRelativePos": 3.678340517241379,
+ "constraints": {
+ "maxVelocity": 1.5,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Start Shooting",
+ "waypointRelativePos": 0.6965765915119365,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "requestShooting5S"
+ }
+ }
+ },
+ {
+ "name": "Start Shooting",
+ "waypointRelativePos": 4.0,
+ "endWaypointRelativePos": null,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "requestShooting20S"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": -40.499575607732986
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Center Depot Path.path b/src/main/deploy/pathplanner/paths/Center Depot Path.path
new file mode 100644
index 0000000..a547bf5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Center Depot Path.path
@@ -0,0 +1,80 @@
+{
+ "version": "2025.0",
+ "waypoints": [
+ {
+ "anchor": {
+ "x": 3.660794487847223,
+ "y": 6.020440321180556
+ },
+ "prevControl": null,
+ "nextControl": {
+ "x": 2.702405813510649,
+ "y": 5.9506997240295485
+ },
+ "isLocked": false,
+ "linkedName": null
+ },
+ {
+ "anchor": {
+ "x": 0.4294309810169956,
+ "y": 5.919893391927083
+ },
+ "prevControl": {
+ "x": 1.477937911184211,
+ "y": 5.930899465460525
+ },
+ "nextControl": null,
+ "isLocked": false,
+ "linkedName": null
+ }
+ ],
+ "rotationTargets": [],
+ "constraintZones": [
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 0.2,
+ "maxWaypointRelativePos": 1.0,
+ "constraints": {
+ "maxVelocity": 2.0,
+ "maxAcceleration": 0.5,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
+ }
+ ],
+ "pointTowardsZones": [],
+ "eventMarkers": [
+ {
+ "name": "Start Shooting",
+ "waypointRelativePos": 0.3,
+ "endWaypointRelativePos": 2.0,
+ "command": {
+ "type": "named",
+ "data": {
+ "name": "requestShooting20S"
+ }
+ }
+ }
+ ],
+ "globalConstraints": {
+ "maxVelocity": 3.0,
+ "maxAcceleration": 1.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ },
+ "goalEndState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "reversed": false,
+ "folder": null,
+ "idealStartingState": {
+ "velocity": 0,
+ "rotation": 180.0
+ },
+ "useDefaultConstraints": true
+}
\ No newline at end of file
From a98b41fabb931b6af1ac9cacbfa9fdb727fd4ef3 Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 16 Apr 2026 10:00:21 -0400
Subject: [PATCH 10/14] Fixes to auto
---
.../autos/Center Depot + Middle Score.auto | 29 +++---
.../paths/Center Depot + Middle.path | 99 ++++++++++---------
.../java/frc/robot/commands/DeployIntake.java | 4 +-
.../vision/photon/CameraManager.java | 2 +-
4 files changed, 67 insertions(+), 67 deletions(-)
diff --git a/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
index 32e7fae..3eeb7bf 100644
--- a/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
+++ b/src/main/deploy/pathplanner/autos/Center Depot + Middle Score.auto
@@ -17,22 +17,21 @@
}
},
{
- "type": "parallel",
+ "type": "named",
+ "data": {
+ "name": "startFlywheel"
+ }
+ },
+ {
+ "type": "path",
+ "data": {
+ "pathName": "Center Depot + Middle"
+ }
+ },
+ {
+ "type": "named",
"data": {
- "commands": [
- {
- "type": "named",
- "data": {
- "name": "startFlywheel"
- }
- },
- {
- "type": "path",
- "data": {
- "pathName": "Center Depot + Middle"
- }
- }
- ]
+ "name": "requestShooting20S"
}
}
]
diff --git a/src/main/deploy/pathplanner/paths/Center Depot + Middle.path b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path
index b0e248d..d21b2e3 100644
--- a/src/main/deploy/pathplanner/paths/Center Depot + Middle.path
+++ b/src/main/deploy/pathplanner/paths/Center Depot + Middle.path
@@ -8,68 +8,68 @@
},
"prevControl": null,
"nextControl": {
- "x": 2.6595376190544933,
- "y": 5.979182559231182
+ "x": 2.9266887417218546,
+ "y": 5.8209437086092715
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 0.5849304607865122,
- "y": 5.88581945961034
+ "x": 0.777748344370861,
+ "y": 5.762864238410596
},
"prevControl": {
- "x": 0.5689476128826817,
- "y": 6.135308033643065
+ "x": 0.7617654964670305,
+ "y": 6.012352812443321
},
"nextControl": {
- "x": 0.6009133086903428,
- "y": 5.636330885577614
+ "x": 0.7937311922746916,
+ "y": 5.51337566437787
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 5.859219135488436,
- "y": 5.378003914818528
+ "x": 5.961341059602649,
+ "y": 5.486986754966887
},
"prevControl": {
- "x": 5.831043102583499,
- "y": 5.7761905789879595
+ "x": 5.933165026697712,
+ "y": 5.8851734191363185
},
"nextControl": {
- "x": 5.948829073271375,
- "y": 4.1116268374419915
+ "x": 6.050950997385588,
+ "y": 4.2206096775903506
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 5.859219135488436,
- "y": 2.749355383088813
+ "x": 5.961341059602649,
+ "y": 2.633832781456953
},
"prevControl": {
- "x": 6.147259069885608,
- "y": 3.1473902857579765
+ "x": 6.143094347740103,
+ "y": 2.805488664697883
},
"nextControl": {
- "x": 5.595641065232987,
- "y": 2.3851236950090944
+ "x": 5.779587771465196,
+ "y": 2.4621768982160233
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
- "x": 2.8313093218881566,
- "y": 2.8686942759753267
+ "x": 3.2316059602649005,
+ "y": 2.633832781456953
},
"prevControl": {
- "x": 3.815959554021979,
- "y": 2.8579860032932682
+ "x": 4.216256192398722,
+ "y": 2.6231245087748944
},
"nextControl": null,
"isLocked": false,
@@ -82,26 +82,14 @@
"rotationDegrees": 160.0
},
{
- "waypointRelativePos": 1.9488748034591195,
- "rotationDegrees": 180.0
- },
- {
- "waypointRelativePos": 2.2073751965408794,
- "rotationDegrees": -90.0
- },
- {
- "waypointRelativePos": 2.5366548742138373,
- "rotationDegrees": -90.0
- },
- {
- "waypointRelativePos": 3.3280267295597485,
- "rotationDegrees": 0.0
+ "waypointRelativePos": 1.249394673123488,
+ "rotationDegrees": -90.9478722909092
}
],
"constraintZones": [
{
"name": "Constraints Zone",
- "minWaypointRelativePos": 0.6697612732095494,
+ "minWaypointRelativePos": 0.8141342756183786,
"maxWaypointRelativePos": 1.1274867374005306,
"constraints": {
"maxVelocity": 0.25,
@@ -140,8 +128,8 @@
},
{
"name": "Constraints Zone",
- "minWaypointRelativePos": 3.3256382625994703,
- "maxWaypointRelativePos": 3.678340517241379,
+ "minWaypointRelativePos": 3.1038869257950408,
+ "maxWaypointRelativePos": 3.776678445229686,
"constraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
@@ -150,29 +138,42 @@
"nominalVoltage": 12.0,
"unlimited": false
}
+ },
+ {
+ "name": "Constraints Zone",
+ "minWaypointRelativePos": 1.125088339222631,
+ "maxWaypointRelativePos": 1.6621908127208602,
+ "constraints": {
+ "maxVelocity": 1.0,
+ "maxAcceleration": 3.0,
+ "maxAngularVelocity": 540.0,
+ "maxAngularAcceleration": 720.0,
+ "nominalVoltage": 12.0,
+ "unlimited": false
+ }
}
],
"pointTowardsZones": [],
"eventMarkers": [
{
- "name": "Start Shooting",
- "waypointRelativePos": 0.6965765915119365,
- "endWaypointRelativePos": null,
+ "name": "requestShooting20S",
+ "waypointRelativePos": 1.2325088339222643,
+ "endWaypointRelativePos": 1.5943462897526506,
"command": {
"type": "named",
"data": {
- "name": "requestShooting5S"
+ "name": "requestShooting20S"
}
}
},
{
- "name": "Start Shooting",
- "waypointRelativePos": 4.0,
+ "name": "startFlywheel",
+ "waypointRelativePos": 2.318021201413423,
"endWaypointRelativePos": null,
"command": {
"type": "named",
"data": {
- "name": "requestShooting20S"
+ "name": "startFlywheel"
}
}
}
@@ -187,7 +188,7 @@
},
"goalEndState": {
"velocity": 0,
- "rotation": -40.499575607732986
+ "rotation": -90.72522429905929
},
"reversed": false,
"folder": null,
diff --git a/src/main/java/frc/robot/commands/DeployIntake.java b/src/main/java/frc/robot/commands/DeployIntake.java
index d17df9f..1c4e723 100644
--- a/src/main/java/frc/robot/commands/DeployIntake.java
+++ b/src/main/java/frc/robot/commands/DeployIntake.java
@@ -20,14 +20,14 @@ public void initialize() {
@Override
public void execute() {
- if (timer.hasElapsed(1.0)) {
+ if (timer.hasElapsed(0.75)) {
intake.setDeployServo(0.0);
}
}
@Override
public boolean isFinished() {
- if (timer.hasElapsed(2.0)) {
+ if (timer.hasElapsed(1.5)) {
return true;
}
return false;
diff --git a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
index 9c55703..bdc1993 100644
--- a/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
+++ b/src/main/java/frc/robot/subsystems/vision/photon/CameraManager.java
@@ -59,7 +59,7 @@ public static List getCameras() {
new Camera(
"left",
new Transform3d(
- Inches.of(-2.5),
+ Inches.of(-2.25),
Inches.of(-13.75),
Inches.of(8.5625),
new Rotation3d(
From 015d3d6d8d5304ac0efd46bf6a12ccd8364670cd Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 16 Apr 2026 10:04:38 -0400
Subject: [PATCH 11/14] Remove new LED code
---
.../frc/lib/led/AddressableLEDController.java | 89 ----
src/main/java/frc/lib/led/Color.java | 271 -------------
.../led/DuplicateLEDAssignmentException.java | 9 -
.../java/frc/lib/led/FixedLEDController.java | 21 -
.../frc/lib/led/InvalidColorException.java | 9 -
src/main/java/frc/lib/led/LEDPreset.java | 6 -
src/main/java/frc/lib/led/LEDStrip.java | 114 ------
.../java/frc/lib/led/PWMLEDController.java | 383 ------------------
.../led/presets/addressable/BlinkPattern.java | 51 ---
.../led/presets/addressable/ChasePattern.java | 65 ---
.../led/presets/addressable/FadePattern.java | 46 ---
.../led/presets/addressable/LEDPattern.java | 9 -
.../presets/addressable/MergeSortPattern.java | 179 --------
.../presets/addressable/RainbowPattern.java | 62 ---
.../addressable/SolidColorPattern.java | 30 --
.../led/presets/addressable/WavePattern.java | 52 ---
src/main/java/frc/robot/Robot.java | 1 -
src/main/java/frc/robot/RobotContainer.java | 114 ------
.../robot/subsystems/targeting/Targeting.java | 5 -
.../frc/robot/subsystems/turret/Turret.java | 1 -
20 files changed, 1517 deletions(-)
delete mode 100644 src/main/java/frc/lib/led/AddressableLEDController.java
delete mode 100644 src/main/java/frc/lib/led/Color.java
delete mode 100644 src/main/java/frc/lib/led/DuplicateLEDAssignmentException.java
delete mode 100644 src/main/java/frc/lib/led/FixedLEDController.java
delete mode 100644 src/main/java/frc/lib/led/InvalidColorException.java
delete mode 100644 src/main/java/frc/lib/led/LEDPreset.java
delete mode 100644 src/main/java/frc/lib/led/LEDStrip.java
delete mode 100644 src/main/java/frc/lib/led/PWMLEDController.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/ChasePattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/FadePattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/LEDPattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java
delete mode 100644 src/main/java/frc/lib/led/presets/addressable/WavePattern.java
diff --git a/src/main/java/frc/lib/led/AddressableLEDController.java b/src/main/java/frc/lib/led/AddressableLEDController.java
deleted file mode 100644
index 878e2a1..0000000
--- a/src/main/java/frc/lib/led/AddressableLEDController.java
+++ /dev/null
@@ -1,89 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led;
-
-/**
- * A controller for individually addressable LED strips connected to a PWM port on the RoboRIO.
- * Thin wrapper over PWMLEDController implementation for compatibility purposes.
- */
-public class AddressableLEDController {
- private final PWMLEDController controller;
-
- /**
- * Creates a new LED controller.
- * @param port The PWM port the LED controller is connected to.
- */
- public AddressableLEDController(int port) {
- controller = new PWMLEDController(port, PWMLEDController.Mode.ADDRESSABLE);
- }
-
- /**
- * Adds an LED strip to the controller.
- * This is a relatively expensive call, so it should be avoided in the loop.
- * If a new strip is added between two existing strips,
- * all strips after the new strip will have their index shifted.
- * But the lengths and offsets will be unaffected.
- * @param strip The LED strip to be added.
- * @throws DuplicateLEDAssignmentException If the new strip overlaps with an existing strip.
- */
- public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
- controller.addStrip(strip);
- }
-
- /**
- * Adds a new LED strip to the controller with a certain length and offset from the start.
- * This is a relatively expensive call, so it should be avoided in the loop.
- * If a new strip is added between two existing strips,
- * all strips after the new strip will have their index shifted.
- * But the lengths and offsets will be unaffected.
- * @param length The length of the strip in LEDs.
- * @param offset The offset to the start of the strip in LEDs.
- * @throws DuplicateLEDAssignmentException If the new strip overlaps with an existing strip.
- */
- public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
- controller.addStrip(length, offset);
- }
-
- /**
- * Gets the LED strip at a certain index.
- * @param index The index of the strip.
- * @return The LED strip at the index.
- */
- public LEDStrip getStrip(int index) {
- return controller.getStrip(index);
- }
-
- /**
- * Removes the LED strip at a certain index.
- * This is a relatively expensive call, so it should be avoided in the loop.
- * If the strip that gets removed is not the last strip,
- * all strips after the removed strip will have their index shifted.
- * @param index The index of the strip to remove.
- */
- public void removeStrip(int index) {
- controller.removeStrip(index);
- }
-
- /**
- * Starts the output of the LED controller.
- * The output writes continuously.
- */
- public void start() {
- controller.start();
- }
-
- /**
- * Stops the output of the LED controller.
- */
- public void stop() {
- controller.stop();
- }
-
- /**
- * Updates the LED strips.
- * This should be called in the loop.
- */
- public void updateStrips() {
- controller.update();
- }
-}
diff --git a/src/main/java/frc/lib/led/Color.java b/src/main/java/frc/lib/led/Color.java
deleted file mode 100644
index 05d3fb4..0000000
--- a/src/main/java/frc/lib/led/Color.java
+++ /dev/null
@@ -1,271 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led;
-
-/**
- * Class for representing colors in RGB and HSV color modes.
- * Allows for quick conversion between the two modes.
- * All values are in ranges supported by the WPILib {@code AddressableLED} class.
- * RGB values are in the range [0-255].
- * HSV values are in the range [0-180), [0-255], [0-255].
- * Intended to be used with {@code PWMLEDController} and {@code LEDStrip} classes.
- *
- * @see HSV on Wikipedia
- * @see edu.wpi.first.wpilibj.AddressableLED
- * @see frc.team4481.lib.feedback.led.PWMLEDController
- * @see frc.team4481.lib.feedback.led.LEDStrip
- */
-public class Color {
- private final RGB rgb;
- private final HSV hsv;
-
- /**
- * Creates a new color from HSV values.
- * @param hue The hue of the color [0-180).
- * @param saturation The saturation of the color [0-255].
- * @param value The value of the color [0-255].
- */
- public Color(int hue, int saturation, int value) {
- hsv = new HSV(hue, saturation, value);
- rgb = HSVtoRGB(hue, saturation, value);
- }
-
- /**
- * Creates a new color from RGB values.
- * @param red The red value of the color [0-255].
- * @param green The green value of the color [0-255].
- * @param blue The blue value of the color [0-255].
- */
- public static Color fromRGB(int red, int green, int blue) {
- HSV hsv = RGBtoHSV(red, green, blue);
- return new Color(hsv.hue(), hsv.saturation(), hsv.value());
- }
-
- /**
- * Creates a new color from a hex string.
- * @param hex The hex string to convert (without #).
- */
- public static Color fromHex(String hex) throws InvalidColorException {
- if (hex.length() != 6) {
- throw new InvalidColorException("Hex string must be 6 characters long.");
- }
-
- return fromRGB(
- Integer.valueOf(hex.substring(0, 2), 16),
- Integer.valueOf(hex.substring(2, 4), 16),
- Integer.valueOf(hex.substring(4, 6), 16)
- );
- }
-
- /**
- * Gets the RGB values of the color.
- * @return The RGB values of the color.
- */
- public RGB getRGB() {
- return rgb;
- }
-
- /**
- * Gets the HSV values of the color.
- * @return The HSV values of the color.
- */
- public HSV getHSV() {
- return hsv;
- }
-
- /**
- * Converts an HSV color to RGB.
- * @param h The hue of the color [0-180).
- * @param s The saturation of the color [0-255].
- * @param v The value of the color [0-255].
- */
- private RGB HSVtoRGB(int h, int s, int v) {
- if (s == 0) {
- return new RGB(v, v, v);
- }
-
- // The below algorithm is copied from Color.fromHSV and moved here for
- // performance reasons.
-
- // Loosely based on
- // https://en.wikipedia.org/wiki/HSL_and_HSV#HSV_to_RGB
- // The hue range is split into 60 degree regions where in each region there
- // is one rgb component at a low value (m), one at a high value (v) and one
- // that changes (X) from low to high (X+m) or high to low (v-X)
-
- // Difference between highest and lowest value of any rgb component
- final int chroma = (s * v) / 255;
-
- // Because hue is 0-180 rather than 0-360 use 30 not 60
- final int region = (h / 30) % 6;
-
- // Remainder converted from 0-30 to 0-255
- final int remainder = (int) Math.round((h % 30) * (255 / 30.0));
-
- // Value of the lowest rgb component
- final int m = v - chroma;
-
- // Goes from 0 to chroma as hue increases
- final int X = (chroma * remainder) >> 8;
-
- return switch (region) {
- case 0 -> new RGB(v, X + m, m);
- case 1 -> new RGB(v - X, v, m);
- case 2 -> new RGB(m, v, X + m);
- case 3 -> new RGB(m, v - X, v);
- case 4 -> new RGB(X + m, m, v);
- default -> new RGB(v, m, v - X);
- };
- }
-
- /**
- * Converts an RGB color to HSV.
- * @param r The red value of the color [0-255].
- * @param g The green value of the color [0-255].
- * @param b The blue value of the color [0-255].
- */
- private static HSV RGBtoHSV(int r, int g, int b) {
- double red = r / 255.0;
- double green = g / 255.0;
- double blue = b / 255.0;
-
- double cMax = Math.max(red, Math.max(green, blue));
- double cMin = Math.min(red, Math.min(green, blue));
-
- double delta = cMax - cMin;
-
- // Hue
- int hue;
-
- if (delta == 0) {
- hue = 0;
- } else if (cMax == red) {
- hue = (int) Math.round(60 * (((green - blue) / delta) % 6));
- } else if (cMax == green) {
- hue = (int) Math.round(60 * (((blue - red) / delta) + 2));
- } else {
- hue = (int) Math.round(60 * (((red - green) / delta) + 4));
- }
-
- // Saturation
- double saturation = (cMax == 0) ? 0 : delta / cMax;
-
- // Convert final values to correct range
- return new HSV(
- hue / 2,
- (int) Math.round(saturation * 255),
- (int) Math.round(cMax * 255)
- );
- }
-
- /**
- * Represents a color in RGB.
- * With red, green, and blue in the range [0-255].
- */
- public static class RGB {
- private final int red;
- private final int green;
- private final int blue;
-
- /**
- * Creates a new RGB color.
- * @param red The red value of the color [0-255].
- * @param green The green value of the color [0-255].
- * @param blue The blue value of the color [0-255].
- */
- public RGB(int red, int green, int blue) {
- this.red = red;
- this.green = green;
- this.blue = blue;
- }
-
- /**
- * Gets the red value of the color.
- * @return The red value of the color [0-255].
- */
- public int red() {
- return red;
- }
-
- /**
- * Gets the green value of the color.
- * @return The green value of the color [0-255].
- */
- public int green() {
- return green;
- }
-
- /**
- * Gets the blue value of the color.
- * @return The blue value of the color [0-255].
- */
- public int blue() {
- return blue;
- }
- }
-
- /**
- * Represents a color in HSV.
- * With hue in the range [0-180),
- * saturation in the range [0-255],
- * and value in the range [0-255].
- */
- public static class HSV {
- private final int hue;
- private final int saturation;
- private final int value;
-
- /**
- * Creates a new HSV color.
- * @param hue The hue of the color [0-180).
- * @param saturation The saturation of the color [0-255].
- * @param value The value of the color [0-255].
- */
- public HSV(int hue, int saturation, int value) {
- this.hue = hue;
- this.saturation = saturation;
- this.value = value;
- }
-
- /**
- * Gets the hue of the color.
- * @return The hue of the color [0-180).
- */
- public int hue() {
- return hue;
- }
-
- /**
- * Gets the saturation of the color.
- * @return The saturation of the color [0-255].
- */
- public int saturation() {
- return saturation;
- }
-
- /**
- * Gets the value of the color.
- * @return The value of the color [0-255].
- */
- public int value() {
- return value;
- }
- }
-
- @Override
- public boolean equals(Object obj) {
- if (obj == null) {
- return false;
- }
-
- if (obj.getClass() != this.getClass()) {
- return false;
- }
-
- final Color other = (Color) obj;
-
- return this.rgb.red == other.rgb.red
- && this.rgb.green == other.rgb.green
- && this.rgb.blue == other.rgb.blue;
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/DuplicateLEDAssignmentException.java b/src/main/java/frc/lib/led/DuplicateLEDAssignmentException.java
deleted file mode 100644
index b7deaf0..0000000
--- a/src/main/java/frc/lib/led/DuplicateLEDAssignmentException.java
+++ /dev/null
@@ -1,9 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led;
-
-public class DuplicateLEDAssignmentException extends RuntimeException {
- public DuplicateLEDAssignmentException(String message) {
- super(message);
- }
-}
diff --git a/src/main/java/frc/lib/led/FixedLEDController.java b/src/main/java/frc/lib/led/FixedLEDController.java
deleted file mode 100644
index 8a5d26c..0000000
--- a/src/main/java/frc/lib/led/FixedLEDController.java
+++ /dev/null
@@ -1,21 +0,0 @@
-package frc.lib.led;
-
-/**
- * A controller for fixed LED strips connected to a PWM port on the RoboRIO.
- * Thin wrapper over PWMLEDController implementation for compatibility purposes.
- */
-public class FixedLEDController {
- private final PWMLEDController controller;
-
- public FixedLEDController(int port) {
- controller = new PWMLEDController(port, PWMLEDController.Mode.FIXED);
- }
-
- public void switchPreset(String name) {
- controller.switchPreset(name);
- }
-
- public void registerPreset(LEDPreset preset) {
- controller.registerPreset(preset);
- }
-}
diff --git a/src/main/java/frc/lib/led/InvalidColorException.java b/src/main/java/frc/lib/led/InvalidColorException.java
deleted file mode 100644
index 40f81ef..0000000
--- a/src/main/java/frc/lib/led/InvalidColorException.java
+++ /dev/null
@@ -1,9 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led;
-
-public class InvalidColorException extends RuntimeException {
- public InvalidColorException(String message) {
- super(message);
- }
-}
diff --git a/src/main/java/frc/lib/led/LEDPreset.java b/src/main/java/frc/lib/led/LEDPreset.java
deleted file mode 100644
index 2b49229..0000000
--- a/src/main/java/frc/lib/led/LEDPreset.java
+++ /dev/null
@@ -1,6 +0,0 @@
-package frc.lib.led;
-
-public record LEDPreset(
- String name,
- double value
-) {}
diff --git a/src/main/java/frc/lib/led/LEDStrip.java b/src/main/java/frc/lib/led/LEDStrip.java
deleted file mode 100644
index dc2edee..0000000
--- a/src/main/java/frc/lib/led/LEDStrip.java
+++ /dev/null
@@ -1,114 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led;
-
-import frc.lib.led.presets.addressable.LEDPattern;
-import frc.lib.led.presets.addressable.RainbowPattern;
-
-/**
- * Class to represent an LED strip with a fixed LED length and offset from the start.
- * A physical LED strip can be divided into multiple LED strip objects.
- * This allows for multiple patterns to be displayed on the same strip.
- * Each strip can have its own color, pattern, and duration.
- * @see Color
- */
-public class LEDStrip {
- private final int length;
- private final int offset;
-
- private double patternDuration = 5;
- private Color primaryColor = new Color(12, 255, 255);
- private Color secondaryColor = new Color(102, 255, 255);
- private LEDPattern pattern = new RainbowPattern();
-
- /**
- * Creates a new LED strip.
- * @param length The length of the strip in LEDs.
- * @param offset The offset of the strip in LEDs.
- */
- public LEDStrip(int length, int offset) {
- this.length = length;
- this.offset = offset;
- }
-
- /**
- * Sets the color of the strip.
- * @param color The color to set the strip to.
- */
- public void setPrimaryColor(Color color) {
- this.primaryColor = color;
- }
-
- /**
- * Sets the secondary of the strip.
- * @param color The color to set the strip to.
- */
- public void setSecondaryColor(Color color) {
- this.secondaryColor = color;
- }
-
-
- /**
- * Sets the pattern of the strip.
- * @param pattern The pattern to set the strip to.
- */
- public void setPattern(LEDPattern pattern) {
- this.pattern = pattern;
- }
-
- /**
- * Sets the time it takes for the pattern to loop.
- * @param patternDuration The pattern duration to set the strip to (in s).
- */
- public void setPatternDuration(double patternDuration) {
- this.patternDuration = patternDuration;
- }
-
- /**
- * Gets the length of the strip.
- * @return The amount of LEDs on the strip.
- */
- public int getLength() {
- return length;
- }
-
- /**
- * Gets the offset of the strip.
- * @return The amount of LEDs before this strip starts.
- */
- public int getOffset() {
- return offset;
- }
-
- /**
- * Gets the primary color of the strip.
- * @return The color of the strip.
- */
- public Color getPrimaryColor() {
- return primaryColor;
- }
-
- /**
- * Gets the secondary color of the strip.
- * @return The color of the strip.
- */
- public Color getSecondaryColor() {
- return secondaryColor;
- }
-
- /**
- * Gets the pattern of the strip.
- * @return The pattern of the strip.
- */
- public LEDPattern getPattern() {
- return pattern;
- }
-
- /**
- * Gets the time it takes for the pattern to loop.
- * @return The pattern duration in s.
- */
- public double getPatternDuration() {
- return patternDuration;
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/PWMLEDController.java b/src/main/java/frc/lib/led/PWMLEDController.java
deleted file mode 100644
index 7931fd8..0000000
--- a/src/main/java/frc/lib/led/PWMLEDController.java
+++ /dev/null
@@ -1,383 +0,0 @@
-package frc.lib.led;
-
-import edu.wpi.first.wpilibj.AddressableLED;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.motorcontrol.Spark;
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.function.Consumer;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-
-/**
- * Unified RGB LED controller that can drive either a fixed 12V strip or an
- * individually addressable 5V strip.
- *
- * Use {@link Mode#FIXED} for Spark/PWM-based controllers and
- * {@link Mode#ADDRESSABLE} for addressable LED buffers.
- */
-public class PWMLEDController {
- /** Selects which hardware model the controller should use. */
- public enum Mode {
- FIXED,
- ADDRESSABLE
- }
-
- /**
- * Defines one logical preset name with a fixed implementation and an
- * addressable implementation.
- *
- *
The addressable mutator receives a read-only strip list. It can mutate
- * strip state (pattern/colors/duration) but should not modify strip topology.
- */
- public record ModePreset(
- String name,
- LEDPreset fixedPreset,
- Consumer> addressableMutator
- ) {}
-
- private final Mode mode;
- private final LEDBackend backend;
-
- /**
- * Creates a new RGB controller for either fixed or addressable hardware.
- *
- * @param port The PWM port the controller is connected to.
- * @param mode The hardware mode to use.
- */
- public PWMLEDController(int port, Mode mode) {
- this.mode = mode;
- this.backend = switch (mode) {
- case FIXED -> new FixedBackend(port);
- case ADDRESSABLE -> new AddressableBackend(port);
- };
- }
-
- /** Returns the configured hardware mode. */
- public Mode getMode() {
- return mode;
- }
-
- /** Returns true when this controller is using addressable LED hardware. */
- public boolean isAddressable() {
- return mode == Mode.ADDRESSABLE;
- }
-
- /** Returns true when this controller is using fixed LED hardware. */
- public boolean isFixed() {
- return mode == Mode.FIXED;
- }
-
- /** Starts the output of the controller. */
- public void start() {
- backend.start();
- }
-
- /** Stops the output of the controller. */
- public void stop() {
- backend.stop();
- }
-
- /**
- * Updates the controller.
- *
- * For fixed hardware this is a no-op, while addressable hardware pushes
- * the current strip buffers to the RoboRIO output.
- */
- public void update() {
- backend.update();
- }
-
- /** Registers a preset for fixed LED hardware. */
- public void registerPreset(LEDPreset preset) {
- backend.registerPreset(preset);
- }
-
- /** Registers a mode-aware preset for either fixed or addressable hardware. */
- public void registerPreset(ModePreset preset) {
- backend.registerPreset(preset);
- }
-
- /** Switches the active preset for the currently selected hardware mode. */
- public void switchPreset(String name) {
- backend.switchPreset(name);
- }
-
- /** Adds a strip for addressable LED hardware. */
- public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
- backend.addStrip(strip);
- }
-
- /** Adds a strip for addressable LED hardware. */
- public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
- backend.addStrip(length, offset);
- }
-
- /** Gets an addressable strip by index. */
- public LEDStrip getStrip(int index) {
- return backend.getStrip(index);
- }
-
- /** Removes an addressable strip by index. */
- public void removeStrip(int index) {
- backend.removeStrip(index);
- }
-
- /** Returns the number of configured strips. */
- public int getStripCount() {
- return backend.getStripCount();
- }
-
- private interface LEDBackend {
- void start();
-
- void stop();
-
- void update();
-
- void registerPreset(LEDPreset preset);
-
- void registerPreset(ModePreset preset);
-
- void switchPreset(String name);
-
- void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException;
-
- void addStrip(int length, int offset) throws DuplicateLEDAssignmentException;
-
- LEDStrip getStrip(int index);
-
- void removeStrip(int index);
-
- int getStripCount();
- }
-
- private static abstract class BaseBackend implements LEDBackend {
- @Override
- public void registerPreset(LEDPreset preset) {
- throw unsupported("registerPreset");
- }
-
- @Override
- public void registerPreset(ModePreset preset) {
- throw unsupported("registerPreset");
- }
-
- @Override
- public void switchPreset(String name) {
- throw unsupported("switchPreset");
- }
-
- @Override
- public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
- throw unsupported("addStrip");
- }
-
- @Override
- public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
- throw unsupported("addStrip");
- }
-
- @Override
- public LEDStrip getStrip(int index) {
- throw unsupported("getStrip");
- }
-
- @Override
- public void removeStrip(int index) {
- throw unsupported("removeStrip");
- }
-
- @Override
- public int getStripCount() {
- return 0;
- }
-
- protected UnsupportedOperationException unsupported(String operation) {
- return new UnsupportedOperationException(
- operation + " is only supported for addressable or fixed hardware as applicable"
- );
- }
- }
-
- private static final class FixedBackend extends BaseBackend {
- private final Spark controller;
- private final Map presets = new HashMap<>();
-
- private FixedBackend(int port) {
- controller = new Spark(port);
- }
-
- @Override
- public void start() {
- // Fixed PWM strips are driven directly by setting the controller output.
- }
-
- @Override
- public void stop() {
- controller.set(0);
- }
-
- @Override
- public void update() {
- // Fixed strips do not need a periodic buffer push.
- }
-
- @Override
- public void registerPreset(LEDPreset preset) {
- presets.put(preset.name(), preset);
- }
-
- @Override
- public void registerPreset(ModePreset preset) {
- if (preset.fixedPreset() != null) {
- registerPreset(preset.fixedPreset());
- }
- }
-
- @Override
- public void switchPreset(String name) {
- LEDPreset targetPreset = presets.get(name);
- if (targetPreset != null) {
- controller.set(targetPreset.value());
- } else {
- System.err.println("Invalid preset name, did you register it?");
- }
- }
- }
-
- private static final class AddressableBackend extends BaseBackend {
- private final AddressableLED controller;
- private AddressableLEDBuffer buffer;
- private final ArrayList strips = new ArrayList<>();
- private final Map>> presets = new HashMap<>();
-
- private AddressableBackend(int port) {
- controller = new AddressableLED(port);
- buffer = new AddressableLEDBuffer(0);
- controller.setLength(0);
- controller.setData(buffer);
- }
-
- @Override
- public void start() {
- controller.start();
- }
-
- @Override
- public void stop() {
- controller.stop();
- }
-
- @Override
- public void update() {
- for (LEDStrip strip : strips) {
- strip.getPattern().updateBuffer(buffer, strip);
- }
-
- controller.setData(buffer);
- }
-
- @Override
- public void addStrip(LEDStrip strip) throws DuplicateLEDAssignmentException {
- int index = getNewStripIndex(strip.getLength(), strip.getOffset());
-
- if (index == strips.size()) {
- strips.add(strip);
- } else {
- strips.add(index, strip);
- }
-
- updateTotalStripLength();
- }
-
- @Override
- public void addStrip(int length, int offset) throws DuplicateLEDAssignmentException {
- addStrip(new LEDStrip(length, offset));
- }
-
- @Override
- public void registerPreset(ModePreset preset) {
- if (preset.addressableMutator() != null) {
- presets.put(preset.name(), preset.addressableMutator());
- }
- }
-
- @Override
- public void registerPreset(LEDPreset preset) {
- // Fixed presets are ignored in addressable mode.
- }
-
- @Override
- public void switchPreset(String name) {
- Consumer> stripMutator = presets.get(name);
- if (stripMutator == null) {
- System.err.println("Invalid preset name, did you register it?");
- return;
- }
-
- try {
- stripMutator.accept(Collections.unmodifiableList(strips));
- } catch (RuntimeException ex) {
- throw new IllegalStateException(
- "Failed to apply addressable preset '" + name + "'",
- ex
- );
- }
- }
-
- @Override
- public LEDStrip getStrip(int index) {
- return strips.get(index);
- }
-
- @Override
- public void removeStrip(int index) {
- strips.remove(index);
- updateTotalStripLength();
- }
-
- @Override
- public int getStripCount() {
- return strips.size();
- }
-
- private void updateTotalStripLength() {
- int totalLength = 0;
- if (!strips.isEmpty()) {
- LEDStrip lastStrip = strips.get(strips.size() - 1);
- totalLength = lastStrip.getOffset() + lastStrip.getLength();
- }
-
- buffer = new AddressableLEDBuffer(totalLength);
- controller.setLength(totalLength);
- controller.setData(buffer);
- }
-
- private int getNewStripIndex(int length, int offset) {
- int newStripEnd = offset + length;
-
- for (int i = 0; i < strips.size(); i++) {
- LEDStrip strip = strips.get(i);
- int stripStart = strip.getOffset();
- int stripEnd = stripStart + strip.getLength();
-
- if (offset < stripEnd && newStripEnd > stripStart) {
- throw new DuplicateLEDAssignmentException(
- "LED strip overlaps with existing strip at index " + i
- );
- }
- }
-
- for (int i = 0; i < strips.size(); i++) {
- if (strips.get(i).getOffset() > offset) {
- return i;
- }
- }
-
- return strips.size();
- }
- }
-}
-
diff --git a/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java b/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java
deleted file mode 100644
index 283e246..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/BlinkPattern.java
+++ /dev/null
@@ -1,51 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-import frc.lib.CountingDelay;
-
-/**
- * A blinking pattern that can be applied to an LED strip.
- * This pattern will alternate between the primary and secondary colors.
- * @see LEDStrip
- */
-public class BlinkPattern implements LEDPattern {
- private final CountingDelay blinkDelay = new CountingDelay();
- private boolean blinkPrimary = true;
-
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- Color.HSV primaryColor = strip.getPrimaryColor().getHSV();
- Color.HSV secondaryColor = strip.getSecondaryColor().getHSV();
- double blinkDuration = strip.getPatternDuration();
-
- // Switching states should be done twice per duration
- if (blinkDelay.delay(blinkDuration / 2)) {
- blinkPrimary = !blinkPrimary;
- blinkDelay.reset();
- }
-
- Color.HSV color = blinkPrimary ? primaryColor : secondaryColor;
-
- // Set the LEDs to the color if blinkOn is true, otherwise set them to black
- for (int i = 0; i < length; i++) {
- buffer.setHSV(
- offset + i,
- color.hue(),
- color.saturation(),
- color.value()
- );
- }
- }
-}
diff --git a/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java b/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java
deleted file mode 100644
index ccc7861..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/ChasePattern.java
+++ /dev/null
@@ -1,65 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-
-/**
- * A chase pattern that can be applied to an LED strip.
- * This pattern will override the value of the primary color.
- * The hue and saturation of the primary color are applied to the chase.
- * The secondary color is not used.
- * @see LEDStrip
- */
-public class ChasePattern implements LEDPattern{
- private double valueShiftBuffer = 0;
- private double previousTime = Timer.getFPGATimestamp();
- private int firstPixelValue = 255;
-
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- double patternSeconds = strip.getPatternDuration();
- Color.HSV color = strip.getPrimaryColor().getHSV();
-
- // Calculate delta time
- double newTime = Timer.getFPGATimestamp();
- double deltaTime = newTime - previousTime;
- previousTime = newTime;
-
- // For every pixel
- for (int i = 0; i < length; i++) {
- // Calculate the hue - hue is easier for rainbows because the color
- // shape is a circle so only one value needs to precess
- int value = (firstPixelValue - (i * color.value() / length)) % color.value();
- if (value < 0) value += color.value();
-
- // Set the value
- buffer.setHSV(i + offset, color.hue(), color.saturation(), value);
- }
-
- // Increase by to make the rainbow "move"
- valueShiftBuffer += ((deltaTime / patternSeconds) * color.value());
- int valueShift = (int) valueShiftBuffer;
- valueShiftBuffer -= valueShift;
- firstPixelValue -= valueShift;
-
- // Check bounds
- if (color.value() > 0){
- firstPixelValue %= color.value();
- if(firstPixelValue < 0) firstPixelValue += color.value();
- } else {
- firstPixelValue = 0;
- }
- }
-}
diff --git a/src/main/java/frc/lib/led/presets/addressable/FadePattern.java b/src/main/java/frc/lib/led/presets/addressable/FadePattern.java
deleted file mode 100644
index a7f3a2c..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/FadePattern.java
+++ /dev/null
@@ -1,46 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-
-/**
- * A fading sine wave pattern that can be applied to an LED strip.
- * This pattern will interpolate between the primary and secondary colors.
- * @see LEDStrip
- */
-public class FadePattern implements LEDPattern {
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- Color.RGB primary = strip.getPrimaryColor().getRGB();
- Color.RGB secondary = strip.getSecondaryColor().getRGB();
-
- // Fading is done by changing the value of the color over a sine wave
- double wavelength = (2 * Math.PI) / strip.getPatternDuration();
- double interpolation = Math.sin(wavelength * Timer.getFPGATimestamp()) * 0.5 + 0.5;
-
- for (int i = 0; i < length; i++) {
- buffer.setRGB(
- offset + i,
- interpolate(primary.red(), secondary.red(), interpolation),
- interpolate(primary.green(), secondary.green(), interpolation),
- interpolate(primary.blue(), secondary.blue(), interpolation)
- );
- }
- }
-
- private int interpolate(int start, int end, double interpolationValue) {
- return (int) (start + (end - start) * interpolationValue);
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java b/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java
deleted file mode 100644
index 685cd2b..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/LEDPattern.java
+++ /dev/null
@@ -1,9 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import frc.lib.led.LEDStrip;
-public interface LEDPattern {
- void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip);
-}
diff --git a/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java b/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java
deleted file mode 100644
index c325620..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/MergeSortPattern.java
+++ /dev/null
@@ -1,179 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-import java.util.ArrayList;
-import java.util.Random;
-
-/**
- * A pattern that can be applied to an LED strip.
- * This pattern will mimic the merge sort algorithm.
- * It will override the hue of the primary color.
- * The saturation and value of the primary color remain the same.
- * The secondary color is not used.
- * The pattern duration is approximated but might not be exact.
- * @see LEDStrip
- */
-public class MergeSortPattern implements LEDPattern {
- private int[] sortArray = new int[0];
- private final ArrayList sortSteps = new ArrayList<>();
-
- private double stepShiftBuffer = 0;
- private double previousTime = Timer.getFPGATimestamp();
- private int stepIndex = 0;
-
- private boolean sorted = false;
- private boolean shuffled = false;
-
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- final int HUE_RANGE = 180;
-
- int offset = strip.getOffset();
- int length = strip.getLength();
- Color.HSV color = strip.getPrimaryColor().getHSV();
- double patternSeconds = strip.getPatternDuration();
-
- // Calculate delta time
- double newTime = Timer.getFPGATimestamp();
- double deltaTime = newTime - previousTime;
- previousTime = newTime;
-
- // Create new array if needed
- if (sortArray.length != length) {
- sortArray = new int[length];
-
- for (int i = 0; i < length; i++) {
- sortArray[i] = i * HUE_RANGE / length % HUE_RANGE;
- }
-
- shuffled = false;
- }
-
- // Shuffle array if needed
- if (!shuffled) {
- sortSteps.clear();
-
- shuffleArray(sortArray);
-
- sortSteps.add(sortArray.clone());
-
- shuffled = true;
- sorted = false;
- stepIndex = 0;
- }
-
- // Sort array if needed
- if (!sorted) {
- mergeSort(sortArray, 0, sortArray.length - 1);
- sorted = true;
- }
-
- // Calculate delta time
- stepShiftBuffer += ((deltaTime / patternSeconds) * sortSteps.size());
- int stepShift = (int) stepShiftBuffer;
- stepShiftBuffer -= stepShift;
- stepIndex += stepShift;
-
-
- // Check bounds
- if (stepIndex >= sortSteps.size()) {
- shuffled = false;
- } else {
- int[] step = sortSteps.get(stepIndex);
-
- // Set colors
- for (int i = 0; i < length; i++) {
- int hue = step[i];
- buffer.setHSV(i + offset, hue, color.saturation(), color.value());
- }
- }
- }
-
- private void mergeSort(int[] array, int left, int right) {
- if (left < right) {
- int mid = left + (right - left) / 2;
-
- // Recursively sort the two halves
- mergeSort(array, left, mid);
- mergeSort(array, mid + 1, right);
-
- // Merge the sorted halves
- merge(array, left, mid, right);
-
- // Add the entire array after each merge
- sortSteps.add(array.clone());
- }
- }
-
- /**
- * Sorts an array using the merge sort algorithm.
- * @param array The array to be sorted.
- * @param left The left index of the array.
- * @param right The right index of the array.
- * @param mid The middle index of the array.
- */
- public void merge(int[] array, int left, int mid, int right) {
- int n1 = mid - left + 1;
- int n2 = right - mid;
-
- int[] leftArray = new int[n1];
- int[] rightArray = new int[n2];
-
- // Copy data to temporary arrays
- for (int i = 0; i < n1; ++i) {
- leftArray[i] = array[left + i];
- }
- for (int j = 0; j < n2; ++j) {
- rightArray[j] = array[mid + 1 + j];
- }
-
- // Merge the temporary arrays back into the original array
- int i = 0, j = 0, k = left;
- while (i < n1 && j < n2) {
- if (leftArray[i] <= rightArray[j]) {
- array[k++] = leftArray[i++];
- } else {
- array[k++] = rightArray[j++];
- }
- sortSteps.add(array.clone());
- }
-
- // Copy the remaining elements of leftArray[], if there are any
- while (i < n1) {
- array[k++] = leftArray[i++];
- sortSteps.add(array.clone());
- }
-
- // Copy the remaining elements of rightArray[], if there are any
- while (j < n2) {
- array[k++] = rightArray[j++];
- sortSteps.add(array.clone());
- }
- }
-
- /**
- * Shuffles an array.
- * @param array The array to be shuffled.
- */
- private static void shuffleArray(int[] array) {
- Random rand = new Random();
-
- for (int i = 0; i < array.length; i++) {
- int randomIndexToSwap = rand.nextInt(array.length);
- int temp = array[randomIndexToSwap];
- array[randomIndexToSwap] = array[i];
- array[i] = temp;
- }
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java b/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java
deleted file mode 100644
index 8546a32..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/RainbowPattern.java
+++ /dev/null
@@ -1,62 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-
-/**
- * A rainbow pattern that can be applied to an LED strip.
- * This pattern will override the hue of the primary color.
- * The saturation and value of the primary color are applied to the rainbow.
- * The secondary color is not used.
- * @see LEDStrip
- */
-public class RainbowPattern implements LEDPattern {
- // Constants
- private static final int HUE_RANGE = 180;
-
- private int rainbowFirstPixelHue = 0;
- private double hueShiftBuffer = 0;
- private double previousTime = Timer.getFPGATimestamp();
-
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- double patternSeconds = strip.getPatternDuration();
- Color.HSV color = strip.getPrimaryColor().getHSV();
-
- // Calculate delta time
- double newTime = Timer.getFPGATimestamp();
- double deltaTime = newTime - previousTime;
- previousTime = newTime;
-
- // For every pixel
- for (int i = 0; i < length; i++) {
- // Calculate the hue - hue is easier for rainbows because the color
- // shape is a circle so only one value needs to precess
- final int hue = (rainbowFirstPixelHue + (i * HUE_RANGE / length)) % HUE_RANGE;
- // Set the value
- buffer.setHSV(i + offset, hue, color.saturation(), color.value());
- }
-
- // Increase by to make the rainbow "move"
- hueShiftBuffer += ((deltaTime / patternSeconds) * HUE_RANGE);
- int hueShift = (int) hueShiftBuffer;
- hueShiftBuffer -= hueShift;
- rainbowFirstPixelHue += hueShift;
-
- // Check bounds
- rainbowFirstPixelHue %= HUE_RANGE;
- if(rainbowFirstPixelHue < 0) rainbowFirstPixelHue += HUE_RANGE;
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java b/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java
deleted file mode 100644
index 05dc965..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/SolidColorPattern.java
+++ /dev/null
@@ -1,30 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-
-/**
- * A solid color pattern that can be applied to an LED strip.
- * @see LEDStrip
- */
-public class SolidColorPattern implements LEDPattern {
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- Color.HSV color = strip.getPrimaryColor().getHSV();
-
- for (int i = 0; i < length; i++) {
- buffer.setHSV(offset + i, color.hue(), color.saturation(), color.value());
- }
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/presets/addressable/WavePattern.java b/src/main/java/frc/lib/led/presets/addressable/WavePattern.java
deleted file mode 100644
index 9282d3c..0000000
--- a/src/main/java/frc/lib/led/presets/addressable/WavePattern.java
+++ /dev/null
@@ -1,52 +0,0 @@
-// Implementation based on Team 4481's LED Controller
-// https://github.com/FRC-4481-Team-Rembrandts/4481-led-controller
-package frc.lib.led.presets.addressable;
-
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.Timer;
-import frc.lib.led.Color;
-import frc.lib.led.LEDStrip;
-
-/**
- * A wave pattern that can be applied to an LED strip.
- * This pattern will interpolate between the primary and secondary colors.
- * These colors will shift over time.
- * @see LEDStrip
- */
-public class WavePattern implements LEDPattern {
- /**
- * Updates the LED buffer with the pattern.
- *
- * @param buffer The LED buffer to update.
- * @param strip The strip to update the buffer with.
- */
- @Override
- public void updateBuffer(AddressableLEDBuffer buffer, LEDStrip strip) {
- int offset = strip.getOffset();
- int length = strip.getLength();
- Color.RGB primary = strip.getPrimaryColor().getRGB();
- Color.RGB secondary = strip.getSecondaryColor().getRGB();
-
- // Fading is done by changing the value of the color over a sine wave
- double ledWavelength = (2 * Math.PI) / length;
- double timeWavelength = (2 * Math.PI) / strip.getPatternDuration();
-
- // Calculate time offset
- double timeOffset = Timer.getFPGATimestamp() * timeWavelength;
-
- for (int i = 0; i < length; i++) {
- double interpolation = Math.sin(ledWavelength * i + timeOffset) * 0.5 + 0.5;
-
- buffer.setRGB(
- offset + i,
- interpolate(primary.red(), secondary.red(), interpolation),
- interpolate(primary.green(), secondary.green(), interpolation),
- interpolate(primary.blue(), secondary.blue(), interpolation)
- );
- }
- }
-
- private int interpolate(int start, int end, double interpolationValue) {
- return (int) (start + (end - start) * interpolationValue);
- }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 856a82a..4684aef 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -158,7 +158,6 @@ void SetupLog() {
@Override
public void robotPeriodic() {
double startTime = Timer.getFPGATimestamp();
- robotContainer.updateLEDs(); // Updates all LEDs for dynamic patterns.
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled commands, running already-scheduled commands, removing
// finished or interrupted commands, and running subsystem periodic() methods.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 49a5ae3..bee9497 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,14 +19,6 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.lib.SpikeController;
-import frc.lib.led.Color;
-import frc.lib.led.LEDPreset;
-import frc.lib.led.LEDStrip;
-import frc.lib.led.PWMLEDController;
-import frc.lib.led.PWMLEDController.ModePreset;
-import frc.lib.led.presets.addressable.ChasePattern;
-import frc.lib.led.presets.addressable.MergeSortPattern;
-import frc.lib.led.presets.addressable.RainbowPattern;
import frc.robot.commands.SetFlywheelState;
import frc.robot.commands.SetIntakeState;
import frc.robot.commands.DeployIntake;
@@ -74,17 +66,7 @@ public class RobotContainer {
private final Targeting targeting;
private final Findexer findexer;
- private static final PWMLEDController.Mode LED_MODE = PWMLEDController.Mode.ADDRESSABLE;
- private static final int ADDRESSABLE_LED_LENGTH = 160;
-
- private static PWMLEDController ledController;
-
public RobotContainer() {
- ledController = new PWMLEDController(1, LED_MODE);
- configureAddressableStrips();
- configureLEDPresets();
- ledController.start();
-
drive = TunerConstants.createDrivetrain();
this.turret = new Turret();
this.vision = new Vision(drive);
@@ -116,102 +98,6 @@ public RobotContainer() {
SmartDashboard.putData("Auto Path", autoChooser);
configureBindings();
-
- ledController.switchPreset("hub");
- }
-
- private void configureLEDPresets() {
- ledController.registerPreset(createHubPreset());
- ledController.registerPreset(createShuttlePreset());
- ledController.registerPreset(createFixedPreset());
- }
-
- private void configureAddressableStrips() {
- if (!ledController.isAddressable()) {
- return;
- }
-
- // Configure physical strip layout once. Presets only mutate strip state.
- ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH / 2, 0));
- ledController.addStrip(new LEDStrip(ADDRESSABLE_LED_LENGTH / 2, ADDRESSABLE_LED_LENGTH / 2));
- }
-
- private ModePreset createHubPreset() {
- return new ModePreset(
- "hub",
- new LEDPreset("hub", -0.25),
- strips -> {
- LEDStrip strip = getRequiredStrip(strips, 0, "hub");
- strip.setPrimaryColor(Color.fromHex("00F5FF"));
- strip.setSecondaryColor(Color.fromHex("FF007F"));
- strip.setPattern(new RainbowPattern());
- strip.setPatternDuration(2.5);
-
- LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "hub");
- secondaryStrip.setPrimaryColor(Color.fromHex("00F5FF"));
- secondaryStrip.setSecondaryColor(Color.fromHex("FF007F"));
- secondaryStrip.setPattern(new RainbowPattern());
- secondaryStrip.setPatternDuration(2.5);
- }
- );
- }
-
- private ModePreset createShuttlePreset() {
- return new ModePreset(
- "shuttle",
- new LEDPreset("shuttle", -0.23),
- strips -> {
- LEDStrip strip = getRequiredStrip(strips, 0, "shuttle");
- strip.setPrimaryColor(new Color(35, 255, 255));
- strip.setSecondaryColor(new Color(0, 0, 0));
- strip.setPattern(new MergeSortPattern());
- strip.setPatternDuration(1.5);
-
- LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "shuttle");
- secondaryStrip.setPrimaryColor(new Color(35, 255, 255));
- secondaryStrip.setSecondaryColor(new Color(0, 0, 0));
- secondaryStrip.setPattern(new MergeSortPattern());
- secondaryStrip.setPatternDuration(1.5);
- }
- );
- }
-
- private ModePreset createFixedPreset() {
- return new ModePreset(
- "fixed",
- new LEDPreset("fixed", -0.21),
- strips -> {
- LEDStrip strip = getRequiredStrip(strips, 0, "fixed");
- strip.setPrimaryColor(new Color(0, 255, 255));
- strip.setSecondaryColor(new Color(0, 0, 0));
- strip.setPattern(new ChasePattern());
- strip.setPatternDuration(1.0);
-
- LEDStrip secondaryStrip = getRequiredStrip(strips, 1, "fixed");
- secondaryStrip.setPrimaryColor(new Color(0, 255, 255));
- secondaryStrip.setSecondaryColor(new Color(0, 0, 0));
- secondaryStrip.setPattern(new ChasePattern());
- secondaryStrip.setPatternDuration(1.0);
- }
- );
- }
-
- private LEDStrip getRequiredStrip(java.util.List strips, int index, String presetName) {
- if (index < 0 || index >= strips.size()) {
- throw new IllegalStateException(
- "Addressable preset '" + presetName + "' requires strip index " + index
- );
- }
-
- return strips.get(index);
- }
-
- public static PWMLEDController getLEDController() {
- return ledController;
- }
-
- public void updateLEDs() {
- ledController.update();
}
public static CommandSwerveDrivetrain getDrive() {
diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
index 25d9c41..368b878 100644
--- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
@@ -135,7 +135,6 @@ public void setTarget(Target target) {
* Set the target location to center of the hub
*/
private void setPoseTargetingHub() {
- RobotContainer.getLEDController().switchPreset("hub");
overrideBlueAlliance = SmartDashboard.getBoolean("OverrideBlueAlliance", overrideBlueAlliance);
overrideRedAlliance = SmartDashboard.getBoolean("OverrideRedAlliance", overrideRedAlliance);
@@ -165,8 +164,6 @@ private void setPoseTargetingHub() {
* Set the target location to 0, 0
*/
private void setPoseTargetingShuttleRight() {
- RobotContainer.getLEDController().switchPreset("shuttle");
-
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) {
targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, FIELD_WIDTH - shuttlingYOffset);
} else {
@@ -175,8 +172,6 @@ private void setPoseTargetingShuttleRight() {
}
private void setPoseTargetingShuttleLeft() {
- RobotContainer.getLEDController().switchPreset("shuttle");
-
if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get().equals(DriverStation.Alliance.Red)) {
targetPos = new Translation2d(FIELD_LENGTH - shuttlingXOffset, 0 + shuttlingYOffset);
} else {
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index ad6b616..a84a597 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -70,7 +70,6 @@ protected Runnable setupDataRefresher() {
}
public void toggleAimingOverride() {
- RobotContainer.getLEDController().switchPreset("fixed");
this.overrideAutomaticAiming = !this.overrideAutomaticAiming;
}
From 7778a44519a5f67f0522b749741b525ef8f34fc9 Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 16 Apr 2026 10:29:46 -0400
Subject: [PATCH 12/14] Add back old LED code controls
---
src/main/java/frc/lib/led/LEDController.java | 28 +++++++++++++++++++
src/main/java/frc/lib/led/LEDPreset.java | 6 ++++
src/main/java/frc/robot/RobotContainer.java | 26 +++++++++++++++++
.../robot/subsystems/targeting/Targeting.java | 6 ++++
4 files changed, 66 insertions(+)
create mode 100644 src/main/java/frc/lib/led/LEDController.java
create mode 100644 src/main/java/frc/lib/led/LEDPreset.java
diff --git a/src/main/java/frc/lib/led/LEDController.java b/src/main/java/frc/lib/led/LEDController.java
new file mode 100644
index 0000000..952bcc7
--- /dev/null
+++ b/src/main/java/frc/lib/led/LEDController.java
@@ -0,0 +1,28 @@
+package frc.lib.led;
+
+import java.util.HashMap;
+import java.util.Map;
+
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+
+public class LEDController {
+ private static final Map presets = new HashMap<>();
+ private Spark controller;
+
+ public LEDController(int port) {
+ controller = new Spark(port);
+ }
+
+ public void switchPreset(String name) {
+ LEDPreset targetPreset = presets.get(name);
+ if (targetPreset != null) {
+ controller.set(targetPreset.value());
+ } else {
+ System.err.println("Invalid preset name, did you register it?");
+ }
+ }
+
+ public void registerPreset(LEDPreset preset) {
+ presets.put(preset.name(), preset);
+ }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/lib/led/LEDPreset.java b/src/main/java/frc/lib/led/LEDPreset.java
new file mode 100644
index 0000000..9057c58
--- /dev/null
+++ b/src/main/java/frc/lib/led/LEDPreset.java
@@ -0,0 +1,6 @@
+package frc.lib.led;
+
+public record LEDPreset(
+ String name,
+ double value
+) {}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index bee9497..2db0b8f 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,8 @@
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import frc.lib.SpikeController;
+import frc.lib.led.LEDController;
+import frc.lib.led.LEDPreset;
import frc.robot.commands.SetFlywheelState;
import frc.robot.commands.SetIntakeState;
import frc.robot.commands.DeployIntake;
@@ -66,8 +68,11 @@ public class RobotContainer {
private final Targeting targeting;
private final Findexer findexer;
+ private static LEDController ledController;
+
public RobotContainer() {
drive = TunerConstants.createDrivetrain();
+ ledController = new LEDController(1);
this.turret = new Turret();
this.vision = new Vision(drive);
this.intake = new Intake();
@@ -98,6 +103,27 @@ public RobotContainer() {
SmartDashboard.putData("Auto Path", autoChooser);
configureBindings();
+ configureLEDPresets();
+
+ ledController.switchPreset("hub");
+ }
+
+ private void configureLEDPresets() {
+ ledController.registerPreset(
+ new LEDPreset("hub", -0.25)
+ );
+
+ ledController.registerPreset(
+ new LEDPreset("shuttle", -0.23)
+ );
+
+ ledController.registerPreset(
+ new LEDPreset("fixed", -0.21)
+ );
+ }
+
+ public static LEDController getLEDController() {
+ return ledController;
}
public static CommandSwerveDrivetrain getDrive() {
diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
index 368b878..c9ae4c4 100644
--- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
@@ -128,6 +128,12 @@ public void periodic() {
* Set the target of the targeting subsystem. This will change the target position
*/
public void setTarget(Target target) {
+ if (target == Target.HUB) {
+ RobotContainer.getLEDController().switchPreset("hub");
+ } else if (target == Target.SHUTTLE_LEFT || target == Target.SHUTTLE_RIGHT) {
+ RobotContainer.getLEDController().switchPreset("shuttle");
+ }
+
currentTarget = target;
}
From a6ea2dd3492efc51b58d4997f1f1c094ab81ce09 Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 16 Apr 2026 12:18:42 -0400
Subject: [PATCH 13/14] Undoing changes done to aiming while turning
---
.../java/frc/robot/subsystems/shooter/Shooter.java | 7 ++++---
.../frc/robot/subsystems/targeting/ShotData.java | 3 ---
.../frc/robot/subsystems/targeting/Targeting.java | 11 +++++------
.../java/frc/robot/subsystems/turret/Turret.java | 12 ++++++------
4 files changed, 15 insertions(+), 18 deletions(-)
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
index 1b0f67d..a83931f 100644
--- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -4,6 +4,7 @@
import org.littletonrobotics.junction.Logger;
import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.subsystem.SpikeSystem;
import frc.robot.subsystems.targeting.ShotData;
@@ -92,9 +93,9 @@ public boolean isAtTargetRPS() {
* @return
*/
public double getDistanceToTarget() {
- Pose2d toGoal = Targeting.differenceBetweenRobotAndTarget();
- Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getTranslation().getNorm());
- return toGoal.getTranslation().getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input
+ Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget();
+ Logger.recordOutput("Targeting/DistanceToTarget", toGoal.getNorm());
+ return toGoal.getNorm() + io.distanceTrimMeters; // add distance trim to adjust the distance based on operator controller input
}
/**
diff --git a/src/main/java/frc/robot/subsystems/targeting/ShotData.java b/src/main/java/frc/robot/subsystems/targeting/ShotData.java
index 4221700..db9796d 100644
--- a/src/main/java/frc/robot/subsystems/targeting/ShotData.java
+++ b/src/main/java/frc/robot/subsystems/targeting/ShotData.java
@@ -37,9 +37,6 @@ public class ShotData {
distanceToRPM.put(4.9, 2700.0);
distanceToHoodAngle.put(4.9, 29.0);
- distanceToRPM.put(4.9, 2700.0);
- distanceToHoodAngle.put(4.9, 29.0);
-
distanceToRPM.put(5.88, 3000.0);
distanceToHoodAngle.put(5.88, 35.0);
diff --git a/src/main/java/frc/robot/subsystems/targeting/Targeting.java b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
index c9ae4c4..5fecede 100644
--- a/src/main/java/frc/robot/subsystems/targeting/Targeting.java
+++ b/src/main/java/frc/robot/subsystems/targeting/Targeting.java
@@ -52,7 +52,7 @@ public Targeting() {
SmartDashboard.putBoolean("OverrideRedAlliance", overrideRedAlliance);
}
- public static Pose2d differenceBetweenRobotAndTarget() {
+ public static Translation2d differenceBetweenRobotAndTarget() {
// calculate field-relative angle of the turret based on the turret motor position and the robot's heading
// get pose of robo
Pose2d robotPose = RobotContainer.getDrive().getPose();
@@ -88,7 +88,7 @@ public static Pose2d differenceBetweenRobotAndTarget() {
Rotation2d predictedHeading =
robotPose.getRotation().plus(
- Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * tof * ROTATION_TOF_MULTIPLIER)
+ Rotation2d.fromRadians(speeds.omegaRadiansPerSecond * tof)
);
Translation2d predictedTurretPivot = Turret.TURRET_OFFSET_FROM_CENTER
@@ -96,17 +96,16 @@ public static Pose2d differenceBetweenRobotAndTarget() {
.plus(predictedRobotPos);
Translation2d toGoalComp = goalPose.minus(predictedTurretPivot);
-
+
Logger.recordOutput("Targeting/StaticDistance", staticDistance);
Logger.recordOutput("Targeting/PredictedRobotPos", new Pose2d(predictedRobotPos, predictedHeading));
Logger.recordOutput("Targeting/PredictedTurretPivot", new Pose2d(predictedTurretPivot, predictedHeading));
Logger.recordOutput("Targeting/ToGoalCompensation", toGoalComp);
Logger.recordOutput("Targeting/TimeOfFlight", tof);
- return new Pose2d(
+ return new Translation2d(
toGoalXFilter.calculate(toGoalComp.getX()),
- toGoalYFilter.calculate(toGoalComp.getY()),
- predictedHeading
+ toGoalYFilter.calculate(toGoalComp.getY())
);
}
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index a84a597..96b31d0 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -49,17 +49,17 @@ public void onPeriodic() {
if (this.overrideAutomaticAiming) {
this.turretIO.setTurretAngleRobotRelativeDegrees(0);
} else {
- this.turretIO.setTurretAngleRobotRelativeDegrees(getTurretAngleDegreesRobotRelative());
+ this.turretIO.setTurretAngleFieldRelativeDegrees(getTurretAngleDegreesFieldRelative());
}
}
- public double getTurretAngleDegreesRobotRelative() {
- Pose2d toGoal = Targeting.differenceBetweenRobotAndTarget();
+ public double getTurretAngleDegreesFieldRelative() {
+ Translation2d toGoal = Targeting.differenceBetweenRobotAndTarget();
- double robotRelativeAngleToTarget = toGoal.getTranslation().getAngle().minus(toGoal.getRotation()).getDegrees();
- Logger.recordOutput("Targeting/PredictedTargetAngleRobotRelative", robotRelativeAngleToTarget);
- return robotRelativeAngleToTarget;
+ double angleToTarget = toGoal.getAngle().getDegrees();
+ Logger.recordOutput("Targeting/AngleToTargetDeg", angleToTarget);
+ return angleToTarget;
}
From 190c2bb9aafe385737fedb853a1c7becba974bc0 Mon Sep 17 00:00:00 2001
From: NathanEdg
Date: Thu, 16 Apr 2026 13:28:45 -0400
Subject: [PATCH 14/14] change turret pids
---
src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
index dd26676..4471ff5 100644
--- a/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/turret/TurretIOTalonFX.java
@@ -181,7 +181,7 @@ private double getAngularVelocityDegreesPerSecond() {
private Pair getTurretMotionConfigs() {
Slot0Configs configs = new Slot0Configs();
- configs.kP = 150;
+ configs.kP = 300;
configs.kI = 10;
configs.kD = 25;