Fix evil intake stowing too early

team581 · Oct 10, 2023 · 6f9d82f · 6f9d82f
1 parent d8c160a
commit 6f9d82f
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/managers/SuperstructureManager.java b/src/main/java/frc/robot/managers/SuperstructureManager.java
@@ -211,10 +211,13 @@ public Command getShelfIntakeCommand() {
   }
 
   public Command getShelfIntakeEvilCommand() {
-    return getCommand(States.INTAKING_CONE_SHELF_EVIL)
-        // Interrupt command early, start stowing as soon as we are holding a cone - don't wait
-        // until wrist is at goal (since it gets messed up after the impact with the shelf)
-        .until(() -> intake.getGamePiece() == HeldGamePiece.CONE)
+    return Commands.runOnce(() -> intake.setGamePiece(HeldGamePiece.NOTHING), intake)
+        .andThen(
+            getCommand(States.INTAKING_CONE_SHELF_EVIL)
+                // Interrupt command early, start stowing as soon as we are holding a cone - don't
+                // wait until wrist is at goal (since it gets messed up after the impact with the
+                // shelf)
+                .until(() -> intake.getGamePiece() == HeldGamePiece.CONE))
         .andThen(getCommand(States.STOWED))
         .withName("SuperstructureShelfIntakeEvil");
   }