From 85728e30ddb469eae9f74391ff8c499762e1cf73 Mon Sep 17 00:00:00 2001 From: Jordan <117786700+jordanjcoderman@users.noreply.github.com> Date: Wed, 8 Nov 2023 20:13:36 -0800 Subject: [PATCH] Maybe fix auto stow after scoring --- .../superstructure/SuperstructureManager.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java index de5a91d..c122f37 100644 --- a/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java +++ b/src/main/java/frc/robot/managers/superstructure/SuperstructureManager.java @@ -45,6 +45,7 @@ public SuperstructureManager( private void setGoal(SuperstructureState goalState) { this.goalState = goalState; if (goalState.equals(States.STOWED)) { + scoringHeight = null; scoringProgress = ScoringProgress.NOT_SCORING; } } @@ -232,18 +233,18 @@ public Command getScoreFinishCommand(Supplier height) { scoringHeight = height.get(); scoringProgress = ScoringProgress.PLACING; }) - .andThen(setStateCommand(() -> getScoringState(height.get()).aligning)) - .andThen(setStateCommand(() -> getScoringState(height.get()).scoring)) + .andThen(setStateCommand(() -> getScoringState(scoringHeight).aligning)) + .andThen(setStateCommand(() -> getScoringState(scoringHeight).scoring)) .andThen( Commands.runOnce( () -> { - scoringHeight = null; - scoringProgress = ScoringProgress.DONE_SCORING; - - if (height.get() == NodeHeight.LOW) { + if (scoringHeight == NodeHeight.LOW) { // Auto stow after scoring low setGoal(States.STOWED); } + + scoringHeight = null; + scoringProgress = ScoringProgress.DONE_SCORING; })) .withName("ScoreFinishCommand"); }