Delete FieldConstants.

diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java
index 9473d327645dd5b555e703455220824e28ccd9c8..2785d82769aee32ab08ad8c7174e83f7dbe57623 100644 (file)
--- a/src/main/java/frc/robot/commands/vision/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/vision/AimAtTag.java
@@ -5,7 +5,7 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.drivetrain.Drivetrain;
 
 /**
@@ -39,7 +39,7 @@ public class AimAtTag extends Command {
     double dist = Double.POSITIVE_INFINITY;
     Translation2d closest = new Translation2d();
     Translation2d driveTranslation = drive.getPose().getTranslation();
-    for(AprilTag tag : FieldConstants.APRIL_TAGS){
+    for(AprilTag tag : VisionConstants.field.getTags()){
       Translation2d translation = tag.pose.toPose2d().getTranslation();
       double dist2 = driveTranslation.getDistance(translation);
       if(dist2 < dist){

diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java
deleted file mode 100644 (file)
index 03c90e0..0000000
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ /dev/null
@@ -1,46 +0,0 @@
-package frc.robot.constants;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.util.Units;
-
-public class FieldConstants {
-  /** Width of the field [meters] */
-  public static final double FIELD_LENGTH = Units.inchesToMeters(57*12 + 6+7.0/8.0);
-  /** Height of the field [meters] */
-  public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5);
-
-  /** 
-   * ArrayList of April Tags to use when the field layout is not available.
-   * <p>
-   * Obtain AprilTag ID i with APRIL_TAGS.get(i-1).
-   */
-  public static final ArrayList<AprilTag> APRIL_TAGS = new ArrayList<AprilTag>(List.of(
-    new AprilTag(1 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(126)))),
-    new AprilTag(2 , new Pose3d(Units.inchesToMeters(657.37), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(234)))),
-    new AprilTag(3 , new Pose3d(Units.inchesToMeters(455.15), Units.inchesToMeters(317.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(270)))),
-    new AprilTag(4 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))),
-    new AprilTag(5 , new Pose3d(Units.inchesToMeters(365.20), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), 0))),
-    new AprilTag(6 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300)))),
-    new AprilTag(7 , new Pose3d(Units.inchesToMeters(546.87), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))),
-    new AprilTag(8 , new Pose3d(Units.inchesToMeters(530.49), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))),
-    new AprilTag(9 , new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))),
-    new AprilTag(10, new Pose3d(Units.inchesToMeters(481.39), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))),
-    new AprilTag(11, new Pose3d(Units.inchesToMeters(497.77), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))),
-    new AprilTag(12, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(25.80), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(54)))),
-    new AprilTag(13, new Pose3d(Units.inchesToMeters(33.51), Units.inchesToMeters(291.20), Units.inchesToMeters(58.50), new Rotation3d(0, 0, Units.degreesToRadians(306)))),
-    new AprilTag(14, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(241.64), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))),
-    new AprilTag(15, new Pose3d(Units.inchesToMeters(325.68), Units.inchesToMeters(75.39), Units.inchesToMeters(73.54), new Rotation3d(0, Units.degreesToRadians(30), Units.degreesToRadians(180)))),
-    new AprilTag(16, new Pose3d(Units.inchesToMeters(235.73), Units.inchesToMeters(-0.15), Units.inchesToMeters(51.25), new Rotation3d(0, 0, Units.degreesToRadians(90)))),
-    new AprilTag(17, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(240)))),
-    new AprilTag(18, new Pose3d(Units.inchesToMeters(144.00), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(180)))),
-    new AprilTag(19, new Pose3d(Units.inchesToMeters(160.39), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(120)))),
-    new AprilTag(20, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(186.83), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(60)))),
-    new AprilTag(21, new Pose3d(Units.inchesToMeters(209.49), Units.inchesToMeters(158.50), Units.inchesToMeters(12.13), new Rotation3d(0, 0, 0))),
-    new AprilTag(22, new Pose3d(Units.inchesToMeters(193.10), Units.inchesToMeters(130.17), Units.inchesToMeters(12.13), new Rotation3d(0, 0, Units.degreesToRadians(300))))
-  ));
-}

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 4d61900c31137e0460e6d59e0e79488c59c35369..c19d84676955f98226709a4725efb2e59c3b26ac 100644 (file)
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -5,6 +5,8 @@ import java.util.List;
 
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.VecBuilder;
@@ -132,6 +134,8 @@ public class VisionConstants {
 
     public static final int MAX_EMPTY_TICKS = 10;
 
+       public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
+
     /**
      * The camera poses
      * <p>
@@ -174,4 +178,4 @@ public class VisionConstants {
             new Transform3d(
                     new Translation3d(Units.inchesToMeters(10), 0, Units.inchesToMeters(24)),
                     new Rotation3d(0, Units.degreesToRadians(20), 0))));
-}
\ No newline at end of file
+}

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 8edbb783cc6e1edd9eb5885eedef08cef74df9cc..4bb493dc2e85a04c44d1b06113010012efb94c0f 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -26,7 +26,6 @@ import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
-import frc.robot.constants.FieldConstants;
 import frc.robot.constants.VisionConstants;
 import frc.robot.constants.swerve.DriveConstants;
 import frc.robot.constants.swerve.ModuleConstants;
@@ -310,7 +309,7 @@ public class Drivetrain extends SubsystemBase {
         if(!Vision.nearField(prevPose)){
             // If the pose at the beginning of the method is off the field, reset to a position in the middle of the field
             // Use the rotation of the pose after updating odometry so the yaw is right
-            prevPose = new Pose2d(FieldConstants.FIELD_LENGTH/2, FieldConstants.FIELD_WIDTH/2, pose2.getRotation());
+            prevPose = new Pose2d(VisionConstants.field.getFieldLength()/2, VisionConstants.field.getFieldWidth()/2, pose2.getRotation());
             resetOdometry(prevPose);
         }else if(!Vision.nearField(pose2)){
             // if the drivetrain pose is off the field, reset our odometry to the pose before(this is the right pose)

diff --git a/src/main/java/frc/robot/util/ConversionUtils.java b/src/main/java/frc/robot/util/ConversionUtils.java
index 7c6da1685a9be6109f35b2ed7e3c551f16c7f6f8..418672b1526f6d3618af6cccbd05dd6d6e6cfd8b 100644 (file)
--- a/src/main/java/frc/robot/util/ConversionUtils.java
+++ b/src/main/java/frc/robot/util/ConversionUtils.java
@@ -3,7 +3,7 @@ package frc.robot.util;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import frc.robot.constants.FieldConstants;
+import frc.robot.constants.VisionConstants;
 
 public class ConversionUtils {
 
@@ -139,7 +139,7 @@ public class ConversionUtils {
      */
     public static Pose2d absolutePoseToPathPlannerPose(Pose2d pose, Alliance alliance) {
         if (alliance == Alliance.Red) {
-            return pose.relativeTo(new Pose2d(FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH, new Rotation2d(Math.PI)));
+            return pose.relativeTo(new Pose2d(VisionConstants.field.getFieldLength(), VisionConstants.field.getFieldWidth(), new Rotation2d(Math.PI)));
         }
         return new Pose2d(pose.getX(), pose.getY(), pose.getRotation());
     }

diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java
index 0501edb5598c39f526a3a61d3c1e6830e4e23671..24bc4149f8cbd6b74cdc1e6ee7ee3c96b8d85f50 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/Vision.java
+++ b/src/main/java/frc/robot/util/Vision/Vision.java
@@ -32,7 +32,6 @@ import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.Timer;
-import frc.robot.constants.FieldConstants;
 import frc.robot.constants.VisionConstants;
 import frc.robot.constants.swerve.DriveConstants;
 import frc.robot.util.MathUtils;
@@ -47,8 +46,6 @@ public class Vision {
   private NetworkTableEntry objectClass;
   private NetworkTableEntry cameraIndex;
   
-  // The field layout. Instance variable
-  private AprilTagFieldLayout aprilTagFieldLayout;
   // A list of the cameras on the robot.
   private ArrayList<VisionCamera> cameras = new ArrayList<>();
 
@@ -76,11 +73,8 @@ public class Vision {
     // Start NetworkTables server
     NetworkTableInstance.getDefault().startServer();
 
-    // Load field layout
-    aprilTagFieldLayout = new AprilTagFieldLayout(FieldConstants.APRIL_TAGS, FieldConstants.FIELD_LENGTH, FieldConstants.FIELD_WIDTH);
-
     // Sets the origin to the right side of the blue alliance wall
-    aprilTagFieldLayout.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+    VisionConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
 
     if(VisionConstants.ENABLED){
       // Puts the cameras in an array list
@@ -90,7 +84,7 @@ public class Vision {
 
       if(RobotBase.isSimulation()){
         visionSim = new VisionSystemSim("Vision");
-        visionSim.addAprilTags(aprilTagFieldLayout);
+        visionSim.addAprilTags(VisionConstants.field);
         for(VisionCamera c : cameras){
           PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
           cameraSim.enableDrawWireframe(true);
@@ -263,7 +257,7 @@ public class Vision {
   }
 
   public AprilTagFieldLayout getAprilTagFieldLayout(){
-    return aprilTagFieldLayout;
+    return VisionConstants.field;
   }
 
   /**
@@ -407,7 +401,7 @@ public class Vision {
    * @return If the pose is on the field
    */
   public static boolean onField(Pose2d pose){
-    return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.FIELD_LENGTH && pose.getY()>0 && pose.getY()<FieldConstants.FIELD_WIDTH;
+    return pose!=null && pose.getX()>0 && pose.getX()<VisionConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<VisionConstants.field.getFieldWidth();
   }
 
   /**
@@ -416,7 +410,7 @@ public class Vision {
    * @return If the pose is within an area with twice the length and width of the field
    */
   public static boolean nearField(Pose2d pose){
-    return pose!=null && pose.getX()>-FieldConstants.FIELD_LENGTH/2 && pose.getX()<FieldConstants.FIELD_LENGTH*1.5 && pose.getY()>-FieldConstants.FIELD_WIDTH/2 && pose.getY()<FieldConstants.FIELD_WIDTH*1.5;
+    return pose!=null && pose.getX()>-VisionConstants.field.getFieldLength()/2 && pose.getX()<VisionConstants.field.getFieldLength()*1.5 && pose.getY()>-VisionConstants.field.getFieldWidth()/2 && pose.getY()<VisionConstants.field.getFieldWidth()*1.5;
   }
   
   private class VisionCamera implements VisionIO {
@@ -435,7 +429,7 @@ public class Vision {
     public VisionCamera(String cameraName, Transform3d robotToCam) {
       camera = new PhotonCamera(cameraName);
       photonPoseEstimator = new PhotonPoseEstimator(
-        aprilTagFieldLayout, 
+        VisionConstants.field, 
         VisionConstants.POSE_STRATEGY, 
         robotToCam
       );
@@ -615,7 +609,7 @@ public class Vision {
           continue;
         }
         // Stores target pose and robot to camera transformation for easy access later
-        Pose3d targetPose = FieldConstants.APRIL_TAGS.get(id-1).pose;
+        Pose3d targetPose = VisionConstants.field.getTagPose(id).get();
         Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform();
 
         double timestamp = result.getTimestampSeconds();
@@ -650,7 +644,7 @@ public class Vision {
 
     public boolean useTag(int id){
       // Never use tags that don't exist
-      if(id <= 0 || id > FieldConstants.APRIL_TAGS.size()){
+      if(id <= 0 || id > VisionConstants.field.getTags().size()){
         return false;
       }
       // Return false if it is in the list of tags to ignore

diff --git a/src/test/java/frc/robot/constants/AprilTagPoseTest.java b/src/test/java/frc/robot/constants/AprilTagPoseTest.java
index b5ce24d1b398e9f4c220b3825aa79a5357be8f70..45705ba6b4328b6c54f206366ef21b63b2e0a260 100644 (file)
--- a/src/test/java/frc/robot/constants/AprilTagPoseTest.java
+++ b/src/test/java/frc/robot/constants/AprilTagPoseTest.java
@@ -30,7 +30,7 @@ public class AprilTagPoseTest {
     Vision vision = new Vision(new ArrayList<Pair<String, Transform3d>>());
 
     // we should have 22 tags
-    assertEquals(22, FieldConstants.APRIL_TAGS.size());
+    assertEquals(22, VisionConstants.field.getTags().size());
     assertEquals(22, vision.getAprilTagFieldLayout().getTags().size());
 
     // Check each tag in the field layout
@@ -40,7 +40,7 @@ public class AprilTagPoseTest {
 
       // Get the poses from the two sources
       // From the ArrayList<AprilTag> source
-      AprilTag apriltag1 = FieldConstants.APRIL_TAGS.get(i);
+      AprilTag apriltag1 = VisionConstants.field.getTags().get(i);
       Pose3d p1 = apriltag1.pose;
       // From the vision source
       Pose3d p2 = vision.getTagPose(tagId);
@@ -60,17 +60,17 @@ public class AprilTagPoseTest {
 
       // 1-11 should be on the right, and 12-22 should be on the left
       if(tagId > 11){
-        assertTrue(p1.getX() < FieldConstants.FIELD_LENGTH/2);
+        assertTrue(p1.getX() < VisionConstants.field.getFieldLength()/2);
       }else{
-        assertTrue(p1.getX() > FieldConstants.FIELD_LENGTH/2);
+        assertTrue(p1.getX() > VisionConstants.field.getFieldLength()/2);
       }
     }
   }
 
   @Test
   public void testReefTags(){
-    List<Pose3d> redPoses = FieldConstants.APRIL_TAGS.subList(5, 11).stream().map(tag->tag.pose).toList();
-    List<Pose3d> bluePoses = FieldConstants.APRIL_TAGS.subList(16, 22).stream().map(tag->tag.pose).toList();
+    List<Pose3d> redPoses = VisionConstants.field.getTags().subList(5, 11).stream().map(tag->tag.pose).toList();
+    List<Pose3d> bluePoses = VisionConstants.field.getTags().subList(16, 22).stream().map(tag->tag.pose).toList();
     Pose3d redCenter = findCenter(redPoses);
     Pose3d blueCenter = findCenter(bluePoses);
     
@@ -87,16 +87,16 @@ public class AprilTagPoseTest {
     assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001);
 
     // X should be mirrored
-    assertEquals(redCenter.getX(), FieldConstants.FIELD_LENGTH-blueCenter.getX(), 0.01);
+    assertEquals(redCenter.getX(), VisionConstants.field.getFieldLength()-blueCenter.getX(), 0.01);
 
     // Compare each matching pair of tags
     for(int i = 5; i < 11; i++){
-      Pose3d red = FieldConstants.APRIL_TAGS.get(i).pose;
-      Pose3d blue = FieldConstants.APRIL_TAGS.get(i+11).pose;
+      Pose3d red = VisionConstants.field.getTagPose(i+1).get();
+      Pose3d blue = VisionConstants.field.getTagPose(i+12).get();
       assertEquals(red.getY(), blue.getY(), 0.0001);
       assertEquals(red.getZ(), blue.getZ(), 0.0001);
       assertEquals(red.getZ(), redCenter.getZ(), 0.0001);
-      assertEquals(red.getX(), FieldConstants.FIELD_LENGTH-blue.getX(), 0.01);
+      assertEquals(red.getX(), VisionConstants.field.getFieldLength()-blue.getX(), 0.01);
       assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(Math.PI-blue.getRotation().getZ()), 0.0001);
     }
   }