fixed field

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index dc629785f66bc034a017983101d04865540e02e2..5dc7aa1ca32bad7c1bb901e642a082f45bac67e4 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -188,6 +188,16 @@ public class FieldConstants {
     }
   }
 
+  public static boolean underTrench(double x, double y) {
+    if (y > Units.inchesToMeters(50.0) && y < FIELD_WIDTH - Units.inchesToMeters(50)) {
+      return false;
+    }
+    if (!(x > Units.inchesToMeters(182.5) && x < Units.inchesToMeters(230.0)) || !(x < Units.inchesToMeters(FIELD_LENGTH - 182.5) && x > Units.inchesToMeters(FIELD_LENGTH - 230.0))) {
+      return false;
+    }
+    return true;
+  }
+  
   /**
    * 
    * @return Whether Y coordinate is in the upper half (left side on blue alliance)