Add LEDs

src/main/java/frc/lib/util/swerve/SwerveModuleReal.java

@@ -54,14 +54,14 @@ public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, in
         driveMotorSelectedPosition = mDriveMotor.getPosition();
         driveMotorSelectedSensorVelocity = mDriveMotor.getVelocity();
         absolutePositionAngleEncoder = angleEncoder.getAbsolutePosition();
-        mAngleMotor.restoreFactoryDefaults();
 
 
 
     }
 
     private void configAngleMotor() {
         /* Angle Motor Config */
+        this.mAngleMotor.restoreFactoryDefaults();
 
         /* Motor Inverts and Neutral Mode */
         this.mAngleMotor.setInverted(false);

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

@@ -12,6 +12,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.util.Color;
 
 /**
  * Constants file.
@@ -199,6 +200,18 @@ public static final class LightsaberConstants {
         public static final int ls3 = 7;
     }
 
+    /**
+     * LED constants.
+     */
+    public static final class LEDConstants {
+        public static final int PWM_PORT = 0;
+        public static final int LED_COUNT = 60;
+        public static final Color INTAKE_COLOR = Color.kGreen;
+        public static final Color INDEXER_COLOR = Color.kPurple;
+        public static final Color ALERT_COLOR = Color.kWhite;
+    }
+
+
     /**
      * Auto constants
      */

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

@@ -6,12 +6,15 @@
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Robot.RobotRunType;
+import frc.robot.commands.MovingColorLEDs;
 import frc.robot.commands.TeleopSwerve;
+import frc.robot.subsystems.LEDs;
 import frc.robot.subsystems.lightsabers.Lightsaber;
 import frc.robot.subsystems.lightsabers.LightsaberIO;
 import frc.robot.subsystems.lightsabers.LightsaberReal;
@@ -38,6 +41,7 @@ public class RobotContainer {
     /* Subsystems */
     private Swerve s_Swerve;
     private Lightsaber s_Lightsabers;
+    private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT);
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -60,6 +64,7 @@ public RobotContainer(RobotRunType runtimeType) {
         // Configure the button bindings
         s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
             Constants.Swerve.isFieldRelative, Constants.Swerve.isOpenLoop));
+        leds.setDefaultCommand(new MovingColorLEDs(leds, Color.kRed, 4, false));
         configureButtonBindings();
     }
 
