Skip to content

Refactor field region builder #28

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
nlaverdure merged 6 commits into from
Jan 25, 2026
Merged

Refactor field region builder #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
93469ee
wip use wpilib geometry classes
nlaverdure Jan 25, 2026
f48cecf
complete enum definitions
nlaverdure Jan 25, 2026
73b060a
fuel arrangement distances
nlaverdure Jan 25, 2026
9365618
final cleanup
nlaverdure Jan 25, 2026
f892d1c
plot field zones
nlaverdure Jan 25, 2026
0591223
use field dimensions in vision tests
nlaverdure Jan 25, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
231 changes: 150 additions & 81 deletions src/main/java/frc/game/Field.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,110 +1,179 @@
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 {
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;
// Measured
public static final Distance centerField_x_pos = Inches.of(325.06);
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);
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);
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
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);
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(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)),
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(
build()
.xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH)
.ypos(0)
.xlen(ALLIANCE_ZONE_LENGTH)
.ylen(FIELD_WIDTH)),
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(
build()
.xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH)
.ypos(FIELD_WIDTH)
.xlen(65.65)
.ylen(47)),
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(
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)),
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(
build().xpos(ALLIANCE_ZONE_LENGTH + NEUTRAL_ZONE_LENGTH).ypos(135.8).xlen(73).ylen(44.4)),
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(
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));
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 double xpos;
public final double ypos;
public final double xlen;
public final double ylen;
public final Translation2d center;
private final Rectangle2d rect;

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));
private Region(Rectangle2d rect) {
this.rect = rect;
}

public static Builder build() {
return new Builder();
public boolean contains(Pose2d pose) {
return rect.contains(pose.getTranslation());
}

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;
}

public Builder xlen(double xlen) {
this.xlen = xlen;
return this;
}

public Builder ylen(double ylen) {
this.ylen = ylen;
return this;
}
}

public boolean contains(Pose2d pose) {
return (pose.getX() >= xpos)
&& (pose.getX() < xpos + xlen)
&& (pose.getY() >= ypos)
&& (pose.getY() < ypos + ylen);
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.NeutralZone;
}

public static void plotRegions() {
for (Region region : Region.values()) {
Robot.field.getObject(region.name()).setPoses(rectangleToPoses(region.rect));
}
return Region.RedZone;
}

private static List<Pose2d> 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
);
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/game/GameState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
}
8 changes: 0 additions & 8 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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));
}
}
5 changes: 5 additions & 0 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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));
}
Expand Down
6 changes: 2 additions & 4 deletions src/main/java/frc/robot/subsystems/vision/Vision.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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());

Expand Down