From 93469ee96135151532661a1617ff25ce34eaec86 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 10:02:06 -0500 Subject: [PATCH 1/6] wip use wpilib geometry classes --- src/main/java/frc/game/Field.java | 121 ++++++++----------------- src/main/java/frc/robot/Constants.java | 8 -- 2 files changed, 40 insertions(+), 89 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 0dba87f..55e8621 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -1,100 +1,59 @@ package frc.game; -import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.*; 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.Translation2d; +import edu.wpi.first.units.measure.Distance; public class Field { - public static final double FIELD_WIDTH = 317.7; - public static final double ALLIANCE_ZONE_LENGTH = 158.6; - public static final double NEUTRAL_ZONE_LENGTH = 283; + public static final Distance centerField_x_pos = Inches.of(325.06); + public static final Distance centerField_y_pos = Inches.of(316.64); + private static final Distance hub_x_pos = Inches.of(181.56); + private static final Distance hub_x_len = Inches.of(47); + private static final Distance hub_y_len = Inches.of(47); + private static final Distance trench_y_len = Inches.of(49.86); + + private static final Distance field_x_len = centerField_x_pos.times(2); + private static final Distance field_y_len = centerField_y_pos.times(2); + private static final Distance allianceZone_x_len = hub_x_pos.minus(hub_x_len.div(2)); + private static final Distance neutralZone_x_len = centerField_x_pos.minus(hub_x_pos.plus(hub_x_len)).times(2); + private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); + private static final Pose2d blueHubCenter = new Pose2d(new Translation2d(hub_x_pos, centerField_y_pos), Rotation2d.kZero); + private static final Pose2d redHubCenter = new Pose2d(new Translation2d(centerField_x_pos.times(2).minus(hub_x_pos), centerField_y_pos), Rotation2d.kZero); enum Region { - BlueZone(build().xpos(0).ypos(0).xlen(ALLIANCE_ZONE_LENGTH).ylen(FIELD_WIDTH)), - NeutralZone( - build().xpos(ALLIANCE_ZONE_LENGTH).ypos(0).xlen(NEUTRAL_ZONE_LENGTH).ylen(FIELD_WIDTH)), - RedZone( - build() - .xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH) - .ypos(0) - .xlen(ALLIANCE_ZONE_LENGTH) - .ylen(FIELD_WIDTH)), - RedLeftTrench( - build() - .xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH) - .ypos(FIELD_WIDTH) - .xlen(65.65) - .ylen(47)), - RedRightTrench( - build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(0).xlen(65.65).ylen(47)), - BlueLeftTrench(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(FIELD_WIDTH).xlen(65.65).ylen(47)), - BlueRightTrench(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(0).xlen(65.65).ylen(47)), - BlueDepot(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(65.65).xlen(42).ylen(27)), - RedDepot(build().xpos(FIELD_WIDTH).ypos(91.4 + 73).xlen(42).ylen(27)), - BlueTower(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(91.4).xlen(49.25).ylen(45)), - RedTower(build().xpos(FIELD_WIDTH).ypos(91.4).xlen(49.25).ylen(45)), - BlueHub(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(138.65).xlen(47).ylen(47)), - RedHub(build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(138.65).xlen(47).ylen(47)), + BlueZone(new Rectangle2d(new Translation2d(0, 0), new Translation2d(allianceZone_x_len, field_y_len))), + NeutralZone(new Rectangle2d(fieldCenter, neutralZone_x_len, field_y_len)), + RedZone(new Rectangle2d(new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), new Translation2d(field_x_len, field_y_len))), + BlueHub(new Rectangle2d(blueHubCenter, hub_x_len, hub_y_len)), + RedHub(new Rectangle2d(redHubCenter, hub_x_len, hub_y_len)), + RedLeftTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_pos).minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(field_x_len.minus(hub_x_pos).plus(hub_x_len.div(2)), trench_y_len))), + RedRightTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_pos).minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(field_x_len.minus(hub_x_pos).plus(hub_x_len.div(2)), field_y_len))), + BlueLeftTrench(new Rectangle2d(new Translation2d(hub_x_pos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(hub_x_pos.plus(hub_x_len.div(2)), field_y_len))), + BlueRightTrench(new Rectangle2d(new Translation2d(hub_x_pos.minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(hub_x_pos.plus(hub_x_len.div(2)), trench_y_len))), + BlueDepot(build().xpos(0).ypos(centerField_y_pos + 75.93 - 42/2).xlen(27).ylen(42)), + RedDepot(build().xpos(2*centerField_x_pos - 27).ypos(centerField_y_pos - 75.93 - 42/2).xlen(27).ylen(42)), + // BlueTower(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(91.4).xlen(49.25).ylen(45)), + // RedTower(build().xpos(FIELD_WIDTH).ypos(91.4).xlen(49.25).ylen(45)), RedLeftBump( - build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(135.8).xlen(73).ylen(44.4)), + build().xpos(allianceZone_x_len + 47 + neutralZone_x_len).ypos(49.86 + 12 + 73 + 47).xlen(47).ylen(73)), RedRightBump( - build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(47).xlen(73).ylen(44.4)), - BlueLeftBump(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(135.8).xlen(73).ylen(44.4)), - BlueRightBump(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(47).xlen(73).ylen(44.4)), - FuelArrangement( - build().xpos(NEUTRAL_ZONE_LENGTH / 2 + 72 / 2).ypos(317.7 - 47 * 2).xlen(72).ylen(206)); - - public final double xpos; - public final double ypos; - public final double xlen; - public final double ylen; - public final Translation2d center; - - private Region(Builder builder) { - this.xpos = builder.xpos; - this.ypos = builder.ypos; - this.xlen = builder.xlen; - this.ylen = builder.ylen; - this.center = new Translation2d(Inches.of(xpos + xlen / 2), Inches.of(ypos + ylen / 2)); - } - - public static Builder build() { - return new Builder(); - } - - public static class Builder { - double xpos; - double ypos; - double xlen; - double ylen; - - public Builder xpos(double xpos) { - this.xpos = xpos; - return this; - } - - public Builder ypos(double ypos) { - this.ypos = ypos; - return this; - } + build().xpos(allianceZone_x_len + 47 + neutralZone_x_len).ypos(49.86 + 12).xlen(47).ylen(73)), + BlueLeftBump(build().xpos(allianceZone_x_len).ypos(49.86 + 12 + 73 + 47).xlen(47).ylen(73)), + BlueRightBump(build().xpos(allianceZone_x_len).ypos(49.86 + 12).xlen(47).ylen(73)), + FuelArrangement(new Rectangle2d(fieldCenter, Inches.of(35.95*2), Inches.of(90.95*2))); - public Builder xlen(double xlen) { - this.xlen = xlen; - return this; - } + public final Rectangle2d rect; - public Builder ylen(double ylen) { - this.ylen = ylen; - return this; - } + private Region(Rectangle2d rect) { + this.rect = rect; } public boolean contains(Pose2d pose) { - return (pose.getX() >= xpos) - && (pose.getX() < xpos + xlen) - && (pose.getY() >= ypos) - && (pose.getY() < ypos + ylen); + return rect.contains(pose.getTranslation()); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c30709a..c06a1ad 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,7 +15,6 @@ import static edu.wpi.first.units.Units.*; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; @@ -76,11 +75,4 @@ public static final class OIConstants { public static final int kDefaultDriverPort = 0; public static final int kDefaultOperatorPort = 1; } - - public static final class FieldConstants { - public static final Translation2d kBlueHubCenter = - new Translation2d(Inches.of(181.56), Inches.of(158.32)); - public static final Translation2d kRedHubCenter = - new Translation2d(Inches.of(650.12 - 181.56), Inches.of(158.32)); - } } From f48cecf9fb239bbf8c11dd93cf51d96b03e4dfd2 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 10:43:53 -0500 Subject: [PATCH 2/6] complete enum definitions --- src/main/java/frc/game/Field.java | 45 ++++++++++++++++++------------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 55e8621..292db36 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -9,20 +9,29 @@ import edu.wpi.first.units.measure.Distance; public class Field { + // Measured public static final Distance centerField_x_pos = Inches.of(325.06); public static final Distance centerField_y_pos = Inches.of(316.64); - private static final Distance hub_x_pos = Inches.of(181.56); + private static final Distance hub_x_centerPos = Inches.of(181.56); private static final Distance hub_x_len = Inches.of(47); private static final Distance hub_y_len = Inches.of(47); private static final Distance trench_y_len = Inches.of(49.86); + private static final Distance bump_y_len = Inches.of(73); + private static final Distance depot_y_centerPos = centerField_y_pos.plus(Inches.of(75.93)); + private static final Distance depot_x_len = Inches.of(27); + private static final Distance depot_y_len = Inches.of(42); + private static final Distance tower_y_centerPos = centerField_y_pos.minus(Inches.of(11.46)); + private static final Distance tower_x_len = Inches.of(43.8); + private static final Distance tower_y_len = Inches.of(47); + // Constructed private static final Distance field_x_len = centerField_x_pos.times(2); private static final Distance field_y_len = centerField_y_pos.times(2); - private static final Distance allianceZone_x_len = hub_x_pos.minus(hub_x_len.div(2)); - private static final Distance neutralZone_x_len = centerField_x_pos.minus(hub_x_pos.plus(hub_x_len)).times(2); + private static final Distance allianceZone_x_len = hub_x_centerPos.minus(hub_x_len.div(2)); + private static final Distance neutralZone_x_len = centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len)).times(2); private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d blueHubCenter = new Pose2d(new Translation2d(hub_x_pos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d redHubCenter = new Pose2d(new Translation2d(centerField_x_pos.times(2).minus(hub_x_pos), centerField_y_pos), Rotation2d.kZero); + private static final Pose2d blueHubCenter = new Pose2d(new Translation2d(hub_x_centerPos, centerField_y_pos), Rotation2d.kZero); + private static final Pose2d redHubCenter = new Pose2d(new Translation2d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos), Rotation2d.kZero); enum Region { BlueZone(new Rectangle2d(new Translation2d(0, 0), new Translation2d(allianceZone_x_len, field_y_len))), @@ -30,20 +39,18 @@ enum Region { RedZone(new Rectangle2d(new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), new Translation2d(field_x_len, field_y_len))), BlueHub(new Rectangle2d(blueHubCenter, hub_x_len, hub_y_len)), RedHub(new Rectangle2d(redHubCenter, hub_x_len, hub_y_len)), - RedLeftTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_pos).minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(field_x_len.minus(hub_x_pos).plus(hub_x_len.div(2)), trench_y_len))), - RedRightTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_pos).minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(field_x_len.minus(hub_x_pos).plus(hub_x_len.div(2)), field_y_len))), - BlueLeftTrench(new Rectangle2d(new Translation2d(hub_x_pos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(hub_x_pos.plus(hub_x_len.div(2)), field_y_len))), - BlueRightTrench(new Rectangle2d(new Translation2d(hub_x_pos.minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(hub_x_pos.plus(hub_x_len.div(2)), trench_y_len))), - BlueDepot(build().xpos(0).ypos(centerField_y_pos + 75.93 - 42/2).xlen(27).ylen(42)), - RedDepot(build().xpos(2*centerField_x_pos - 27).ypos(centerField_y_pos - 75.93 - 42/2).xlen(27).ylen(42)), - // BlueTower(build().xpos(ALLIANCE_ZONE_LENGTH).ypos(91.4).xlen(49.25).ylen(45)), - // RedTower(build().xpos(FIELD_WIDTH).ypos(91.4).xlen(49.25).ylen(45)), - RedLeftBump( - build().xpos(allianceZone_x_len + 47 + neutralZone_x_len).ypos(49.86 + 12 + 73 + 47).xlen(47).ylen(73)), - RedRightBump( - build().xpos(allianceZone_x_len + 47 + neutralZone_x_len).ypos(49.86 + 12).xlen(47).ylen(73)), - BlueLeftBump(build().xpos(allianceZone_x_len).ypos(49.86 + 12 + 73 + 47).xlen(47).ylen(73)), - BlueRightBump(build().xpos(allianceZone_x_len).ypos(49.86 + 12).xlen(47).ylen(73)), + RedLeftTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), trench_y_len))), + RedRightTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), field_y_len))), + BlueLeftTrench(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), field_y_len))), + BlueRightTrench(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), trench_y_len))), + RedLeftBump(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2))))), + RedRightBump(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), + BlueLeftBump(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), + BlueRightBump(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2))))), + BlueDepot(new Rectangle2d(new Translation2d(Inches.of(0), depot_y_centerPos.minus(depot_y_len.div(2))), new Translation2d(depot_x_len, depot_y_centerPos.plus(depot_y_len.div(2))))), + RedDepot(new Rectangle2d(new Translation2d(field_x_len.minus(depot_x_len), field_y_len.minus(depot_y_centerPos).minus(depot_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(depot_y_centerPos).plus(depot_y_len.div(2))))), + BlueTower(new Rectangle2d(new Translation2d(Inches.of(0), tower_y_centerPos.minus(tower_y_len.div(2))), new Translation2d(tower_x_len, tower_y_centerPos.plus(tower_y_len.div(2))))), + RedTower(new Rectangle2d(new Translation2d(field_x_len.minus(tower_x_len), field_y_len.minus(tower_y_centerPos).minus(tower_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(tower_y_centerPos).plus(tower_y_len.div(2))))), FuelArrangement(new Rectangle2d(fieldCenter, Inches.of(35.95*2), Inches.of(90.95*2))); public final Rectangle2d rect; From 73b060a7b13e7ed247994959e792feb7fed2672e Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 10:45:27 -0500 Subject: [PATCH 3/6] fuel arrangement distances --- src/main/java/frc/game/Field.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 292db36..e66d408 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -23,6 +23,8 @@ public class Field { private static final Distance tower_y_centerPos = centerField_y_pos.minus(Inches.of(11.46)); private static final Distance tower_x_len = Inches.of(43.8); private static final Distance tower_y_len = Inches.of(47); + private static final Distance fuel_x_len = Inches.of(35.95*2); + private static final Distance fuel_y_len = Inches.of(90.95*2); // Constructed private static final Distance field_x_len = centerField_x_pos.times(2); @@ -51,7 +53,7 @@ enum Region { RedDepot(new Rectangle2d(new Translation2d(field_x_len.minus(depot_x_len), field_y_len.minus(depot_y_centerPos).minus(depot_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(depot_y_centerPos).plus(depot_y_len.div(2))))), BlueTower(new Rectangle2d(new Translation2d(Inches.of(0), tower_y_centerPos.minus(tower_y_len.div(2))), new Translation2d(tower_x_len, tower_y_centerPos.plus(tower_y_len.div(2))))), RedTower(new Rectangle2d(new Translation2d(field_x_len.minus(tower_x_len), field_y_len.minus(tower_y_centerPos).minus(tower_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(tower_y_centerPos).plus(tower_y_len.div(2))))), - FuelArrangement(new Rectangle2d(fieldCenter, Inches.of(35.95*2), Inches.of(90.95*2))); + FuelArrangement(new Rectangle2d(fieldCenter, fuel_x_len, fuel_y_len)); public final Rectangle2d rect; From 93656189710feeb4b75623810e1702d9392347ff Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 10:57:17 -0500 Subject: [PATCH 4/6] final cleanup --- src/main/java/frc/game/Field.java | 133 ++++++++++++++++++++------ src/main/java/frc/game/GameState.java | 4 +- 2 files changed, 108 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index e66d408..923c85a 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -1,6 +1,6 @@ package frc.game; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Inches; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rectangle2d; @@ -23,39 +23,114 @@ public class Field { private static final Distance tower_y_centerPos = centerField_y_pos.minus(Inches.of(11.46)); private static final Distance tower_x_len = Inches.of(43.8); private static final Distance tower_y_len = Inches.of(47); - private static final Distance fuel_x_len = Inches.of(35.95*2); - private static final Distance fuel_y_len = Inches.of(90.95*2); + private static final Distance fuel_x_len = Inches.of(35.95 * 2); + private static final Distance fuel_y_len = Inches.of(90.95 * 2); // Constructed private static final Distance field_x_len = centerField_x_pos.times(2); private static final Distance field_y_len = centerField_y_pos.times(2); private static final Distance allianceZone_x_len = hub_x_centerPos.minus(hub_x_len.div(2)); - private static final Distance neutralZone_x_len = centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len)).times(2); - private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d blueHubCenter = new Pose2d(new Translation2d(hub_x_centerPos, centerField_y_pos), Rotation2d.kZero); - private static final Pose2d redHubCenter = new Pose2d(new Translation2d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos), Rotation2d.kZero); + private static final Distance neutralZone_x_len = + centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len)).times(2); + private static final Pose2d fieldCenter = + new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); + private static final Pose2d blueHubCenter = + new Pose2d(new Translation2d(hub_x_centerPos, centerField_y_pos), Rotation2d.kZero); + private static final Pose2d redHubCenter = + new Pose2d( + new Translation2d(centerField_x_pos.times(2).minus(hub_x_centerPos), centerField_y_pos), + Rotation2d.kZero); enum Region { - BlueZone(new Rectangle2d(new Translation2d(0, 0), new Translation2d(allianceZone_x_len, field_y_len))), + BlueZone( + new Rectangle2d( + new Translation2d(0, 0), new Translation2d(allianceZone_x_len, field_y_len))), NeutralZone(new Rectangle2d(fieldCenter, neutralZone_x_len, field_y_len)), - RedZone(new Rectangle2d(new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), new Translation2d(field_x_len, field_y_len))), + RedZone( + new Rectangle2d( + new Translation2d(field_x_len.minus(allianceZone_x_len), Inches.of(0)), + new Translation2d(field_x_len, field_y_len))), BlueHub(new Rectangle2d(blueHubCenter, hub_x_len, hub_y_len)), RedHub(new Rectangle2d(redHubCenter, hub_x_len, hub_y_len)), - RedLeftTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), trench_y_len))), - RedRightTrench(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), field_y_len))), - BlueLeftTrench(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), field_y_len))), - BlueRightTrench(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), Inches.of(0)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), trench_y_len))), - RedLeftBump(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2))))), - RedRightBump(new Rectangle2d(new Translation2d(field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), new Translation2d(field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), - BlueLeftBump(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), - BlueRightBump(new Rectangle2d(new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), centerField_y_pos.minus(hub_y_len.div(2))))), - BlueDepot(new Rectangle2d(new Translation2d(Inches.of(0), depot_y_centerPos.minus(depot_y_len.div(2))), new Translation2d(depot_x_len, depot_y_centerPos.plus(depot_y_len.div(2))))), - RedDepot(new Rectangle2d(new Translation2d(field_x_len.minus(depot_x_len), field_y_len.minus(depot_y_centerPos).minus(depot_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(depot_y_centerPos).plus(depot_y_len.div(2))))), - BlueTower(new Rectangle2d(new Translation2d(Inches.of(0), tower_y_centerPos.minus(tower_y_len.div(2))), new Translation2d(tower_x_len, tower_y_centerPos.plus(tower_y_len.div(2))))), - RedTower(new Rectangle2d(new Translation2d(field_x_len.minus(tower_x_len), field_y_len.minus(tower_y_centerPos).minus(tower_y_len.div(2))), new Translation2d(field_x_len, field_y_len.minus(tower_y_centerPos).plus(tower_y_len.div(2))))), - FuelArrangement(new Rectangle2d(fieldCenter, fuel_x_len, fuel_y_len)); + RedLeftTrench( + new Rectangle2d( + new Translation2d( + field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), Inches.of(0)), + new Translation2d( + field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), trench_y_len))), + RedRightTrench( + new Rectangle2d( + new Translation2d( + field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), + field_y_len.minus(trench_y_len)), + new Translation2d( + field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), field_y_len))), + BlueLeftTrench( + new Rectangle2d( + new Translation2d( + hub_x_centerPos.minus(hub_x_len.div(2)), field_y_len.minus(trench_y_len)), + new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), field_y_len))), + BlueRightTrench( + new Rectangle2d( + new Translation2d(hub_x_centerPos.minus(hub_x_len.div(2)), Inches.of(0)), + new Translation2d(hub_x_centerPos.plus(hub_x_len.div(2)), trench_y_len))), + RedLeftBump( + new Rectangle2d( + new Translation2d( + field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), + centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), + new Translation2d( + field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), + centerField_y_pos.minus(hub_y_len.div(2))))), + RedRightBump( + new Rectangle2d( + new Translation2d( + field_x_len.minus(hub_x_centerPos).minus(hub_x_len.div(2)), + centerField_y_pos.plus(hub_y_len.div(2))), + new Translation2d( + field_x_len.minus(hub_x_centerPos).plus(hub_x_len.div(2)), + centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), + BlueLeftBump( + new Rectangle2d( + new Translation2d( + hub_x_centerPos.minus(hub_x_len.div(2)), centerField_y_pos.plus(hub_y_len.div(2))), + new Translation2d( + hub_x_centerPos.plus(hub_x_len.div(2)), + centerField_y_pos.plus(hub_y_len.div(2)).plus(bump_y_len)))), + BlueRightBump( + new Rectangle2d( + new Translation2d( + hub_x_centerPos.minus(hub_x_len.div(2)), + centerField_y_pos.minus(hub_y_len.div(2)).minus(bump_y_len)), + new Translation2d( + hub_x_centerPos.plus(hub_x_len.div(2)), + centerField_y_pos.minus(hub_y_len.div(2))))), + BlueDepot( + new Rectangle2d( + new Translation2d(Inches.of(0), depot_y_centerPos.minus(depot_y_len.div(2))), + new Translation2d(depot_x_len, depot_y_centerPos.plus(depot_y_len.div(2))))), + RedDepot( + new Rectangle2d( + new Translation2d( + field_x_len.minus(depot_x_len), + field_y_len.minus(depot_y_centerPos).minus(depot_y_len.div(2))), + new Translation2d( + field_x_len, field_y_len.minus(depot_y_centerPos).plus(depot_y_len.div(2))))), + BlueTower( + new Rectangle2d( + new Translation2d(Inches.of(0), tower_y_centerPos.minus(tower_y_len.div(2))), + new Translation2d(tower_x_len, tower_y_centerPos.plus(tower_y_len.div(2))))), + RedTower( + new Rectangle2d( + new Translation2d( + field_x_len.minus(tower_x_len), + field_y_len.minus(tower_y_centerPos).minus(tower_y_len.div(2))), + new Translation2d( + field_x_len, field_y_len.minus(tower_y_centerPos).plus(tower_y_len.div(2))))), + FuelArrangement(new Rectangle2d(fieldCenter, fuel_x_len, fuel_y_len)), + Field(new Rectangle2d(new Translation2d(0, 0), new Translation2d(field_x_len, field_y_len))); - public final Rectangle2d rect; + private final Rectangle2d rect; private Region(Rectangle2d rect) { this.rect = rect; @@ -64,15 +139,19 @@ private Region(Rectangle2d rect) { public boolean contains(Pose2d pose) { return rect.contains(pose.getTranslation()); } + + public Pose2d getCenter() { + return rect.getCenter(); + } } public static Region getZone(Pose2d pose) { - if (pose.getX() < Region.NeutralZone.xpos) { + if (Region.BlueZone.contains(pose)) { return Region.BlueZone; } - if (pose.getX() < Region.RedZone.xpos) { - return Region.NeutralZone; + if (Region.RedZone.contains(pose)) { + return Region.RedZone; } - return Region.RedZone; + return Region.NeutralZone; } } diff --git a/src/main/java/frc/game/GameState.java b/src/main/java/frc/game/GameState.java index b723cc7..8bb39e7 100644 --- a/src/main/java/frc/game/GameState.java +++ b/src/main/java/frc/game/GameState.java @@ -124,8 +124,8 @@ public static void logValues() { public static Translation2d getMyHubLocation() { if (Robot.getAlliance() == Alliance.Red) { - return Region.RedHub.center; + return Region.RedHub.getCenter().getTranslation(); } - return Region.BlueHub.center; + return Region.BlueHub.getCenter().getTranslation(); } } From f892d1c1bf5b99fcd1afe8806f730e3e024d0969 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 11:32:58 -0500 Subject: [PATCH 5/6] plot field zones --- src/main/java/frc/game/Field.java | 28 +++++++++++++++++++++++++--- src/main/java/frc/robot/Robot.java | 5 +++++ 2 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 923c85a..6f3ddf6 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -1,17 +1,19 @@ package frc.game; -import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.*; 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.Translation2d; import edu.wpi.first.units.measure.Distance; +import frc.robot.Robot; +import java.util.List; public class Field { // Measured public static final Distance centerField_x_pos = Inches.of(325.06); - public static final Distance centerField_y_pos = Inches.of(316.64); + public static final Distance centerField_y_pos = Inches.of(158.32); private static final Distance hub_x_centerPos = Inches.of(181.56); private static final Distance hub_x_len = Inches.of(47); private static final Distance hub_y_len = Inches.of(47); @@ -31,7 +33,7 @@ public class Field { private static final Distance field_y_len = centerField_y_pos.times(2); private static final Distance allianceZone_x_len = hub_x_centerPos.minus(hub_x_len.div(2)); private static final Distance neutralZone_x_len = - centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len)).times(2); + (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2); private static final Pose2d fieldCenter = new Pose2d(new Translation2d(centerField_x_pos, centerField_y_pos), Rotation2d.kZero); private static final Pose2d blueHubCenter = @@ -154,4 +156,24 @@ public static Region getZone(Pose2d pose) { } return Region.NeutralZone; } + + public static void plotRegions() { + for (Region region : Region.values()) { + Robot.field.getObject(region.name()).setPoses(rectangleToPoses(region.rect)); + } + } + + private static List rectangleToPoses(Rectangle2d rect) { + Translation2d center = rect.getCenter().getTranslation(); + double x = rect.getXWidth() / 2.0; + double y = rect.getYWidth() / 2.0; + + return List.of( + new Pose2d(center.getX() - x, center.getY() - y, Rotation2d.kZero), + new Pose2d(center.getX() - x, center.getY() + y, Rotation2d.kZero), + new Pose2d(center.getX() + x, center.getY() + y, Rotation2d.kZero), + new Pose2d(center.getX() + x, center.getY() - y, Rotation2d.kZero), + new Pose2d(center.getX() - x, center.getY() - y, Rotation2d.kZero) // close loop + ); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a3f38a2..9f191ef 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,11 +4,13 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.game.Field; import frc.game.GameState; import frc.lib.AllianceSelector; import frc.lib.AutoOption; @@ -59,6 +61,7 @@ public class Robot extends LoggedRobot { private final AutoSelector autoSelector = new AutoSelector( AutoConstants.kAutonomousModeSelectorPorts, allianceSelector::getAllianceColor); + public static final Field2d field = new Field2d(); // Subsystems private Drive drive; @@ -177,6 +180,8 @@ public Robot() { SmartDashboard.putData( "Align Encoders", new InstantCommand(() -> drive.zeroAbsoluteEncoders()).ignoringDisable(true)); + SmartDashboard.putData("Field", field); + Field.plotRegions(); turret.setDefaultCommand(Commands.run(turret::aimAtHub, turret)); } From 0591223eb6a216b3d47a01e6063d9faf3f1dedd7 Mon Sep 17 00:00:00 2001 From: Nate Laverdure Date: Sun, 25 Jan 2026 11:37:52 -0500 Subject: [PATCH 6/6] use field dimensions in vision tests --- src/main/java/frc/game/Field.java | 4 ++-- src/main/java/frc/robot/subsystems/vision/Vision.java | 6 ++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 6f3ddf6..1e74583 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -29,8 +29,8 @@ public class Field { private static final Distance fuel_y_len = Inches.of(90.95 * 2); // Constructed - private static final Distance field_x_len = centerField_x_pos.times(2); - private static final Distance field_y_len = centerField_y_pos.times(2); + public static final Distance field_x_len = centerField_x_pos.times(2); + public static final Distance field_y_len = centerField_y_pos.times(2); private static final Distance allianceZone_x_len = hub_x_centerPos.minus(hub_x_len.div(2)); private static final Distance neutralZone_x_len = (centerField_x_pos.minus(hub_x_centerPos.plus(hub_x_len.div(2)))).times(2); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 426087b..dee85c3 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.game.Field; import frc.robot.subsystems.vision.VisionIO.PoseObservation; import java.io.IOException; import java.util.ArrayList; @@ -328,10 +329,7 @@ public double test(PoseObservation observation) { @Override public double test(PoseObservation observation) { var cornerA = new Translation2d(minRobotWidth.div(2.0), minRobotWidth.div(2.0)); - var cornerB = - new Translation2d( - getAprilTagLayout().getFieldLength(), getAprilTagLayout().getFieldWidth()) - .minus(cornerA); + var cornerB = new Translation2d(Field.field_x_len, Field.field_y_len).minus(cornerA); var arena = new Rectangle2d(cornerA, cornerB); boolean pass = arena.contains(observation.pose().toPose2d().getTranslation());