From 4942ea28dff71a5b20d33bb533db0af6d10d2ecf Mon Sep 17 00:00:00 2001
From: Daniel Burke <danielburke1206@gmail.com>
Date: Thu, 4 Apr 2024 09:41:49 -0400
Subject: [PATCH] Refactored useFallback (v2)

---
 .../robot/commands/AutonomousCommands.java    | 19 ++++++++++++-------
 .../com/team1701/robot/states/RobotState.java | 11 -----------
 2 files changed, 12 insertions(+), 18 deletions(-)

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,