From c9afdbb68c5eeb9db64a0e90d9f71718abfd3189 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Mon, 23 Feb 2026 18:25:58 -0800
Subject: [PATCH] phase manager

---
 .../frc/robot/subsystems/turret/Turret.java   |  2 +-
 .../java/frc/robot/util/PhaseManager.java     | 81 +++++++++++++++++++
 2 files changed, 82 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/util/PhaseManager.java

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 363927b..cbab18d 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -143,7 +143,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 	 * @return If the turret is at setpoint with tolerance of 2 degrees
 	 */
 	public boolean atSetpoint() {
-		return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(2.0);
+		return Math.abs(goalAngle.getRadians() - getPositionRad()) < Units.degreesToRadians(5.0);
 	}
 
 	/**
diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java
new file mode 100644
index 0000000..54250fb
--- /dev/null
+++ b/src/main/java/frc/robot/util/PhaseManager.java
@@ -0,0 +1,81 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.constants.FieldConstants;
+import frc.robot.constants.FieldConstants.FieldZone;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.turret.Turret;
+
+public class PhaseManager {
+    
+    public enum WantedState {
+        IDLE,
+        SHOOTING,
+        PASSING
+    }
+
+    public enum CurrentState {
+        IDLE,
+        STARTING_UP,
+        TURNING_AROUND,
+        SHOOTING,
+        PASSING
+    }
+
+    private WantedState wantedState = WantedState.SHOOTING;
+    private CurrentState currentState = CurrentState.IDLE;
+
+    public void update(Pose2d drivePose, Shooter shooter, Turret turret) {
+        updateWantedState(drivePose);
+        updateCurrentState(drivePose, shooter, turret);
+    }
+
+    private void updateCurrentState(Pose2d drivePose, Shooter shooter, Turret turret) {
+        // if shooter is not trying to run -- idle
+        if (shooter.getTargetVelocityMPS() == 0.0) {
+            currentState = CurrentState.IDLE;
+            return;
+        }
+        // if shooter velocity not ready yet -- starting up
+        if (!shooter.atTargetVelocity()) { // TODO: but then what happens when the ball goes in and the velocity dips??
+            currentState = CurrentState.STARTING_UP;
+            return;
+        }
+        // if turret is not at setpoint -- turning around
+        if (!turret.atSetpoint()) {
+            currentState = CurrentState.TURNING_AROUND;
+            return;
+        }
+        
+        FieldZone zone = FieldConstants.getZone(drivePose.getTranslation());
+        if (zone == FieldConstants.FieldZone.ALLIANCE) {
+            currentState = CurrentState.SHOOTING;
+        } else {
+            currentState = CurrentState.PASSING;
+        }
+    }
+
+    private void updateWantedState(Pose2d drivePose) {
+        FieldZone zone = FieldConstants.getZone(drivePose.getTranslation());
+        if (zone == FieldConstants.FieldZone.ALLIANCE) {
+            wantedState = WantedState.SHOOTING;
+        } else {
+            wantedState = WantedState.PASSING;
+        }
+    }
+
+    public WantedState getWantedState() { 
+        return wantedState; 
+    }
+    public CurrentState getCurrentState() { 
+        return currentState; 
+    }
+    
+    public boolean isIdle() { 
+        return wantedState == WantedState.IDLE; 
+    }
+    
+    public boolean shouldFeed() {
+        return !(currentState == CurrentState.STARTING_UP) && !(currentState == CurrentState.TURNING_AROUND);
+    }
+}
\ No newline at end of file
-- 
2.39.5