From ed6ba29a4dff97af3bfc57152dae4cf63be42a5a Mon Sep 17 00:00:00 2001
From: QuackitsQuinn <quinnweismiller@gmail.com>
Date: Thu, 21 Mar 2024 19:14:17 -0500
Subject: [PATCH] add AddressableLightStrip

---
 src/main/java/frc/robot/Constants.java        |  9 +++
 src/main/java/frc/robot/RobotContainer.java   |  6 ++
 .../subsystems/AddressableLightStrip.java     | 78 +++++++++++++++++++
 3 files changed, 93 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/AddressableLightStrip.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 413bd29c..ee5d6a67 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -233,4 +233,13 @@ public static class Camera {
     /** The camera id for the intake camera. */
     public static final int INTAKE_CAMERA_ID = 0;
   }
+
+  /** The constants for the light strips */
+  public static class LightStrips {
+    /** The PWM ID for the underglow light strip */
+    public static final int UNDERGLOW_PWM_ID = 0;
+
+    /** The count of leds for the underglow light strip. */
+    public static final int UNDERGLOW_LED_COUNT = 60;
+  }
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index dc023519..61b61628 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -43,6 +43,7 @@
 import frc.robot.commands.mailbox.FireNoteRoutine;
 import frc.robot.commands.mailbox.FireNoteRoutineNoLimitSwitch;
 import frc.robot.commands.mailbox.RunBelts;
+import frc.robot.subsystems.AddressableLightStrip;
 import frc.robot.subsystems.Camera;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drive;
@@ -229,6 +230,11 @@ public class RobotContainer {
   public static SetDrivePerspectiveFieldOriented setDrivePerspectiveFieldOriented =
       new SetDrivePerspectiveFieldOriented();
 
+  /** Singleton instance of {@link AddressableLightStrip} for the whole robot. */
+  public static AddressableLightStrip robotUnderglow =
+      new AddressableLightStrip(
+          Constants.LightStrips.UNDERGLOW_PWM_ID, Constants.LightStrips.UNDERGLOW_LED_COUNT);
+
   /**
    * Singleton instance of {@link SetDrivePerspectiveFieldOrientedHeadingSnapping} for the whole
    * robot.
diff --git a/src/main/java/frc/robot/subsystems/AddressableLightStrip.java b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java
new file mode 100644
index 00000000..0b8a937d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/AddressableLightStrip.java
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/*
+ * Asimov's Laws:
+ * The First Law: A robot may not injure a human being or, through inaction, allow a human being to come to harm.
+ * The Second Law: A robot must obey the orders given it by human beings except where such orders would conflict with the First Law.
+ * The Third Law: A robot must protect its own existence as long as such protection does not conflict with the First or Second Law.
+ */
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class AddressableLightStrip extends SubsystemBase {
+
+  private final AddressableLED ledStrip;
+  private final AddressableLEDBuffer buffer;
+
+  /**
+   * Creates a new AddressableLightStrip
+   *
+   * @param pwmPort The PWM port for the light strip.
+   * @param lightCount The count of lights on the switch
+   */
+  public AddressableLightStrip(int pwmPort, int lightCount) {
+    this.ledStrip = new AddressableLED(pwmPort);
+    this.buffer = new AddressableLEDBuffer(lightCount);
+    this.ledStrip.setLength(lightCount);
+  }
+
+  /**
+   * Sets a light on the light strip to a color in RGB.
+   *
+   * @param lightNumber The light number
+   * @param r The red component
+   * @param g The green component
+   * @param b The blue component
+   */
+  public void setRgbLight(int lightNumber, int r, int g, int b) {
+    this.buffer.setRGB(lightNumber, r, g, b);
+  }
+
+  /**
+   * Sets a light on the light strip to a color in HSV
+   *
+   * @param lightNumber The light number
+   * @param h The hue component
+   * @param s The saturation component
+   * @param v The value component
+   */
+  public void setHSVLight(int lightNumber, int h, int s, int v) {
+    this.buffer.setHSV(lightNumber, h, s, v);
+  }
+
+  /** Fushes the buffer to the light strip. */
+  public void flush() {
+    this.ledStrip.setData(this.buffer);
+  }
+
+  /** Starts the light strip. */
+  public void startLights() {
+    this.ledStrip.start();
+  }
+
+  /** Stops the light strip. */
+  public void stopLights() {
+    this.ledStrip.stop();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}