Add scoring progress into lights (#29)

* scoring progress into superstructure manager

* continue scoring state and format

* implement 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/SuperstructureManager.java

@@ -27,6 +27,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) {
@@ -41,6 +42,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) {
@@ -93,6 +97,10 @@ public HeldGamePiece getMode() {
     return mode;
   }
 
+  public ScoringProgress getScoringProgress() {
+    return scoringProgress;
+  }
+
   public Command getIntakeFloorCommand() {
     return setStateCommand(
         () -> {
@@ -154,6 +162,7 @@ public Command getScoreAlignCommand(NodeHeight height) {
     return Commands.runOnce(
             () -> {
               scoringHeight = height;
+              scoringProgress = ScoringProgress.ALIGNING;
             })
         .andThen(setStateCommand(getScoringState(height).aligning));
   }
@@ -166,12 +175,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;
                 }));
   }