Addressable rgb

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

@@ -14,6 +14,7 @@
 import com.pathplanner.lib.util.PIDConstants;
 import com.revrobotics.CANSparkBase.IdleMode;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.util.Color;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -234,4 +235,33 @@ public static final 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 = 150;
+
+    /** The speed of the fade animation. [0, 1] */
+    public static final double STRIP_FADE_SPEED = 0.05;
+
+    public static final class Colors {
+      /** The color for the lights when the robot is disabled. */
+      public static final Color DISABLED_COLOR = Color.fromHSV(15, 255, 100);
+
+      /** The color for the pulse during the boot sequence. */
+      public static final Color BOOT_SEQUENCE_PULSE_COLOR = Color.fromHSV(0, 0, 255);
+
+      /** The enable color for when the robot is not on an alliance */
+      public static final Color ENABLE_COLOR_NO_ALLIANCE = Color.fromHSV(137, 255, 200);
+
+      /** The enable color for when the robot is on the blue alliance */
+      public static final Color ENABLE_COLOR_BLUE_ALLIANCE = Color.fromHSV(120, 255, 200);
+
+      /** The enable color for when the robot is on the red alliance */
+      public static final Color ENABLE_COLOR_RED_ALLIANCE = Color.fromHSV(0, 255, 200);
+    }
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -35,6 +35,7 @@ public void robotPeriodic() {
 
   @Override
   public void disabledInit() {
+    CommandScheduler.getInstance().schedule(RobotContainer.disabledLights);
     RobotContainer.mailboxPneumatics.off();
   }
 
@@ -44,6 +45,7 @@ public void disabledPeriodic() {}
   @Override
   public void disabledExit() {
     RobotContainer.mailboxPneumatics.retract();
+    RobotContainer.disabledLights.cancel();
   }
 
   @Override

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

@@ -37,12 +37,16 @@
 import frc.robot.commands.drive.SetDrivePerspectiveFieldOrientedHeadingSnapping;
 import frc.robot.commands.drive.SetDrivePerspectiveRobotOriented;
 import frc.robot.commands.drive.ZeroGyro;
+import frc.robot.commands.lightstrip.DisabledLight;
+import frc.robot.commands.lightstrip.EnabledLight;
+import frc.robot.commands.lightstrip.NodeLight;
 import frc.robot.commands.mailbox.DeindexNote;
 import frc.robot.commands.mailbox.DeployMailbox;
 import frc.robot.commands.mailbox.DeployPneumatics;
 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 +233,11 @@ public class RobotContainer {
   public static SetDrivePerspectiveFieldOriented setDrivePerspectiveFieldOriented =
       new SetDrivePerspectiveFieldOriented();
 
+  /** Singleton instance of {@link AddressableLightStrip} for the whole robot. */
+  public static AddressableLightStrip robotLights =
+      new AddressableLightStrip(
+          Constants.LightStrips.UNDERGLOW_PWM_ID, Constants.LightStrips.UNDERGLOW_LED_COUNT);
+
   /**
    * Singleton instance of {@link SetDrivePerspectiveFieldOrientedHeadingSnapping} for the whole
    * robot.
@@ -257,6 +266,15 @@ public class RobotContainer {
   /** Singleton instance of {@link ZeroGyro} for the whole robot. */
   public static ZeroGyro zeroGyro = new ZeroGyro();
 
+  /** Singleton instance of {@link DisabledLight} for the whole robot. */
+  public static DisabledLight disabledLights = new DisabledLight();
+
+  /** Singleton instance of {@link EnableLights} for the whole robot. */
+  public static EnabledLight enabledLights = new EnabledLight();
+
+  /** Singleton instance of {@link NodeLight} for the whole robot. */
+  public static NodeLight nodeLight = new NodeLight();
+
   /*
    * ***********************
    * * OTHER INSTANCE VARS *
@@ -286,6 +304,7 @@ public RobotContainer() {
     startIntakeCamera.schedule();
 
     drive.setDefaultCommand(driveFieldOrientedHeadingSnapping);
+    robotLights.setDefaultCommand(enabledLights);
   }
 
   private void configureAuto() {

src/main/java/frc/robot/commands/lightstrip/DisabledLight.java

@@ -0,0 +1,81 @@
+// 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.commands.lightstrip;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.AddressableLightStrip;
+
+/** Activates the light strip when the robot is disabled. */
+public class DisabledLight extends Command {
+  private AddressableLightStrip robotLights;
+  private boolean firstStart = true;
+  private int rainbowTick = 0;
+  private int ledCount = 1;
+
+  /** Creates a new RobotDisabledLights. */
+  public DisabledLight() {
+    this.robotLights = RobotContainer.robotLights;
+    addRequirements(this.robotLights);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (!firstStart) {
+      robotLights.setStripColor(Constants.LightStrips.Colors.DISABLED_COLOR);
+      robotLights.flush();
+    } else {
+      robotLights.setFadeSpeed(0.01);
+    }
+    robotLights.startLights();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (firstStart) {
+      rainbowTick++;
+      for (int led = 0; led < ledCount; led++) {
+        robotLights.setColorLight(
+            led, Color.fromHSV((led + rainbowTick) % 180, 255, rainbowTick - led));
+      }
+      robotLights.flush();
+      if ((rainbowTick >= robotLights.getLength() * 2)) {
+        firstStart = false;
+        robotLights.setStripColorRaw(Constants.LightStrips.Colors.BOOT_SEQUENCE_PULSE_COLOR);
+        robotLights.setStripColor(Constants.LightStrips.Colors.DISABLED_COLOR);
+      }
+      ledCount = Math.min(ledCount + 1, robotLights.getLength());
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    firstStart = false;
+    robotLights.resetFadeSpeed();
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/lightstrip/EnabledLight.java

@@ -0,0 +1,62 @@
+// 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.commands.lightstrip;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.AddressableLightStrip;
+import java.util.Optional;
+
+/** Activates the light strip when the robot is enabled. */
+public class EnabledLight extends Command {
+  private AddressableLightStrip robotLights;
+
+  /** Creates a new RobotEnabledLights. */
+  public EnabledLight() {
+    this.robotLights = RobotContainer.robotLights;
+    addRequirements(this.robotLights);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+    if (alliance.isPresent()) {
+      if (alliance.get() == Alliance.Red) {
+        robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_RED_ALLIANCE);
+      } else {
+        robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_BLUE_ALLIANCE);
+      }
+    } else {
+      robotLights.setStripColor(Constants.LightStrips.Colors.ENABLE_COLOR_NO_ALLIANCE);
+    }
+    robotLights.flush();
+    robotLights.startLights();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/commands/lightstrip/NodeLight.java

@@ -0,0 +1,49 @@
+// 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.commands.lightstrip;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.AddressableLightStrip;
+
+/** Activates the light strip when the robot has a node. */
+public class NodeLight extends Command {
+  private AddressableLightStrip robotLights;
+
+  /** Creates a new RobotNodeLight. */
+  public NodeLight() {
+    this.robotLights = RobotContainer.robotLights;
+    addRequirements(this.robotLights);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    robotLights.startLights();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    robotLights.updateRainbow();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}