diff --git a/src/main/java/frc/game/Field.java b/src/main/java/frc/game/Field.java index 0dba87f..1e74583 100644 --- a/src/main/java/frc/game/Field.java +++ b/src/main/java/frc/game/Field.java @@ -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 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/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(); } } 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)); - } } 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)); } 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());