gradual zones

diff --git a/src/main/java/frc/robot/util/PhaseManager.java b/src/main/java/frc/robot/util/PhaseManager.java
index c5e445191f1c9a66fb9adbdf80215d35371a4436..27a085e27eab20dec4eb7030b86f35221795ea54 100644 (file)
--- a/src/main/java/frc/robot/util/PhaseManager.java
+++ b/src/main/java/frc/robot/util/PhaseManager.java
@@ -85,10 +85,21 @@ public class PhaseManager {
     }
 
     public Translation2d getTarget(Pose2d drivePose) {
-        return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d()
-                : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation())
-                //TODO: reversed for sm reason
-                        ? FieldConstants.getAllianceSideTranslation(false).toTranslation2d()
-                        : FieldConstants.getAllianceSideTranslation(true).toTranslation2d());
+        // return wantedState == WantedState.SHOOTING ? FieldConstants.getHubTranslation().toTranslation2d()
+        //         : (FieldConstants.isOnLeftSideOfField(drivePose.getTranslation())
+        //         //TODO: reversed for sm reason
+        //                 ? FieldConstants.getAllianceSideTranslation(false).toTranslation2d()
+        //                 : FieldConstants.getAllianceSideTranslation(true).toTranslation2d());
+        if (wantedState == WantedState.SHOOTING) {
+            return FieldConstants.getHubTranslation().toTranslation2d();
+        } else  {
+            double targetY;
+            if (drivePose.getY() > 4.0) {
+                targetY = FieldConstants.FIELD_WIDTH * 1.5 - drivePose.getY();
+            } {
+                targetY = FieldConstants.FIELD_WIDTH * 0.5 - drivePose.getY();
+            }
+            return new Translation2d(FieldConstants.getAllianceSideTranslation(true).getX(), targetY);
+        }
     }
-}
\ No newline at end of file
+}