put field in fieldconstants

diff --git a/src/main/java/frc/robot/commands/vision/AimAtTag.java b/src/main/java/frc/robot/commands/vision/AimAtTag.java
index 2785d82769aee32ab08ad8c7174e83f7dbe57623..ac97d986a81a24475d29d19ed3f5c0a948f81e09 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.VisionConstants;
+import frc.robot.constants.FieldConstants;
 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 : VisionConstants.field.getTags()){
+    for(AprilTag tag : FieldConstants.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
index 839ec00256673313e695e3e2af63e7f994193858..fb36bcad3d082ef1cd2b1271d3a60a0303b63d9b 100644 (file)
--- a/src/main/java/frc/robot/constants/FieldConstants.java
+++ b/src/main/java/frc/robot/constants/FieldConstants.java
@@ -1,5 +1,7 @@
 package frc.robot.constants;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 
@@ -9,6 +11,9 @@ public class FieldConstants {
   /** Height of the field [meters] */
   public static final double FIELD_WIDTH = Units.inchesToMeters(26*12 + 5);
 
+  /**Apriltag layout for 2026 REBUILT */
+  public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltWelded);
+
   /** Location of hub target */
   public static final Translation3d HUB_TRANSLATION3D = new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72));
 

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
index 50899fb8ab1a5f6a8fa0aa898d1123a9e822e557..8cc7d48a227ad9423de71d055917c0c062ca27bc 100644 (file)
--- a/src/main/java/frc/robot/constants/VisionConstants.java
+++ b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -5,8 +5,6 @@ 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;
@@ -134,8 +132,6 @@ public class VisionConstants {
 
     public static final int MAX_EMPTY_TICKS = 10;
 
-       public static final AprilTagFieldLayout field = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded);
-
     /**
      * The camera poses
      * <p>

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 4bb493dc2e85a04c44d1b06113010012efb94c0f..e77f3d257cd241c3759bec0a947e9a35cd178c2a 100644 (file)
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -26,6 +26,7 @@ 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;
@@ -309,7 +310,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(VisionConstants.field.getFieldLength()/2, VisionConstants.field.getFieldWidth()/2, pose2.getRotation());
+            prevPose = new Pose2d(FieldConstants.field.getFieldLength()/2, FieldConstants.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 418672b1526f6d3618af6cccbd05dd6d6e6cfd8b..57b9f4e2d9dc77f2538922818db6382d0fa61e8f 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.VisionConstants;
+import frc.robot.constants.FieldConstants;
 
 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(VisionConstants.field.getFieldLength(), VisionConstants.field.getFieldWidth(), new Rotation2d(Math.PI)));
+            return pose.relativeTo(new Pose2d(FieldConstants.field.getFieldLength(), FieldConstants.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 d527ab0fc50f607174809f7e8f4e0e96e76f8703..08bdab2c4b3e495117d17165838853794cdf182a 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/Vision.java
+++ b/src/main/java/frc/robot/util/Vision/Vision.java
@@ -32,6 +32,7 @@ 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;
@@ -74,7 +75,7 @@ public class Vision {
     NetworkTableInstance.getDefault().startServer();
 
     // Sets the origin to the right side of the blue alliance wall
-    VisionConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+    FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide);
 
     if(VisionConstants.ENABLED){
       // Puts the cameras in an array list
@@ -84,7 +85,7 @@ public class Vision {
 
       if(RobotBase.isSimulation()){
         visionSim = new VisionSystemSim("Vision");
-        visionSim.addAprilTags(VisionConstants.field);
+        visionSim.addAprilTags(FieldConstants.field);
         for(VisionCamera c : cameras){
           PhotonCameraSim cameraSim = new PhotonCameraSim(c.camera);
           cameraSim.enableDrawWireframe(true);
@@ -257,7 +258,7 @@ public class Vision {
   }
 
   public AprilTagFieldLayout getAprilTagFieldLayout(){
-    return VisionConstants.field;
+    return FieldConstants.field;
   }
 
   /**
@@ -401,7 +402,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()<VisionConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<VisionConstants.field.getFieldWidth();
+    return pose!=null && pose.getX()>0 && pose.getX()<FieldConstants.field.getFieldLength() && pose.getY()>0 && pose.getY()<FieldConstants.field.getFieldWidth();
   }
 
   /**
@@ -410,7 +411,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()>-VisionConstants.field.getFieldLength()/2 && pose.getX()<VisionConstants.field.getFieldLength()*1.5 && pose.getY()>-VisionConstants.field.getFieldWidth()/2 && pose.getY()<VisionConstants.field.getFieldWidth()*1.5;
+    return pose!=null && pose.getX()>-FieldConstants.field.getFieldLength()/2 && pose.getX()<FieldConstants.field.getFieldLength()*1.5 && pose.getY()>-FieldConstants.field.getFieldWidth()/2 && pose.getY()<FieldConstants.field.getFieldWidth()*1.5;
   }
   
   private class VisionCamera implements VisionIO {
@@ -429,7 +430,7 @@ public class Vision {
     public VisionCamera(String cameraName, Transform3d robotToCam) {
       camera = new PhotonCamera(cameraName);
       photonPoseEstimator = new PhotonPoseEstimator(
-        VisionConstants.field, 
+        FieldConstants.field, 
         robotToCam
       );
       lastPose = null;
@@ -630,7 +631,7 @@ public class Vision {
           continue;
         }
         // Stores target pose and robot to camera transformation for easy access later
-        Pose3d targetPose = VisionConstants.field.getTagPose(id).get();
+        Pose3d targetPose = FieldConstants.field.getTagPose(id).get();
         Transform3d robotToCamera = photonPoseEstimator.getRobotToCameraTransform();
 
         double timestamp = result.getTimestampSeconds();
@@ -665,7 +666,7 @@ public class Vision {
 
     public boolean useTag(int id){
       // Never use tags that don't exist
-      if(id <= 0 || id > VisionConstants.field.getTags().size()){
+      if(id <= 0 || id > FieldConstants.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 45705ba6b4328b6c54f206366ef21b63b2e0a260..80bab25d13a2843b576f6e43efd5a389a6e7ba6a 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, VisionConstants.field.getTags().size());
+    assertEquals(22, FieldConstants.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 = VisionConstants.field.getTags().get(i);
+      AprilTag apriltag1 = FieldConstants.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() < VisionConstants.field.getFieldLength()/2);
+        assertTrue(p1.getX() < FieldConstants.field.getFieldLength()/2);
       }else{
-        assertTrue(p1.getX() > VisionConstants.field.getFieldLength()/2);
+        assertTrue(p1.getX() > FieldConstants.field.getFieldLength()/2);
       }
     }
   }
 
   @Test
   public void testReefTags(){
-    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();
+    List<Pose3d> redPoses = FieldConstants.field.getTags().subList(5, 11).stream().map(tag->tag.pose).toList();
+    List<Pose3d> bluePoses = FieldConstants.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(), VisionConstants.field.getFieldLength()-blueCenter.getX(), 0.01);
+    assertEquals(redCenter.getX(), FieldConstants.field.getFieldLength()-blueCenter.getX(), 0.01);
 
     // Compare each matching pair of tags
     for(int i = 5; i < 11; i++){
-      Pose3d red = VisionConstants.field.getTagPose(i+1).get();
-      Pose3d blue = VisionConstants.field.getTagPose(i+12).get();
+      Pose3d red = FieldConstants.field.getTagPose(i+1).get();
+      Pose3d blue = FieldConstants.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(), VisionConstants.field.getFieldLength()-blue.getX(), 0.01);
+      assertEquals(red.getX(), FieldConstants.field.getFieldLength()-blue.getX(), 0.01);
       assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(Math.PI-blue.getRotation().getZ()), 0.0001);
     }
   }