Leds (#99)

* Updates for new robot

* Merge and fix

* Remove unused imports

* ran spotless

---------

Co-authored-by: DylanTaylor29 <[email protected]>

Author: caparomula

src/main/java/frc/robot/Constants.java

@@ -292,7 +292,7 @@ public static final class ClimberConstants {
   }
 
   public static final class LedConstants {
-    public static final int kLedPort = 0;
+    public static final int kLedPort = 9;
     public static final int kLedBufferLength = 40;
 
     public static final int kLEDsPerBlock = 2;

src/main/java/frc/robot/LEDs/LEDs.java

@@ -2,9 +2,11 @@
 
 import static edu.wpi.first.units.Units.Seconds;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.AddressableLEDBufferView;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.util.Color;
@@ -17,45 +19,48 @@
 
 public class LEDs extends SubsystemBase {
 
-  private final AddressableLED m_led = new AddressableLED(LedConstants.kLedPort);
-  private final AddressableLEDBuffer m_ledData =
+  private final AddressableLED led = new AddressableLED(LedConstants.kLedPort);
+  private final AddressableLEDBuffer ledBuffer =
       new AddressableLEDBuffer(LedConstants.kLedBufferLength);
+  private final AddressableLEDBufferView leftStrip = ledBuffer.createView(20, 39).reversed();
+  private final AddressableLEDBufferView rightStrip = ledBuffer.createView(0, 19);
 
   public LEDs() {
-    m_led.setLength(m_ledData.getLength());
-    m_led.start();
+    led.setLength(ledBuffer.getLength());
+    led.start();
   }
 
   @Override
   public void periodic() {
-    m_led.setData(m_ledData);
+    led.setData(ledBuffer);
   }
 
   private void clearBuffer() {
-    for (var i = 0; i < m_ledData.getLength(); i++) {
-      m_ledData.setRGB(i, 0, 0, 0);
+    for (var i = 0; i < ledBuffer.getLength(); i++) {
+      ledBuffer.setRGB(i, 0, 0, 0);
     }
   }
 
+  private void setBoth(int index, Color color) {
+    leftStrip.setLED(index, color);
+    rightStrip.setLED(index, color);
+  }
+
   private void autoColor(Alliance alliance, int autoMode) {
     clearBuffer();
     int block = LedConstants.kLEDsPerBlock + LedConstants.kLEDsBetweenBlocks;
     if (1 > autoMode) { // 0 indicates no auto selected.
       for (var led = 0; led < LedConstants.kLEDsPerBlock; led++) {
-        m_ledData.setLED(led, Color.kYellow);
+        setBoth(led, Color.kYellow);
       }
     } else {
       for (var mode = 0; mode < autoMode; mode++) {
         for (var i = 0; i < LedConstants.kLEDsPerBlock; i++) {
-          if (alliance.equals(Alliance.Red)) {
-            m_ledData.setLED(i + (mode * block), Color.kRed);
-          } else {
-            m_ledData.setLED(i + (mode * block), Color.kBlue);
-          }
+          setBoth(i + (mode * block), alliance == Alliance.Red ? Color.kRed : Color.kBlue);
         }
       }
     }
-    m_led.setData(m_ledData);
+    led.setData(ledBuffer);
   }
 
   public Command createEnabledCommand() {
@@ -65,6 +70,35 @@ public Command createEnabledCommand() {
         });
   }
 
+  /*
+   * Use LEDs to indicate how to move the robot from the current pose
+   * to the target pose.  The directions must indicate:
+   *
+   * (1) Heading - how to face the robot in the correct direction
+   *   - Use outer pixels to indicate heading: Both green indicates
+   *     proper heading.  Red on one side means rotate away from that
+   *     side.
+   *
+   * (2) Distance - how far from target
+   *   - Some portion of interior pixels light up.  More pixels
+   *     means more distance.  Red means backwards, relative to
+   *     the robot; green means forward, yellow means sideways.
+   *
+   * (3) Direction - which angle to move the robot
+   *   - Offset of pixel block indicates which angle: close to center
+   *     means pretty much straight, close to edge means diagonal.
+   *
+   */
+  public void indicatePoseSeek(Pose2d currentPose, Pose2d targetPose) {
+    var delta = targetPose.minus(currentPose);
+  }
+
+  public Command createPoseSeekingCommand(
+      Supplier<Pose2d> targetPoseSupplier, Supplier<Pose2d> currentPoseSupplier) {
+    return this.run(() -> indicatePoseSeek(currentPoseSupplier.get(), targetPoseSupplier.get()))
+        .ignoringDisable(true);
+  }
+
   public Command createDisabledCommand(
       IntSupplier autoSwitchPositionSupplier,
       Supplier<Alliance> allianceColorSupplier,
@@ -84,7 +118,8 @@ public Command createChangeAutoAnimationCommand() {
     LEDPattern scrollingRainbow = rainbow.scrollAtRelativeSpeed(duration.asFrequency());
     return this.run(
             () -> {
-              scrollingRainbow.applyTo(m_ledData);
+              scrollingRainbow.applyTo(leftStrip);
+              scrollingRainbow.applyTo(rightStrip);
             })
         .withTimeout(duration)
         .ignoringDisable(true);