From 1d9ac185cb76acaee86307869ed73dc85a2c1e7c Mon Sep 17 00:00:00 2001 From: Jason Kreidler Date: Fri, 30 Aug 2024 06:53:52 -0500 Subject: [PATCH] Added getAprilTagPose method to Vision --- .../robot/subsystems/swervedrive/Vision.java | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index b65d9d34..67c8d4d3 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -6,7 +6,9 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -395,5 +397,29 @@ public void updateVisionField() field2d.getObject("tracked targets").setPoses(poses); } + /** + * Calculates a target pose relative to an AprilTag on the field. + * + * @param aprilTag The ID of the AprilTag. + * @param xOffset The X offset in meters from the AprilTag's position, positive is away from the AprilTag. + * @param yOffset The Y offset in meters from the AprilTag's position, positive is to the right of the AprilTag + * regardless of alliance. + * @param rotOffset The rotation offset in degrees from the AprilTag's orientation. + * @return The target pose of the AprilTag. + */ + public static Pose2d getAprilTagPose(int aprilTag, Double xOffset, Double yOffset, Double rotOffset) + { + Optional aprilTagPose3d = + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(aprilTag); + + Pose2d aprilTagPose2d = aprilTagPose3d.get().toPose2d(); + + Transform2d aprilTagGoalTrans2d = new Transform2d(new Translation2d(xOffset, yOffset), + new Rotation2d(Math.toRadians(rotOffset))); + + Pose2d aprilTagTargetPose2d = aprilTagPose2d.transformBy(aprilTagGoalTrans2d); + + return aprilTagTargetPose2d; + } }