Port over LEDs with segment support and LED modes (#29)

# Description

This pull request adds support for addressable LEDs. It also allows you
to create LEDs with multiple segments so you can set the mode of each
segment to your desire and allows you to seamlessly add your own custom
LED modes.

Fixes #28 

## Type of change

Please delete options that are not relevant.

- [x] New feature (non-breaking change which adds functionality)
- [x] This change requires a documentation update

# How Has This Been Tested?

So far it hasn't, right now this will stay as a draft PR until tested
further

# Checklist:

- [x] My code follows the style guidelines of this project
- [x] I have performed a self-review of my code
- [x] I have commented my code, particularly in hard-to-understand areas
- [x] I have made corresponding changes to the documentation, if any
- [x] My changes generate no new warnings
- [ ] I have performed tests that prove my fix is effective or that my
feature works, if necessary
- [ ] New and existing unit tests pass locally with my changes

---------

Co-authored-by: github-actions <>
Co-authored-by: Cole MacPhail <cole.macphail1@gmail.com>

src/main/java/frc/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.led.LEDSubsystem;
 
 public class RobotContainer {
   final double MaxSpeed = 6; // 6 meters per second desired top speed
@@ -20,6 +21,7 @@ public class RobotContainer {
   CommandController driver = new CommandController(0); // Driver Controller
   CommandController operator = new CommandController(1); // Operator Controller
   CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
+  private final LEDSubsystem ledSubsystem;
   SwerveRequest.FieldCentric drive =
       new SwerveRequest.FieldCentric()
           .withIsOpenLoop(true)
@@ -39,6 +41,8 @@ public class RobotContainer {
   Pose2d odomStart = new Pose2d(0, 0, new Rotation2d(0, 0));
 
   private void configureBindings() {
+    ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
+
     drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
         drivetrain
             .applyRequest(
@@ -78,6 +82,7 @@ private void configureBindings() {
 
   public RobotContainer() {
     configureBindings();
+    ledSubsystem = new LEDSubsystem();
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/led/LEDColour.java

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.led;
+
+/**
+ * This is only used for setting solid colour states. Please see the LEDModes.java class for special
+ * LED modes
+ */
+public class LEDColour {
+  private double red, green, blue;
+
+  public LEDColour(int red, int green, int blue) {
+    this.red = red;
+    this.green = green;
+    this.blue = blue;
+  }
+
+  public LEDColour(double red, double green, double blue) {
+    this.red = red;
+    this.green = green;
+    this.blue = blue;
+  }
+
+  /**
+   * Copies the RGB values from an LEDState object to the current one
+   *
+   * @param ledState The LEDState to copy the RGB values from
+   * @return The current instane of LEDColour with the new RGB values
+   */
+  public LEDColour copy(LEDColour ledState) {
+    this.red = ledState.red;
+    this.green = ledState.green;
+    this.blue = ledState.blue;
+
+    return this;
+  }
+
+  /**
+   * Gets the red value of an LED state object
+   *
+   * @return The double value of the red value in the RGB sequence
+   */
+  public double getRed() {
+    return this.red;
+  }
+
+  /**
+   * Gets the red value of an LED state object as an integer
+   *
+   * @return The integer value of the red value in the RGB sequence
+   */
+  public int getRedInt() {
+    return (int) this.red;
+  }
+
+  /**
+   * Gets the green value of an LED state object
+   *
+   * @return The double value of the green value in the RGB sequence
+   */
+  public double getGreen() {
+    return this.green;
+  }
+
+  /**
+   * Gets the green value of an LED state object as an integer
+   *
+   * @return The integer value of the green value in the RGB sequence
+   */
+  public int getGreenInt() {
+    return (int) this.green;
+  }
+
+  /**
+   * Gets the blue value of an LED state object
+   *
+   * @return The integer value of the blue value in the RGB sequence
+   */
+  public double getBlue() {
+    return this.blue;
+  }
+
+  /**
+   * Gets the blue value of an LED state object as an integer
+   *
+   * @return The integer value of the blue value in the RGB sequence
+   */
+  public int getBlueInt() {
+    return (int) this.blue;
+  }
+}

src/main/java/frc/robot/subsystems/led/LEDConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.led;
+
+public class LEDConstants {
+
+  public static final int ledsPerSegment =
+      16; // The number LEDs (actual diodes) there are on each segment
+}

src/main/java/frc/robot/subsystems/led/LEDSegment.java

@@ -0,0 +1,76 @@
+package frc.robot.subsystems.led;
+
+import frc.robot.subsystems.led.exceptions.InvalidLEDSegmentException;
+import frc.robot.subsystems.led.modes.Breathing;
+import frc.robot.subsystems.led.modes.LEDMode;
+import frc.robot.subsystems.led.modes.Solid;
+
+/**
+ * We are going to attach a name to each index in the LED modes array here. Remember that all
+ * indexes start at 0 and NOT 1
+ */
+public enum LEDSegment {
+
+  // Add all aliases for segments below
+  FrontLeft(0, new Solid(new LEDColour(255, 0, 0))), // Set the LEDs to solid red
+  BackLeft(1, new Breathing(new LEDColour(0, 255, 0))), // Create a green breathing effect
+  BackRight(2, new Breathing(new LEDColour(0, 255, 0))), // Create a green breathing effect
+  FrontRight(3, new Solid(new LEDColour(255, 0, 0))); // Set the LEDs to solid red
+
+  public final int segmentNumber; // The segment number of the LED strip (starts at 1 and goes up)
+  public LEDMode ledMode; // The mode of the LED strip
+
+  private LEDSegment(int segmentNumber, LEDMode defaultLedMode) {
+    this.segmentNumber = segmentNumber;
+    this.ledMode = defaultLedMode;
+  }
+
+  /**
+   * Checks if the LED segment is a valid segment based off of the number of segments in
+   * LEDConstants.java
+   *
+   * @return True if it is within the index bounds, false if it isn't
+   */
+  private boolean isValid() {
+    // Return a boolean based on if the segment number is greater than number of
+    // segments
+    return (this.getSegmentIdentifier() < LEDSubsystem.ledSegments.size());
+  }
+
+  /**
+   * Sets the mode of the current segment
+   *
+   * @param ledMode The mode to set the LEDs to
+   */
+  public void setLedMode(LEDMode ledMode) {
+    // Guard clause to check if the segment is within the bounds of the number of
+    // available segments
+    if (!this.isValid()) {
+      throw new InvalidLEDSegmentException(
+          String.format(
+              "Invalid LED segment: %d. Number of segments: %d",
+              this.getSegmentIdentifier(),
+              LEDSubsystem.ledSegments.size())); // Throw an exception with the segment information
+    }
+
+    this.ledMode = ledMode;
+  }
+
+  /**
+   * Retrieves the true segment number of an LED segment
+   *
+   * @return The segment number of the LED segment
+   */
+  public int getSegmentIdentifier() {
+    return this.segmentNumber;
+  }
+
+  /**
+   * Gets the LED mode for this segment
+   *
+   * @return The LED mode of the current segment as an LEDMode object
+   */
+  public LEDMode getLedMode() {
+    return this.ledMode;
+  }
+}

src/main/java/frc/robot/subsystems/led/LEDSubsystem.java

@@ -0,0 +1,60 @@
+package frc.robot.subsystems.led;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.led.modes.LEDMode;
+
+public class LEDSubsystem extends SubsystemBase {
+
+  public static List<LEDSegment> ledSegments = new ArrayList<>();
+
+  public static AddressableLED leds = new AddressableLED(0); // The PWM port the LEDs are plugged into
+  public static AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(
+      (ledSegments.size() * LEDConstants.ledsPerSegment)); // The buffer that holds the LED data
+
+  @Override
+  public void periodic() {
+    // For every segment that is registered, run the periodic function
+    for (LEDSegment ledSegment : ledSegments) {
+      ledSegment.getLedMode().periodic(ledSegment.getSegmentIdentifier());
+    }
+  }
+
+  /** Does the basic initialization process of setting the length of the LEDs */
+  public void initialize() {
+    // Register all LED segments into the array
+    ledSegments.add(LEDSegment.FrontLeft);
+    ledSegments.add(LEDSegment.BackLeft);
+    ledSegments.add(LEDSegment.BackRight);
+    ledSegments.add(LEDSegment.FrontRight);
+
+    leds.setLength(
+        (ledSegments.size()
+            * LEDConstants.ledsPerSegment)); // Set the length of the LED strip
+
+    leds.start(); // Start the LED strip
+  }
+
+  /**
+   * Sets the mode for all segments available
+   *
+   * @param ledMode The mode to set them all as. Please see the modes directory
+   *                for all available
+   *                modes
+   */
+  public void setAllSegmentModesCommand(LEDMode ledMode) {
+    // For every segment we can set the mode of, set the mode as the one provided
+    for (LEDSegment ledSegment : ledSegments) {
+      ledSegment.setLedMode(ledMode);
+    }
+  }
+
+  /** Initialize the LED subsystem when we create an instance of it */
+  public LEDSubsystem() {
+    this.initialize();
+  }
+}

src/main/java/frc/robot/subsystems/led/README.md

@@ -0,0 +1,36 @@
+## How do I create a new segment of LEDs?
+Creating a new segment could not be easier. Simply just do the following:
+1. Open the `LEDSegment` class in this subsystem directory.
+2. Add a new segment with a proper name at the top of the class with the rest of the segments.
+3. Set the segment number to what number of segment it is. This number will start at 1 from the first segment and will go up by one for each segment you create.
+4. Set the default mode to be something like `SolidRed` or to another mode you've created.
+5. Add the following block of code to the initialize function in the `LEDSubsystem` class, be sure to replace segment name with the name of your segment as specified previously :
+```
+LEDConstants.ledSegments.add(LEDSegment.<segment name>);
+```
+
+You're all set now! You've successfully created a new segment of LEDs for your robot.
+
+## How do I create a new LED mode?
+We've made it simple and easy to create LED modes with this subsystem. Just follow the following steps:
+1. Create a new Java class in the `modes` directory.
+2. Extend that class from the `LEDMode` class.
+3. Add any code you want to repeat every 20ms to the periodic section! This should update your LED's constantly when they're on that mode.
+4. Add any code you want to run initially into the initialize section.
+
+You're done! Our goal was to make this as simple and easy to do as a beginner and with limited Java knowledge.
+
+## I'm getting an "Invalid LED segment" error, how do I fix it?
+It seems that the index for the segment is higher than the number of segments specified in the `LEDConstants` class. Be aware that an index is different than the actual number of segments, it will always be one less than what the segment number is.
+
+Be sure to either change the value of the number of segments, or update the index of your segment.
+
+## Why is my LED segment not turning on?
+This could be for several reason. Please see the following for possible causes:
+- Does your console have any errors in it regarding LEDs?
+- Is the segment registered and added to the ledSegments array in the initialize function in `LEDSubsystem` class?
+- Is the segment created in the `LEDSegment` class and have a valid index and LED mode?
+- Is the number of LEDs per segment the actual number on the physical LED strip?
+- Is the number of segments the correct number?
+
+Hopefully this gets the main points of failure. If for some reason the issue is due to our codebase, feel free to open an issue via our [issue tracker](https://github.com/Simbotics/Simbot-Base/issues). Be sure to follow our issue template when opening an issue.

src/main/java/frc/robot/subsystems/led/exceptions/InvalidLEDSegmentException.java

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.led.exceptions;
+
+public class InvalidLEDSegmentException extends RuntimeException {
+  public InvalidLEDSegmentException(String message) {
+    super(message);
+  }
+}

src/main/java/frc/robot/subsystems/led/modes/Breathing.java

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.led.modes;
+
+import edu.wpi.first.wpilibj.util.Color;
+import frc.robot.subsystems.led.LEDColour;
+import frc.robot.subsystems.led.LEDConstants;
+import frc.robot.subsystems.led.LEDSubsystem;
+
+public class Breathing extends LEDMode {
+  private int cycle;
+  private LEDColour ledColor;
+
+  public Breathing(LEDColour ledColour) {
+    this.ledColor = ledColour;
+  }
+
+  @Override
+  public void initialize() {
+    this.cycle = 1;
+    System.out.println("Starting the Breathing mode");
+  }
+
+  @Override
+  public void periodic(int segmentIndex) {
+    int minSegWindow = segmentIndex * LEDConstants.ledsPerSegment;
+    int maxSegWindow = minSegWindow + LEDConstants.ledsPerSegment;
+
+    for (int i = minSegWindow; i < maxSegWindow; i++) {
+      LEDSubsystem.ledBuffer.setLED(i, calculateBreathingColor());
+    }
+
+    this.cycle++;
+  }
+
+  private Color calculateBreathingColor() {
+    double breathingValue = (Math.sin(Math.PI * this.cycle / 80.0) + 1.0) / 2.0;
+    return new Color(
+        this.ledColor.getRed() * breathingValue,
+        this.ledColor.getGreen() * breathingValue,
+        this.ledColor.getBlue() * breathingValue);
+  }
+}

src/main/java/frc/robot/subsystems/led/modes/LEDMode.java

@@ -0,0 +1,10 @@
+package frc.robot.subsystems.led.modes;
+
+public abstract class LEDMode {
+
+  /** Runs when the mode is first called. This can be used to set constants/variabled */
+  public abstract void initialize();
+
+  /** Runs constantly every 20ms, this is where you want to calculate what your LEDs do */
+  public abstract void periodic(int segmentIndex);
+}