From b5fdcd26ae8b3bca4e38aeee965c0d1e0ef5f54d Mon Sep 17 00:00:00 2001 From: Wesley28w Date: Mon, 2 Mar 2026 16:30:13 -0800 Subject: [PATCH] morney reassigned me --- .../frc/robot/constants/FieldConstants.java | 49 +++++++++++++++++-- src/main/java/frc/robot/util/Zone.java | 25 ++++++++++ 2 files changed, 71 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/util/Zone.java diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 4ee974c..424667a 100644 --- 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 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; + } +} -- 2.39.5