started work on note layer

grt192 · Feb 17, 2024 · 476dbb8 · 476dbb8
1 parent 62b5a19
commit 476dbb8
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -16,7 +16,8 @@ public class LEDSubsystem extends SubsystemBase {
     private final RotaryLEDLayer driveAngleLayer;
     private final RotaryLEDLayer driveBackAngleLayer;
     private final RotaryLEDLayer rainbowLayer;
-    private final LEDLayer aprilDetectedLayer;
+    private final RotaryLEDLayer noteLayer;
+    private final RotaryLEDLayer aprilDetectedLayer;
 
     private final Timer blinkTimer;
     private static final double BLINK_DURATION_SECONDS = 0.5;
@@ -64,7 +65,8 @@ public LEDSubsystem() {
         driveAngleLayer = new RotaryLEDLayer(LED_LENGTH);
         driveBackAngleLayer = new RotaryLEDLayer(LED_LENGTH);
         rainbowLayer = new RotaryLEDLayer(LED_LENGTH);
-        aprilDetectedLayer = new LEDLayer(LED_LENGTH);
+        noteLayer = new RotaryLEDLayer(LED_LENGTH);
+        aprilDetectedLayer = new RotaryLEDLayer(LED_LENGTH);
 
         blinkTimer = new Timer();
         ledTimer = new Timer();

diff --git a/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java b/src/main/java/frc/robot/subsystems/leds/RotaryLEDLayer.java
@@ -69,4 +69,27 @@ public void setRainbow(int offset){
             ));
         }
     }
+
+    public void setGroups(double startAngle, double endAngle, int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset, boolean inverted){
+        for (int i = (int) (colorArray.length * startAngle / (2 * Math.PI)); i < colorArray.length * endAngle / (2 * Math.PI); i++) {
+            int ledNumInSegment = (i + (inverted ? offset : -offset)) % (2 * borderLength + onGroupLength + offGroupLength);
+            if (ledNumInSegment < borderLength){
+                setLED(i, color, opacity *  (ledNumInSegment + 1) / (borderLength + 1));
+            } else if (ledNumInSegment < onGroupLength + borderLength) {
+                setLED(i, color, opacity);
+            } else if(ledNumInSegment < onGroupLength + borderLength * 2){
+                setLED(i, color, opacity * (1 - ((ledNumInSegment - onGroupLength - borderLength + 1.) / (borderLength + 1))));                
+            } else {
+                setLED(i, null, opacity);
+            }
+        }
+    }
+
+    public void setGroups(int onGroupLength, int offGroupLength, int borderLength, Color color, double opacity, int offset){
+        setGroups(0, Math.PI * 2, onGroupLength, offGroupLength, borderLength, color, opacity, offset, false);
+    }
+
+    public void setGroups(int onGroupLength, int offGroupLength, Color color, double opacity, int offset){
+        setGroups(0, Math.PI * 2, onGroupLength, offGroupLength, 0, color, opacity, offset, false);
+    }
 }