From 120fb52af4e0a7cc5fd1a9fda40b940ba42e586f Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Tue, 8 Apr 2025 11:45:24 -0700
Subject: [PATCH 1/2] wait on chassis vel instead of time

---
 .../java/frc/robot/subsystems/Superstructure.java     | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java
index 5551b756..207fc417 100644
--- a/src/main/java/frc/robot/subsystems/Superstructure.java
+++ b/src/main/java/frc/robot/subsystems/Superstructure.java
@@ -799,7 +799,16 @@ private void configureStateTransitionCommands() {
         .whileTrue(shoulder.setTargetAngleSlow(ShoulderSubsystem.SHOULDER_SCORE_PROCESSOR_POS))
         .whileTrue(wrist.setTargetAngle(WristSubsystem.WRIST_SCORE_PROCESSOR_POS))
         .whileTrue(manipulator.setVoltage(-2.0))
-        .and(() -> stateTimer.hasElapsed(2.0))
+        .and(
+            () ->
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                            chassisVel.get().vxMetersPerSecond,
+                            chassisVel.get().vyMetersPerSecond,
+                            chassisVel.get().omegaRadiansPerSecond,
+                            pose.get().getRotation())
+                        .vyMetersPerSecond
+                    < 0)
+        .debounce(0.5)
         .onTrue(this.forceState(SuperState.IDLE));
 
     stateTriggers

From 9664ff1215286f3ce4e91833634ad067ceb17723 Mon Sep 17 00:00:00 2001
From: spellingcat <70864274+spellingcat@users.noreply.github.com>
Date: Tue, 8 Apr 2025 12:12:49 -0700
Subject: [PATCH 2/2] exit based on postiion

---
 .../frc/robot/subsystems/Superstructure.java  | 23 ++++++++++++-------
 1 file changed, 15 insertions(+), 8 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java
index 207fc417..bb099887 100644
--- a/src/main/java/frc/robot/subsystems/Superstructure.java
+++ b/src/main/java/frc/robot/subsystems/Superstructure.java
@@ -1,9 +1,11 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
@@ -20,6 +22,7 @@
 import frc.robot.subsystems.shoulder.ShoulderSubsystem;
 import frc.robot.subsystems.wrist.WristSubsystem;
 import frc.robot.utils.autoaim.AlgaeIntakeTargets;
+import frc.robot.utils.autoaim.AutoAim;
 import frc.robot.utils.autoaim.CoralTargets;
 import frc.robot.utils.autoaim.HumanPlayerTargets;
 import java.util.HashMap;
@@ -801,14 +804,18 @@ private void configureStateTransitionCommands() {
         .whileTrue(manipulator.setVoltage(-2.0))
         .and(
             () ->
-                ChassisSpeeds.fromFieldRelativeSpeeds(
-                            chassisVel.get().vxMetersPerSecond,
-                            chassisVel.get().vyMetersPerSecond,
-                            chassisVel.get().omegaRadiansPerSecond,
-                            pose.get().getRotation())
-                        .vyMetersPerSecond
-                    < 0)
-        .debounce(0.5)
+                !MathUtil.isNear(
+                        pose.get().getX(),
+                        DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                            ? AutoAim.BLUE_PROCESSOR_POS.getX()
+                            : AutoAim.RED_PROCESSOR_POS.getX(),
+                        0.5)
+                    || !MathUtil.isNear(
+                        pose.get().getY(),
+                        DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue
+                            ? AutoAim.BLUE_PROCESSOR_POS.getY()
+                            : AutoAim.RED_PROCESSOR_POS.getY(),
+                        0.5))
         .onTrue(this.forceState(SuperState.IDLE));
 
     stateTriggers