morney reassigned me

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
index 4ee974c7b1fcc018e36fcc8929d9f7dacdfe6d30..424667a9c7226a891a10da42dc6c31030a2c0a76 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -4,9 +4,11 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import frc.robot.util.Zone;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Robot;
 
@@ -57,7 +59,6 @@ public class FieldConstants {
   public static final Translation3d ALLIANCE_RIGHT_BLUE =
     new Translation3d(BLUE_BORDER - 2.2, RIGHT_SIDE_TARGET, 0);
 
-
   public static final Translation3d ALLIANCE_LEFT_RED =
     new Translation3d(RED_BORDER + 2.2, LEFT_SIDE_TARGET, 0); // previous hub + a few feet further back
 
@@ -73,6 +74,30 @@ public class FieldConstants {
   public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump
   public static final double RED_ALLIANCE_LINE = RED_BORDER; // 
 
+  // my zones
+  public static final double leftNeutralLine = FIELD_LENGTH * 0.25;
+  public static final double rightNeutralLine = FIELD_LENGTH * 0.75;
+  public static final double centerLengthLine = FIELD_LENGTH * 0.5;
+  public static final double centerWidthLine = FIELD_WIDTH * 0.5;
+  public static final double redLine = 179.111250;
+  public static final double blueLine = FIELD_LENGTH - 179.111250;
+
+  public static final double hubWidthLeft = FIELD_WIDTH / 2 - (47.0 / 2);
+  public static final double hubWidthRight = FIELD_WIDTH / 2 + (47.0 / 2);
+  public static final double hubBackRed = FIELD_LENGTH + 120.0; 
+  public static final double hubBackBlue = FIELD_LENGTH - 120.0; 
+
+  public static final double ladderRedLeft = FIELD_WIDTH + 13.0;
+  public static final double ladderBlueLeft = FIELD_WIDTH - 12.375;
+  public static final double ladderRedRight = FIELD_WIDTH - 35.75;
+  public static final double ladderBlueRight = FIELD_WIDTH + 35.75;
+
+  public static final Zone neutralStrip = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
+  public static final Zone neutralLeft = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
+  public static final Zone neutralRight = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, redLine - blueLine);
+  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);
+
        public enum ShootingTarget {
                HUB,
                NEUTRAL,
@@ -84,7 +109,9 @@ public class FieldConstants {
                ALLIANCE,
                NEUTRAL,
                OPPOSITION,
-               TRENCH_BUMP
+               TRENCH_BUMP,
+    UNDER_LADDER,
+    UNDER_MY_HUB,
        }
 
   public static Translation3d getHubTranslation() {
@@ -129,11 +156,18 @@ public class FieldConstants {
 
   public static FieldZone getZone(Translation2d drivepose) {
     double x = drivepose.getX();
+    double y = drivepose.getY();
     //double y = drivepose.getY();
     if ((x < FIELD_LENGTH/2 - Units.inchesToMeters(120.0) && x > BLUE_ALLIANCE_LINE)
-        || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE){
+        || x > FIELD_LENGTH/2 + Units.inchesToMeters(120.0) && x < RED_ALLIANCE_LINE) {
           return FieldZone.TRENCH_BUMP;
         }
+    if(((y < FIELD_WIDTH - (48.75 / 2) && y > FIELD_WIDTH - (48.75 / 2))) && (x < 47.0 || x > FIELD_LENGTH - 47.0)) {
+      return FieldZone.UNDER_LADDER;
+    }
+    if(((y < FIELD_WIDTH - (46.75 / 2) && y > FIELD_WIDTH - (46.75 / 2))) && ((x > 207.42375 && x < 207.42375 + 36.0) || (x < FIELD_LENGTH - 207.42375 && x > FIELD_LENGTH - 207.42375 - 36.0))) {
+      return FieldZone.UNDER_LADDER;
+    }
     if(x > FieldConstants.RED_ALLIANCE_LINE) { // inside red
       if (Robot.getAlliance() == Alliance.Red) {
         return FieldZone.ALLIANCE;
@@ -151,6 +185,14 @@ public class FieldConstants {
     }
   }
 
+  public static FieldZone getWorkingZone(Translation2d drivepose) {
+    double x = drivepose.getX();
+    double y = drivepose.getY();
+    Alliance alliance = Robot.getAlliance();
+
+
+  }
+
   /**
    * 
    * @return Whether Y coordinate is in the upper half (left side on blue alliance)
@@ -178,4 +220,5 @@ public class FieldConstants {
       }
     }
   }
+
 }

diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java
new file mode 100644 (file)
index 0000000..83f3447
--- /dev/null
+++ b/src/main/java/frc/robot/util/Zone.java
@@ -0,0 +1,25 @@
+package frc.robot.util;
+
+public class Zone {
+    public double x_center;
+    public double y_center;
+    public double width;
+    public double length;
+
+    public Zone(double x_center, double y_center, double width, double length) {
+        this.x_center = x_center;
+        this.y_center = y_center;
+        this.width = width;
+        this.length = length;
+    }
+
+    public boolean isInside(double x, double y) {
+        if (x > x_center + (length / 2) || x < x_center - (length / 2)) {
+            return false;
+        }
+        if (y > y_center + (width / 2) || y < y_center - (width / 2)) {
+            return false;
+        }
+        return true;
+    }
+}