Add scoring progress into lights

src/main/java/frc/robot/lights/LightsSubsystem.java

@@ -13,6 +13,7 @@
 import frc.robot.intake.HeldGamePiece;
 import frc.robot.intake.IntakeState;
 import frc.robot.intake.IntakeSubsystem;
+import frc.robot.managers.superstructure.ScoringProgress;
 import frc.robot.managers.superstructure.SuperstructureManager;
 import frc.robot.util.scheduling.LifecycleSubsystem;
 import frc.robot.util.scheduling.SubsystemPriority;
@@ -43,6 +44,7 @@ public void enabledPeriodic() {
     HeldGamePiece gamePiece = intake.getGamePiece();
     IntakeState intakeMode = intake.getIntakeState();
     HeldGamePiece superstructureMode = superstructure.getMode();
+    ScoringProgress scoringProgress = superstructure.getScoringProgress();
 
     if (DriverStation.isDisabled()) {
       if (FmsSubsystem.isRedAlliance()) {
@@ -52,6 +54,13 @@ public void enabledPeriodic() {
         color = Color.kBlue;
         blinkPattern = BlinkPattern.SOLID;
       }
+    } else if (scoringProgress == ScoringProgress.ALIGNING
+        || scoringProgress == ScoringProgress.PLACING) {
+      color = Color.kGreen;
+      blinkPattern = BlinkPattern.BLINK_SLOW;
+    } else if (scoringProgress == ScoringProgress.DONE_SCORING) {
+      color = Color.kGreen;
+      blinkPattern = BlinkPattern.BLINK_FAST;
     } else if (gamePiece == HeldGamePiece.CUBE) {
       if (intakeMode == IntakeState.INTAKE_CUBE) {
         color = Color.kPurple;

src/main/java/frc/robot/managers/superstructure/ScoringProgress.java

@@ -0,0 +1,12 @@
+// 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.
+
+package frc.robot.managers.superstructure;
+
+public enum ScoringProgress {
+  NOT_SCORING,
+  ALIGNING,
+  PLACING,
+  DONE_SCORING;
+}

src/main/java/frc/robot/managers/superstructure/States.java

@@ -17,7 +17,7 @@ public class States {
 
   public static final SuperstructureScoringState CUBE_NODE_LOW_BACK =
       new SuperstructureScoringState(Positions.CUBE_NODE_LOW_BACK, IntakeState.OUTTAKE_CUBE);
-      public static final SuperstructureScoringState CUBE_NODE_LOW_FRONT =
+  public static final SuperstructureScoringState CUBE_NODE_LOW_FRONT =
       new SuperstructureScoringState(Positions.CUBE_NODE_LOW_FRONT, IntakeState.OUTTAKE_CUBE);
   public static final SuperstructureScoringState CUBE_NODE_MID =
       new SuperstructureScoringState(Positions.CUBE_NODE_MID, IntakeState.OUTTAKE_CUBE);
@@ -34,7 +34,7 @@ public class States {
 
   public static final SuperstructureScoringState CONE_NODE_LOW_BACK =
       new SuperstructureScoringState(Positions.CONE_NODE_LOW_BACK, IntakeState.OUTTAKE_CONE);
-      public static final SuperstructureScoringState CONE_NODE_LOW_FRONT =
+  public static final SuperstructureScoringState CONE_NODE_LOW_FRONT =
       new SuperstructureScoringState(Positions.CONE_NODE_LOW_FRONT, IntakeState.OUTTAKE_CONE);
   public static final SuperstructureScoringState CONE_NODE_MID =
       new SuperstructureScoringState(Positions.CONE_NODE_MID, IntakeState.OUTTAKE_CONE);

src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java

@@ -26,6 +26,7 @@ public class SuperstructureManager extends LifecycleSubsystem {
   private HeldGamePiece mode = HeldGamePiece.CUBE;
   private NodeHeight scoringHeight = null;
   private IntakeState manualIntakeState = null;
+  private ScoringProgress scoringProgress = ScoringProgress.NOT_SCORING;
 
   public SuperstructureManager(
       SuperstructureMotionManager motionManager, IntakeSubsystem intake, ImuSubsystem imu) {
@@ -40,6 +41,9 @@ public SuperstructureManager(
 
   private void setGoal(SuperstructureState goalState) {
     this.goalState = goalState;
+    if (goalState.equals(States.STOWED)) {
+      scoringProgress = ScoringProgress.NOT_SCORING;
+    }
   }
 
   public boolean atGoal(SuperstructureState state) {
@@ -92,6 +96,10 @@ public HeldGamePiece getMode() {
     return mode;
   }
 
+  public ScoringProgress getScoringProgress() {
+    return scoringProgress;
+  }
+
   public Command getIntakeFloorCommand() {
     return setStateCommand(
         () -> {
@@ -151,6 +159,7 @@ public Command getScoreAlignCommand(NodeHeight height) {
     return Commands.runOnce(
             () -> {
               scoringHeight = height;
+              scoringProgress = ScoringProgress.ALIGNING;
             })
         .andThen(setStateCommand(getScoringState(height).aligning));
   }
@@ -163,12 +172,14 @@ public Command getScoreFinishCommand(NodeHeight height) {
     return Commands.runOnce(
             () -> {
               scoringHeight = height;
+              scoringProgress = ScoringProgress.PLACING;
             })
         .andThen(setStateCommand(getScoringState(height).scoring))
         .andThen(
             Commands.runOnce(
                 () -> {
                   scoringHeight = null;
+                  scoringProgress = ScoringProgress.DONE_SCORING;
                 }));
   }
 }