yay works

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index fe58991f626e4ce294e536127005342aa2e03af7..a65a01dfb373095c7df81c5e005143afb0f37c96 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -229,7 +229,7 @@ public class Superstructure extends Command {
             hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
             double x = drivepose.getX(); // compared as meters
             double y = drivepose.getY();
-            System.out.println("X: " + x + "Y: " + y);
+            System.out.println("X: " + Units.metersToInches(x) + "Y: " + Units.metersToInches(y));
             if (FieldConstants.underTrench(x, y)) {
                 hood.forceHoodDown(true);
                 System.out.println("Hood forced down");

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index d114d054c5dac3708bdebf284fc24cf921e972ab..451307a585170ed39b46cf78a393ac1b05dbcd81 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -193,17 +193,14 @@ public class FieldConstants {
 
   public static boolean underTrench(double x, double y) {
     // ensures we aren't in center channel
-    if (y > Units.inchesToMeters(50.0
-    ) && y < FIELD_WIDTH - Units.inchesToMeters(50)) {
+    if (y > Units.inchesToMeters(50.0) && y < FIELD_WIDTH - Units.inchesToMeters(50)) {
       return false;
     }
     // if our location is to far away from right underneath trench in terms of x
-    if (x - Units.inchesToMeters(180.0) > 0.1 || y - FIELD_LENGTH - Units.inchesToMeters(180.0) > 0.1) {
+    // in between blue alliance trench
+    if (!(x > Units.inchesToMeters(152.5) && x < Units.inchesToMeters(187.5)) && !(x < FIELD_LENGTH - Units.inchesToMeters(152.5) && x > FIELD_LENGTH - Units.inchesToMeters(187.5))) {
       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;
   }