-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
0310fca
commit 0d11149
Showing
17 changed files
with
1,172 additions
and
80 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,203 @@ | ||
| package frc.robot; | ||
|
|
||
| import edu.wpi.first.math.geometry.*; | ||
| import edu.wpi.first.math.util.Units; | ||
| import java.util.ArrayList; | ||
| import java.util.HashMap; | ||
| import java.util.List; | ||
| import java.util.Map; | ||
|
|
||
| /** | ||
| * Contains various field dimensions and useful reference points. All units are in meters and poses | ||
| * have a blue alliance origin. | ||
| */ | ||
| public class FieldConstants { | ||
| public static final double fieldLength = Units.inchesToMeters(690.876); | ||
| public static final double fieldWidth = Units.inchesToMeters(317); | ||
| public static final double startingLineX = | ||
| Units.inchesToMeters(299.438); // Measured from the inside of starting line | ||
|
|
||
| public static class Processor { | ||
| public static final Pose2d centerFace = | ||
| new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90)); | ||
| } | ||
|
|
||
| public static class Barge { | ||
| public static final Translation2d farCage = | ||
| new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(286.779)); | ||
| public static final Translation2d middleCage = | ||
| new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(242.855)); | ||
| public static final Translation2d closeCage = | ||
| new Translation2d(Units.inchesToMeters(345.428), Units.inchesToMeters(199.947)); | ||
|
|
||
| // Measured from floor to bottom of cage | ||
| public static final double deepHeight = Units.inchesToMeters(3.125); | ||
| public static final double shallowHeight = Units.inchesToMeters(30.125); | ||
| } | ||
|
|
||
| public static class CoralStation { | ||
| public static final Pose2d leftCenterFace = | ||
| new Pose2d( | ||
| Units.inchesToMeters(33.526), | ||
| Units.inchesToMeters(291.176), | ||
| Rotation2d.fromDegrees(90 - 144.011)); | ||
| public static final Pose2d rightCenterFace = | ||
| new Pose2d( | ||
| Units.inchesToMeters(33.526), | ||
| Units.inchesToMeters(25.824), | ||
| Rotation2d.fromDegrees(144.011 - 90)); | ||
| } | ||
|
|
||
| public static class Reef { | ||
| public static final Translation2d center = | ||
| new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501)); | ||
| public static final double faceToZoneLine = | ||
| Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line | ||
|
|
||
| public static final Pose2d[] centerFaces = | ||
| new Pose2d[6]; // Starting facing the driver station in clockwise order | ||
| public static final List<Map<ReefHeight, Pose3d>> branchPositions = | ||
| new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise | ||
|
|
||
| static { | ||
| // Initialize faces | ||
| centerFaces[0] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(144.003), | ||
| Units.inchesToMeters(158.500), | ||
| Rotation2d.fromDegrees(180)); | ||
| centerFaces[1] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(160.373), | ||
| Units.inchesToMeters(186.857), | ||
| Rotation2d.fromDegrees(120)); | ||
| centerFaces[2] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(193.116), | ||
| Units.inchesToMeters(186.858), | ||
| Rotation2d.fromDegrees(60)); | ||
| centerFaces[3] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(209.489), | ||
| Units.inchesToMeters(158.502), | ||
| Rotation2d.fromDegrees(0)); | ||
| centerFaces[4] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(193.118), | ||
| Units.inchesToMeters(130.145), | ||
| Rotation2d.fromDegrees(-60)); | ||
| centerFaces[5] = | ||
| new Pose2d( | ||
| Units.inchesToMeters(160.375), | ||
| Units.inchesToMeters(130.144), | ||
| Rotation2d.fromDegrees(-120)); | ||
|
|
||
| // Initialize branch positions | ||
| for (int face = 0; face < 6; face++) { | ||
| Map<ReefHeight, Pose3d> fillRight = new HashMap<>(); | ||
| Map<ReefHeight, Pose3d> fillLeft = new HashMap<>(); | ||
| for (var level : ReefHeight.values()) { | ||
| Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face))); | ||
| double adjustX = Units.inchesToMeters(30.738); | ||
| double adjustY = Units.inchesToMeters(6.469); | ||
|
|
||
| fillRight.put( | ||
| level, | ||
| new Pose3d( | ||
| new Translation3d( | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) | ||
| .getX(), | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, adjustY, new Rotation2d())) | ||
| .getY(), | ||
| level.height), | ||
| new Rotation3d( | ||
| 0, | ||
| Units.degreesToRadians(level.pitch), | ||
| poseDirection.getRotation().getRadians()))); | ||
| fillLeft.put( | ||
| level, | ||
| new Pose3d( | ||
| new Translation3d( | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) | ||
| .getX(), | ||
| poseDirection | ||
| .transformBy(new Transform2d(adjustX, -adjustY, new Rotation2d())) | ||
| .getY(), | ||
| level.height), | ||
| new Rotation3d( | ||
| 0, | ||
| Units.degreesToRadians(level.pitch), | ||
| poseDirection.getRotation().getRadians()))); | ||
| } | ||
| branchPositions.add((face * 2) + 1, fillRight); | ||
| branchPositions.add((face * 2) + 2, fillLeft); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| public static class StagingPositions { | ||
| // Measured from the center of the ice cream | ||
| public static final Pose2d leftIceCream = | ||
| new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d()); | ||
| public static final Pose2d middleIceCream = | ||
| new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d()); | ||
| public static final Pose2d rightIceCream = | ||
| new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d()); | ||
| } | ||
|
|
||
| public enum ReefHeight { | ||
| L4(Units.inchesToMeters(72), -90), | ||
| L3(Units.inchesToMeters(47.625), -35), | ||
| L2(Units.inchesToMeters(31.875), -35), | ||
| L1(Units.inchesToMeters(18), 0); | ||
|
|
||
| ReefHeight(double height, double pitch) { | ||
| this.height = height; | ||
| this.pitch = pitch; // in degrees | ||
| } | ||
|
|
||
| public final double height; | ||
| public final double pitch; | ||
| } | ||
|
|
||
| // TODO | ||
| // public static final double aprilTagWidth = Units.inchesToMeters(6.50); | ||
| // public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; | ||
| // public static final int aprilTagCount = 22; | ||
| // | ||
| // @Getter | ||
| // public enum AprilTagLayoutType { | ||
| // OFFICIAL("2025-official"); | ||
| // | ||
| // AprilTagLayoutType(String name) { | ||
| // if (Constants.disableHAL) { | ||
| // layout = null; | ||
| // } else { | ||
| // try { | ||
| // layout = | ||
| // new AprilTagFieldLayout( | ||
| // Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + | ||
| // ".json")); | ||
| // } catch (IOException e) { | ||
| // throw new RuntimeException(e); | ||
| // } | ||
| // } | ||
| // if (layout == null) { | ||
| // layoutString = ""; | ||
| // } else { | ||
| // try { | ||
| // layoutString = new ObjectMapper().writeValueAsString(layout); | ||
| // } catch (JsonProcessingException e) { | ||
| // throw new RuntimeException( | ||
| // "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar"); | ||
| // } | ||
| // } | ||
| // } | ||
| // | ||
| // private final AprilTagFieldLayout layout; | ||
| // private final String layoutString; | ||
| // } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.