diff --git a/src/main/deploy/pathplanner/BlueLongSide3.path b/src/main/deploy/pathplanner/BlueLongSide3.path index 65ed324..9f027d6 100644 --- a/src/main/deploy/pathplanner/BlueLongSide3.path +++ b/src/main/deploy/pathplanner/BlueLongSide3.path @@ -3,12 +3,12 @@ { "anchorPoint": { "x": 1.831789277613643, - "y": 1.61410638610751 + "y": 1.4100000000000001 }, "prevControl": null, "nextControl": { "x": 2.233698996273372, - "y": 0.3959396976544713 + "y": 0.19183331154696143 }, "holonomicAngle": 180.0, "isReversal": false, diff --git a/src/main/deploy/pathplanner/BlueMidBump2Balance.path b/src/main/deploy/pathplanner/BlueMidBump2Balance.path index 3780cf7..58bc677 100644 --- a/src/main/deploy/pathplanner/BlueMidBump2Balance.path +++ b/src/main/deploy/pathplanner/BlueMidBump2Balance.path @@ -300,4 +300,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/RedLongSide3.path b/src/main/deploy/pathplanner/RedLongSide3.path index e2da7cd..441d721 100644 --- a/src/main/deploy/pathplanner/RedLongSide3.path +++ b/src/main/deploy/pathplanner/RedLongSide3.path @@ -3,12 +3,12 @@ { "anchorPoint": { "x": 14.708210722386356, - "y": 1.61410638610751 + "y": 1.4100000000000001 }, "prevControl": null, "nextControl": { "x": 14.306301003726627, - "y": 0.39593969765447135 + "y": 0.19183331154696148 }, "holonomicAngle": 0, "isReversal": false, diff --git a/src/main/deploy/pathplanner/RedMidBump2Balance.path b/src/main/deploy/pathplanner/RedMidBump2Balance.path index c68e2fa..134e55f 100644 --- a/src/main/deploy/pathplanner/RedMidBump2Balance.path +++ b/src/main/deploy/pathplanner/RedMidBump2Balance.path @@ -300,4 +300,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/RedMidFlat2Balance.path b/src/main/deploy/pathplanner/RedMidFlat2Balance.path index c95c44c..b52a3ce 100644 --- a/src/main/deploy/pathplanner/RedMidFlat2Balance.path +++ b/src/main/deploy/pathplanner/RedMidFlat2Balance.path @@ -300,4 +300,4 @@ ] } ] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/managers/SuperstructureManager.java b/src/main/java/frc/robot/managers/SuperstructureManager.java index 0a9ddf0..eb529e0 100644 --- 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.hasCone()) + 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"); }