@@ -71,6 +76,7 @@ public RobotContainer(RobotRunType runtimeType) {
      */
     private void configureButtonBindings() {
         driver.a().whileTrue(s_Lightsabers.turnLightsabers(.2));
+        driver.b().whileTrue(new MovingColorLEDs(leds, Color.kBlue, 4, false));
     }
 
     /**

src/main/java/frc/robot/commands/FlashingLEDColor.java

@@ -0,0 +1,65 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LEDs;
+
+/** Command to flash LEDs on and off once per second. */
+public class FlashingLEDColor extends Command {
+    private LEDs leds;
+    private int ledLength;
+    private int flashingDelay = 0;
+    private Color color;
+    private Color altColor;
+
+    /**
+     * Command to flash the LED strip between 2 colors
+     *
+     * @param leds LED Subsystem
+     * @param color The first color
+     * @param altColor The second color
+     */
+    public FlashingLEDColor(LEDs leds, Color color, Color altColor) {
+        this.leds = leds;
+        this.color = color;
+        this.altColor = altColor;
+        ledLength = leds.getLength();
+        addRequirements(leds);
+    }
+
+    /**
+     * Command to flash the LED strip between a color and black
+     *
+     * @param leds LED Subsystem
+     * @param color The color
+     */
+    public FlashingLEDColor(LEDs leds, Color color) {
+        this(leds, color, Color.kBlack);
+    }
+
+    @Override
+    public void execute() {
+        if (flashingDelay < 10) {
+            for (var i = 0; i < ledLength; i++) {
+                leds.setColor(i, color);
+            }
+        } else {
+            for (var i = 0; i < ledLength; i++) {
+                leds.setColor(i, altColor);
+            }
+        }
+        leds.setData();
+        flashingDelay++;
+        flashingDelay %= 20;
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return true;
+    }
+}

src/main/java/frc/robot/commands/MovingColorLEDs.java

@@ -0,0 +1,67 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LEDs;
+
+/**
+ * Command to move LEDs back and forth like a Cylon eye
+ */
+public class MovingColorLEDs extends Command {
+    private final LEDs leds;
+    private int ledLength;
+    private Color color;
+    private int count;
+    private boolean inverted = false;
+    private int movingColorDelay = 0;
+    private int movingLED = 0;
+    private boolean movingDirection = true;
+
+    /**
+     * Command to move LEDs back and forth like a Cylon eye
+     *
+     * @param leds LED subsystem
+     * @param color Color to which to set the LEDs
+     * @param count The number of LEDs in the moving dot. Best is an odd number.
+     * @param inverted Whether to invert the Color that moves.
+     */
+    public MovingColorLEDs(LEDs leds, Color color, int count, boolean inverted) {
+        this.leds = leds;
+        this.color = color;
+        this.count = count + 2;
+        this.inverted = inverted;
+        ledLength = leds.getLength();
+        addRequirements(leds);
+    }
+
+    @Override
+    public void execute() {
+        Color theColor = inverted ? Color.kBlack : color;
+        Color secondColor = inverted ? color : Color.kBlack;
+        if (movingColorDelay == 0) {
+            for (var i = 0; i < ledLength; i++) {
+                if (Math.abs(i - movingLED) < count) {
+                    leds.setColor(i, theColor);
+                } else {
+                    leds.setColor(i, secondColor);
+                }
+            }
+            if (movingDirection) {
+                movingLED++;
+            } else {
+                movingLED--;
+            }
+            if (movingLED >= ledLength - 1 || movingLED <= 0) {
+                movingDirection = !movingDirection;
+            }
+            leds.setData();
+        }
+        movingColorDelay += 1;
+        movingColorDelay %= 1;
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+        return true;
+    }
+}

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

@@ -0,0 +1,110 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This is the class header for the LEDs Subsystem
+ */
+public class LEDs extends SubsystemBase {
+    private AddressableLEDBuffer controLedBuffer;
+    @SuppressWarnings("IOCheck")
+    private AddressableLED addressableLED;
+
+    /**
+     * constructs a LED Subsystem
+     *
+     * @param length length of the addressable LEDS
+     * @param port port ID for PWM
+     */
+    public LEDs(int length, int port) {
+        controLedBuffer = new AddressableLEDBuffer(length);
+        addressableLED = new AddressableLED(port);
+
+        addressableLED.setLength(controLedBuffer.getLength());
+        addressableLED.setData(controLedBuffer);
+        addressableLED.start();
+    }
+
+    /**
+     * Get LED strip length
+     *
+     * @return number of LEDs
+     */
+    public int getLength() {
+        return controLedBuffer.getLength();
+    }
+
+    /**
+     * Set individual LED color via HSV
+     *
+     * @param index the LED index to set
+     * @param h the h value [0-180)
+     * @param s the s value [0-255]
+     * @param v the v value [0-255]
+     */
+    public void setHSV(int index, int h, int s, int v) {
+        controLedBuffer.setHSV(index, h, s, v);
+    }
+
+    /**
+     * Sets the LED output data.
+     */
+    public void setData() {
+        addressableLED.setData(controLedBuffer);
+    }
+
+    /**
+     * Set individual LED color via RGB
+     *
+     * @param index the LED index to set
+     * @param r the r value [0-255]
+     * @param g the g value [0-255]
+     * @param b the b value [0-255]
+     */
+    public void setRGB(int index, int r, int g, int b) {
+        controLedBuffer.setRGB(index, r, g, b);
+    }
+
+    /**
+     * Sets RGB Color of the entire LED strip
+     *
+     * @param r - [0 - 255]
+     * @param g - [0 - 255]
+     * @param b - [0 - 255]
+     */
+    public void setRGB(int r, int g, int b) {
+        for (var i = 0; i < getLength(); i++) {
+            setRGB(i, r, g, b);
+        }
+        setData();
+    }
+
+    /**
+     * Set individual LED color via Color
+     *
+     * @param index the LED index to set
+     * @param color The color of the LED
+     */
+    public void setColor(int index, Color color) {
+        controLedBuffer.setLED(index, color);
+    }
+
+    /**
+     * Sets the Color of the entire LED strip
+     *
+     * @param color color set for the LEDs
+     */
+    public void setColor(Color color) {
+        for (var i = 0; i < getLength(); i++) {
+            setColor(i, color);
+        }
+        setData();
+    }
+
+    public Color getColor(int index) {
+        return controLedBuffer.getLED(index);
+    }
+}