From: Wesley28w <wesleycwong@gmail.com>
Date: Tue, 24 Mar 2026 01:31:07 +0000 (-0700)
Subject: okay lets try this
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=3fcb686dcd07e6814f75bc41fc74b1c3cff5724a;p=FRC2026.git

okay lets try this
---

diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index ce01637..e7e6829 100644
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -227,7 +227,10 @@ public class Superstructure extends Command {
             // }
 
             hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity);
-            if (FieldConstants.underTrench(drivepose.getX(), drivepose.getY())) {
+            double x = drivepose.getX();
+            double y = drivepose.getY();
+            System.out.println("X: " + x + "Y: " + y);
+            if (FieldConstants.underTrench(x, y)) {
                 hood.forceHoodDown(true);
             } else {
                 hood.forceHoodDown(false);
diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 7adbf88..6445508 100644
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -47,6 +47,7 @@ public class FieldConstants {
   public static final Translation3d HUB_RED =
       new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11), FIELD_WIDTH/2, Units.inchesToMeters(72));
     
+  // shuttling locations
   public static final Translation3d NEUTRAL_LEFT =
     new Translation3d(FIELD_LENGTH/2, LEFT_SIDE_TARGET, 0);
 
@@ -98,6 +99,8 @@ public class FieldConstants {
   public static final Zone blueHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
   public static final Zone redHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
 
+  // trenches
+
 	public enum ShootingTarget {
 		HUB,
 		NEUTRAL,
@@ -189,12 +192,17 @@ 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)) {
       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))) {
+    // 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) {
       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;
   }
   
diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java
index 1959139..2623ff2 100644
--- a/src/main/java/frc/robot/subsystems/hood/Hood.java
+++ b/src/main/java/frc/robot/subsystems/hood/Hood.java
@@ -103,6 +103,7 @@ public class Hood extends SubsystemBase implements HoodIO {
 		// SmartDashboard.putNumber("Hood Setpoint", goalAngle.getDegrees());
 
 		if (forceHoodDown){
+			System.out.println("Forcing the hood down");
 			goalAngle = Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE);
 			goalVelocityRadPerSec = 0.0;
 		}