diff --git a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java index ff865e5f..25c75e6c 100644 --- a/src/main/java/com/team1701/robot/commands/AutonomousCommands.java +++ b/src/main/java/com/team1701/robot/commands/AutonomousCommands.java @@ -196,11 +196,7 @@ private Command followChoreoPath(String pathName, boolean resetPose) { } private Command driveBackPreWarmAndShoot(String pathName) { - return loggedSequence( - pauseDrive(pathName), - followChoreoPathAndPreWarm(pathName), - aimAndShoot(), - runOnce(() -> mRobotState.setUseAutonFallback(false))); + return loggedSequence(pauseDrive(pathName), followChoreoPathAndPreWarm(pathName), aimAndShoot()); } private Command driveToNextPiece(AutoNote nextNote) { @@ -246,15 +242,24 @@ private Pose2d getFinalPose(String pathName) { return trajectory == null ? GeometryUtil.kPoseIdentity : trajectory.getFinalPose(); } + private class UseEfficientAutonFallback { + boolean useFallback = false; + + public UseEfficientAutonFallback(boolean use) { + useFallback = use; + } + } + private Command efficientlyPreWarmShootAndDrive(String pathName, String returnPath, AutoNote nextNote) { + var useAutonFallback = new UseEfficientAutonFallback(false); return race( driveBackPreWarmAndShoot(pathName).andThen(followChoreoPathAndSeekNote(returnPath)), waitSeconds(.7) .andThen(either( idle(), - runOnce(() -> mRobotState.setUseAutonFallback(true)), + runOnce(() -> useAutonFallback.useFallback = true), mRobotState::hasNote))) - .andThen(either(driveToNextPiece(nextNote), none(), mRobotState::getUseAutonFallback)); + .andThen(either(driveToNextPiece(nextNote), none(), () -> useAutonFallback.useFallback)); } private Command followChoreoPathAndSeekNote(String pathName) { diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java index 6307d17a..7fafc548 100644 --- a/src/main/java/com/team1701/robot/states/RobotState.java +++ b/src/main/java/com/team1701/robot/states/RobotState.java @@ -58,8 +58,6 @@ public class RobotState { private ShootingState mShootingState = ShootingState.kDefault; - private boolean mUseAutonFallback = false; - public RobotState() { mField = new Field2d(); SmartDashboard.putData("Field", mField); @@ -391,15 +389,6 @@ public boolean isClimbMode() { return mScoringMode == ScoringMode.CLIMB; } - public boolean getUseAutonFallback() { - return mUseAutonFallback; - } - - public boolean setUseAutonFallback(boolean use) { - mUseAutonFallback = use; - return mUseAutonFallback; - } - public enum ScoringMode { SPEAKER, AMP,