From: mixxlto Date: Mon, 19 Jan 2026 00:04:25 +0000 (-0800) Subject: put field in fieldconstants X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=32a2e42b1db348850ad8f742736eb7e27ede991f;p=FRC2026.git 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 2785d82..ac97d98 100644 --- 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 839ec00..fb36bca 100644 --- 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 50899fb..8cc7d48 100644 --- 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 *

diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 4bb493d..e77f3d2 100644 --- 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 418672b..57b9f4e 100644 --- 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 d527ab0..08bdab2 100644 --- 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()0 && pose.getY()0 && pose.getX()0 && pose.getY()-VisionConstants.field.getFieldLength()/2 && pose.getX()-VisionConstants.field.getFieldWidth()/2 && pose.getY()-FieldConstants.field.getFieldLength()/2 && pose.getX()-FieldConstants.field.getFieldWidth()/2 && pose.getY() 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 45705ba..80bab25 100644 --- 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>()); // 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 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 redPoses = VisionConstants.field.getTags().subList(5, 11).stream().map(tag->tag.pose).toList(); - List bluePoses = VisionConstants.field.getTags().subList(16, 22).stream().map(tag->tag.pose).toList(); + List redPoses = FieldConstants.field.getTags().subList(5, 11).stream().map(tag->tag.pose).toList(); + List 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); } }