diff --git a/src/main/java/frc/robot/lights/LightsSubsystem.java b/src/main/java/frc/robot/lights/LightsSubsystem.java index 9c27199..252f80f 100644 --- a/src/main/java/frc/robot/lights/LightsSubsystem.java +++ b/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; diff --git a/src/main/java/frc/robot/managers/superstructure/ScoringProgress.java b/src/main/java/frc/robot/managers/superstructure/ScoringProgress.java new file mode 100644 index 0000000..24a589c --- /dev/null +++ b/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; +} diff --git a/src/main/java/frc/robot/managers/superstructure/States.java b/src/main/java/frc/robot/managers/superstructure/States.java index bbb7236..66c5d10 100644 --- a/src/main/java/frc/robot/managers/superstructure/States.java +++ b/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); diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java index 6a2ff3b..d826c57 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java +++ b/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; })); } }