From 316611f36a225ddd0399519f936d5976a67e742f Mon Sep 17 00:00:00 2001 From: mixxlto Date: Sun, 18 Jan 2026 19:16:33 -0800 Subject: [PATCH] Update AprilTagPoseTest.java --- .../frc/robot/constants/AprilTagPoseTest.java | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/test/java/frc/robot/constants/AprilTagPoseTest.java b/src/test/java/frc/robot/constants/AprilTagPoseTest.java index 80bab25..2633336 100644 --- a/src/test/java/frc/robot/constants/AprilTagPoseTest.java +++ b/src/test/java/frc/robot/constants/AprilTagPoseTest.java @@ -29,9 +29,9 @@ public class AprilTagPoseTest { // makes the field layout Vision vision = new Vision(new ArrayList>()); - // we should have 22 tags - assertEquals(22, FieldConstants.field.getTags().size()); - assertEquals(22, vision.getAprilTagFieldLayout().getTags().size()); + // we should have 32 tags + assertEquals(32, FieldConstants.field.getTags().size()); + assertEquals(32, vision.getAprilTagFieldLayout().getTags().size()); // Check each tag in the field layout for (int i = 0; i < vision.getAprilTagFieldLayout().getTags().size(); i++) { @@ -58,8 +58,8 @@ public class AprilTagPoseTest { assertEquals(p1.getRotation().getY(), p2.getRotation().getY(), 0.0001); assertEquals(p1.getRotation().getZ(), p2.getRotation().getZ(), 0.0001); - // 1-11 should be on the right, and 12-22 should be on the left - if(tagId > 11){ + // 1-16 should be on the right, and 17-36 should be on the left + if(tagId > 16){ assertTrue(p1.getX() < FieldConstants.field.getFieldLength()/2); }else{ assertTrue(p1.getX() > FieldConstants.field.getFieldLength()/2); @@ -69,8 +69,8 @@ public class AprilTagPoseTest { @Test public void testReefTags(){ - 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(); + List redPoses = FieldConstants.field.getTags().subList(1, 16).stream().map(tag->tag.pose).toList(); + List bluePoses = FieldConstants.field.getTags().subList(17, 32).stream().map(tag->tag.pose).toList(); Pose3d redCenter = findCenter(redPoses); Pose3d blueCenter = findCenter(bluePoses); @@ -80,24 +80,24 @@ public class AprilTagPoseTest { assertEquals(redCenter.getRotation().getY(), 0, 0.0001); assertEquals(blueCenter.getRotation().getY(), 0, 0.0001); assertEquals(MathUtil.angleModulus(redCenter.getRotation().getZ()), Math.PI, 0.0001); - assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), Math.PI, 0.0001); + assertEquals(MathUtil.angleModulus(blueCenter.getRotation().getZ()), 0, 0.0001); - // Y and Z should be equal - assertEquals(redCenter.getY(), blueCenter.getY(), 0.0001); + // Y are symmetrical diagonally + assertEquals(redCenter.getY(), FieldConstants.field.getFieldWidth() - blueCenter.getY(), 0.01); + // Z should be equal assertEquals(redCenter.getZ(), blueCenter.getZ(), 0.0001); // X should be mirrored 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 = FieldConstants.field.getTagPose(i+1).get(); - Pose3d blue = FieldConstants.field.getTagPose(i+12).get(); - assertEquals(red.getY(), blue.getY(), 0.0001); + for(int i = 1; i < 17; i++){ + Pose3d red = FieldConstants.field.getTagPose(i).get(); + Pose3d blue = FieldConstants.field.getTagPose(i+16).get(); + assertEquals(red.getY(), FieldConstants.field.getFieldWidth() - blue.getY(), 0.01); assertEquals(red.getZ(), blue.getZ(), 0.0001); - assertEquals(red.getZ(), redCenter.getZ(), 0.0001); 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); + assertEquals(MathUtil.angleModulus(red.getRotation().getZ()), MathUtil.angleModulus(blue.getRotation().getZ() + Math.PI), 0.0001); } } -- 2.39.5