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 ce016372131404c5030f54212a9689b8f33adbef..e7e6829e0bf2a9537884b5f36a249b9aae8ec365 100644 (file)
--- 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 7adbf88cab8d57f6dd64632e2f3512e5620c68f6..644550850a907dced58e1a3d5c6889b53dd70a69 100644 (file)
--- 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 19591391cd0804fb1618d25a1217ac011889cb1c..2623ff2e557c6c9f68ce61f95fa88a0e1ff13d43 100644 (file)
--- 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;
                }