LED rework

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

@@ -446,20 +446,33 @@ public static final class IntakeConstants {
 
     /** Colors for the LEDs based on different robot states (see BCRRobotState) */
     public enum BCRColor {
-      IDLE(255, 255, 255), // White             (nothing running)
-      INTAKING(0, 0, 255), // Blue       (intake running)
-      SHOOTING(0, 255, 0); // Green       (shooter running)
+      IDLE(255, 255, 255), // White   (nothing running)
+      INTAKING(0, 0, 255), // Blue    (intake running)
+      SHOOTER_WITHIN_TARGET_VELOCITY(0, 255, 0),
+      PIECE_PRESENT(255, 30, 0),
+      WRIST_UNCALIBRATED(255, 255, 0),  // CANdle only
+      STICKY_FAULT_PRESENT(255, 0, 0),  // CANdle only
+      CANDLE_IDLE(0, 0, 0);          // CANdle only
 
       public final int r, g, b;
       BCRColor(int r, int g, int b) {
           this.r = r;
           this.g = g;
           this.b = b;
+          this.col = Color.kBlack;
+      }
+
+      public final Color col;
+      BCRColor (Color c) {
+        this.r = 0;
+        this.g = 0;
+        this.b = 0;
+        this.col = c;
       }
   }
 
     public static final class LEDConstants {
-      public static final double accuracyDisplayThreshold = 35; //TODO Decide what the threshold should be
+      public static final double accuracyDisplayThreshold = 35; // TODO Decide what the threshold should be
 
       public static final class Patterns {
           // Static Patterns
@@ -479,10 +492,11 @@ public static final class Patterns {
       }
 
       public enum LEDSegmentRange {
+          // TODO test the changed ranges for the strips
           CANdle(0,8), // Whole CANdle
-          StripLeft(32, 29),  // Left strip only -- D5:  Updated for 2 less LEDs
+          StripLeft(31, 30),  // Left strip only -- D5:  Updated for 2 less LEDs
           StripRight(61, 30), // Right strip only -- D5:  Updated for 2 less LEDs
-          StripHorizontal(8, 24); // Horizontal strip only -- D5:  Updated for 2 less LEDs
+          StripHorizontal(8, 24);
 
           public final int index, count;
           LEDSegmentRange(int index, int count) {

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

@@ -35,6 +35,7 @@
 import frc.robot.commands.Sequences.*;
 import frc.robot.commands.ShooterSetVelocity.VelocityType;
 import frc.robot.subsystems.*;
+import frc.robot.subsystems.LED.StripEvents;
 import frc.robot.utilities.*;
 import frc.robot.utilities.BCRRobotState.ShotMode;
 import frc.robot.utilities.BCRRobotState.State;
@@ -51,20 +52,23 @@ public class RobotContainer {
   private final AllianceSelection allianceSelection = new AllianceSelection(log);
   private final Timer matchTimer = new Timer();
 
+  // Need to define this before LED because LED uses it
+  private final BCRRobotState robotState = new BCRRobotState();
+
+  // Is a subsystem, but requires a utility
+  private final LED led = new LED(Constants.Ports.CANdle1, "LED", matchTimer, log, robotState);
+
   // Define robot subsystems  
   private final DriveTrain driveTrain = new DriveTrain(allianceSelection, log);
-  private final Intake intake = new Intake("Intake", log);
-  private final Shooter shooter = new Shooter(log);
-  private final Feeder feeder = new Feeder(log);
-  private final Wrist wrist = new Wrist(log);
+  private final Intake intake = new Intake("Intake", led, log);
+  private final Shooter shooter = new Shooter(log, led);
+  private final Feeder feeder = new Feeder(log, led);
+  private final Wrist wrist = new Wrist(log, led);
 
   // Define other utilities
   private final TrajectoryCache trajectoryCache = new TrajectoryCache(log);
   private final AutoSelection autoSelection = new AutoSelection(trajectoryCache, allianceSelection, log);
-  private final BCRRobotState robotState = new BCRRobotState();
 
-  // Is a subsystem, but requires a utility
-  private final LED led = new LED(Constants.Ports.CANdle1, "LED", shooter, feeder, robotState, matchTimer, wrist, log);
 
 
   // Define controllers
@@ -416,6 +420,8 @@ public void disabledInit() {
 
     matchTimer.stop();
     SignalLogger.stop();
+
+    led.sendEvent(StripEvents.ROBOT_DISABLED);
   }
 
   /**
@@ -474,6 +480,8 @@ public void teleopInit() {
 
     matchTimer.reset();
     matchTimer.start();
+
+    led.sendEvent(StripEvents.IDLE);
   }
 
   /**

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

@@ -9,6 +9,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.LEDConstants.LEDSegmentRange;
 import frc.robot.subsystems.LED;
+import frc.robot.subsystems.LED.StripEvents;
+
 
 public class CANdleRainbowAnimation extends Command {
   private LED led;
@@ -32,7 +34,7 @@ public CANdleRainbowAnimation(LED led, LEDSegmentRange segment) {
   public void initialize() {
     RainbowAnimation anim = new RainbowAnimation(1, .7, segment.count, false, segment.index);
     led.animate(anim);
-    led.setRainbow();
+    led.sendEvent(StripEvents.RAINBOW);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -43,12 +45,12 @@ public void execute() {}
   @Override
   public void end(boolean interrupted) {
     led.clearAnimation();
-    led.clearRainbow();
+    led.sendEvent(StripEvents.IDLE);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
     return false;
   }
-}
+}

src/main/java/frc/robot/commands/Sequences/ClimbEnd.java

@@ -4,15 +4,15 @@
 
 package frc.robot.commands.Sequences;
 
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.WristConstants;
-import frc.robot.Constants.LEDConstants.LEDSegmentRange;
 import frc.robot.Constants.WristConstants.WristAngle;
-import frc.robot.commands.CANdleRainbowAnimation;
 import frc.robot.commands.WristSetAngle;
 import frc.robot.commands.WristSetPercentOutput;
 import frc.robot.subsystems.LED;
+import frc.robot.subsystems.LED.StripEvents;
 import frc.robot.subsystems.Wrist;
 import frc.robot.utilities.FileLog;
 
@@ -29,7 +29,7 @@ public ClimbEnd(Wrist wrist, FileLog log, LED led) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
-      new CANdleRainbowAnimation(led, LEDSegmentRange.StripHorizontal),
+      new InstantCommand(() -> { led.sendEvent(StripEvents.RAINBOW); }),
       new SequentialCommandGroup(
         new WristSetPercentOutput(WristConstants.climbPercentOutput, wrist, log).until(() -> (wrist.getWristAngle() <= WristAngle.climbStop.value + 5.0)),
         new WristSetAngle(WristAngle.climbStop, wrist, log)

src/main/java/frc/robot/commands/Sequences/ClimbStart.java

@@ -4,13 +4,13 @@
 
 package frc.robot.commands.Sequences;
 
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.Constants.LEDConstants.LEDSegmentRange;
 import frc.robot.Constants.WristConstants.WristAngle;
-import frc.robot.commands.CANdleRainbowAnimation;
 import frc.robot.commands.WristSetAngle;
 import frc.robot.subsystems.LED;
 import frc.robot.subsystems.Wrist;
+import frc.robot.subsystems.LED.StripEvents;
 import frc.robot.utilities.FileLog;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -27,7 +27,7 @@ public ClimbStart(Wrist wrist, FileLog log, LED led) {
     // addCommands(new FooCommand(), new BarCommand());
     addCommands(
       new WristSetAngle(WristAngle.climbStart, wrist, log),
-      new CANdleRainbowAnimation(led, LEDSegmentRange.StripHorizontal)
+      new InstantCommand(() -> { led.sendEvent(StripEvents.RAINBOW); })
     );
   }
 }

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

@@ -562,26 +562,26 @@ public void periodic() {
       SmartDashboard.putNumber("Drive Odometry Y", pose.getTranslation().getY());
       SmartDashboard.putNumber("Drive Odometry Theta", pose.getRotation().getDegrees());
 
-      SmartDashboard.putData("Swerve Drive", new Sendable() {
-        @Override
-        public void initSendable(SendableBuilder builder) {
-          builder.setSmartDashboardType("SwerveDrive");
+      // SmartDashboard.putData("Swerve Drive", new Sendable() {
+      //   @Override
+      //   public void initSendable(SendableBuilder builder) {
+      //     builder.setSmartDashboardType("SwerveDrive");
 
-          builder.addDoubleProperty("Front Left Angle", () -> swerveFrontLeft.getTurningEncoderDegrees()*(Math.PI/180), null);
-          builder.addDoubleProperty("Front Left Velocity", () -> swerveFrontLeft.getDriveEncoderVelocity(), null);
+      //     builder.addDoubleProperty("Front Left Angle", () -> swerveFrontLeft.getTurningEncoderDegrees()*(Math.PI/180), null);
+      //     builder.addDoubleProperty("Front Left Velocity", () -> swerveFrontLeft.getDriveEncoderVelocity(), null);
 
-          builder.addDoubleProperty("Front Right Angle", () -> swerveFrontRight.getTurningEncoderDegrees()*(Math.PI/180), null);
-          builder.addDoubleProperty("Front Right Velocity", () -> swerveFrontRight.getDriveEncoderVelocity(), null);
+      //     builder.addDoubleProperty("Front Right Angle", () -> swerveFrontRight.getTurningEncoderDegrees()*(Math.PI/180), null);
+      //     builder.addDoubleProperty("Front Right Velocity", () -> swerveFrontRight.getDriveEncoderVelocity(), null);
 
-          builder.addDoubleProperty("Back Left Angle", () -> swerveBackLeft.getTurningEncoderDegrees()*(Math.PI/180), null);
-          builder.addDoubleProperty("Back Left Velocity", () -> swerveBackLeft.getDriveEncoderVelocity(), null);
+      //     builder.addDoubleProperty("Back Left Angle", () -> swerveBackLeft.getTurningEncoderDegrees()*(Math.PI/180), null);
+      //     builder.addDoubleProperty("Back Left Velocity", () -> swerveBackLeft.getDriveEncoderVelocity(), null);
 
-          builder.addDoubleProperty("Back Right Angle", () -> swerveBackRight.getTurningEncoderDegrees()*(Math.PI/180), null);
-          builder.addDoubleProperty("Back Right Velocity", () -> swerveBackRight.getDriveEncoderVelocity(), null);
+      //     builder.addDoubleProperty("Back Right Angle", () -> swerveBackRight.getTurningEncoderDegrees()*(Math.PI/180), null);
+      //     builder.addDoubleProperty("Back Right Velocity", () -> swerveBackRight.getDriveEncoderVelocity(), null);
 
-          builder.addDoubleProperty("Robot Angle", () -> getGyroRotation()*(Math.PI/180), null);
-        }
-      });
+      //     builder.addDoubleProperty("Robot Angle", () -> getGyroRotation()*(Math.PI/180), null);
+      //   }
+      // });
       // using vision to update odometry
       SmartDashboard.putBoolean("Vision Updating Odometry", useVisionForOdometry);
 

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

@@ -23,6 +23,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 import frc.robot.Constants.*;
+import frc.robot.subsystems.LED.StripEvents;
 import frc.robot.utilities.FileLog;
 import frc.robot.utilities.Loggable;
 import frc.robot.utilities.StringUtil;
@@ -32,6 +33,7 @@ public class Feeder extends SubsystemBase implements Loggable{
   private boolean fastLogging = false;
   private int logRotationKey;
   private final String subsystemName;
+  private final LED led;
 
   // Create Kraken for feeder motor
   private final TalonFX feeder = new TalonFX(Ports.CANFeeder);
@@ -54,15 +56,17 @@ public class Feeder extends SubsystemBase implements Loggable{
   private boolean velocityControlOn = false;
   private double setpointRPM;
   private double setpointPercent;
+  private boolean lastPiecePresentReading; // true = piece was present in feeder
 
   // Piece sensor inside the intake 
   private final DigitalInput pieceSensor = new DigitalInput(Ports.DIOFeederPieceSensor);
 
   /** Creates a new Feeder. */
-  public Feeder(FileLog log) {
+  public Feeder(FileLog log, LED led) {
     this.log = log;
     logRotationKey = log.allocateLogRotation();
     subsystemName = "Feeder";
+    this.led = led;
 
     // Configure feeder
     feederConfigurator = feeder.getConfigurator();
@@ -94,6 +98,8 @@ public Feeder(FileLog log) {
 
     // Stop Feeder Motor
     stopFeeder();
+
+    lastPiecePresentReading = false;
   }
 
   /**
@@ -183,7 +189,6 @@ public double getFeederVoltage() {
   // ***  Piece sensor
 
   /**
-   * 
    * @return true if piece is in feeder
    */
   public boolean isPiecePresent(){
@@ -201,6 +206,17 @@ public void periodic() {
       SmartDashboard.putNumber(StringUtil.buildString(subsystemName, " RPM"), getFeederVelocity());
       SmartDashboard.putNumber(StringUtil.buildString(subsystemName, " Temp C"), feederTemp.refresh().getValueAsDouble());
       SmartDashboard.putBoolean("Feeder has piece", isPiecePresent());
+
+      // if we have a piece and previously didn't have a piece, update state
+      if (isPiecePresent() && !lastPiecePresentReading) {
+        led.sendEvent(StripEvents.PIECE_PRESENT);
+        lastPiecePresentReading = true;
+      }
+      // if we don't have a piece and previously did have a piece, update state
+      else if (!isPiecePresent() && lastPiecePresentReading) {
+          led.sendEvent(StripEvents.IDLE);
+          lastPiecePresentReading = false;
+      }
     }
   }
 

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

@@ -28,6 +28,7 @@
 import frc.robot.Constants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.Ports;
+import frc.robot.subsystems.LED.StripEvents;
 import frc.robot.utilities.FileLog;
 import frc.robot.utilities.Loggable;
 import static frc.robot.utilities.StringUtil.*;
@@ -60,6 +61,7 @@ public class Intake extends SubsystemBase implements Loggable {
   private final TalonFXConfigurator intakeConfigurator = intakeMotor.getConfigurator();
 	private TalonFXConfiguration intakeConfig;
 	private VoltageOut intakeVoltageControl = new VoltageOut(0.0);
+  private final LED led;
 
   // Create Falcon variables for intake motor
   private final StatusSignal<Voltage> intakeSupplyVoltage;				// Incoming bus voltage to motor controller, in volts
@@ -78,9 +80,10 @@ public class Intake extends SubsystemBase implements Loggable {
    * @param subsystemName
    * @param log
    */
-  public Intake(String subsystemName, FileLog log) {
+  public Intake(String subsystemName, LED led, FileLog log) {
     this.log = log; // save reference to the fileLog
     this.subsystemName = subsystemName;
+    this.led = led;
     logRotationKey = log.allocateLogRotation();
     currentTimer.reset();
     currentTimer.start();
@@ -154,6 +157,12 @@ public String getName() {
    */
   public void setIntakePercentOutput(double percent){
     intakeMotor.setControl(intakeVoltageControl.withOutput(percent*IntakeConstants.compensationVoltage));
+    if (percent > 0) {
+      led.sendEvent(StripEvents.INTAKING);
+    }
+    else {
+      led.sendEvent(StripEvents.IDLE);
+    }
   }
 
   /**