Maybe fix auto stow after scoring

team581 · Nov 9, 2023 · 85728e3 · 85728e3
1 parent 2013a17
commit 85728e3
Showing 1 changed file with 7 additions and 6 deletions.
diff --git 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<NodeHeight> 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");
   }