diff --git a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java index 3546b53..be6f10e 100644 --- a/src/main/java/frc/robot/interfaces/IAprilTagProvider.java +++ b/src/main/java/frc/robot/interfaces/IAprilTagProvider.java @@ -3,6 +3,7 @@ package frc.robot.interfaces; import java.util.OptionalDouble; public interface IAprilTagProvider { + public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes); public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters); public OptionalDouble getTagPitchByID(int id); public OptionalDouble getTagYawByID(int id); diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/utilities/PhotonVision.java index 591e219..b6f9f2d 100644 --- a/src/main/java/frc/robot/utilities/PhotonVision.java +++ b/src/main/java/frc/robot/utilities/PhotonVision.java @@ -12,6 +12,7 @@ import org.photonvision.PhotonUtils; import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; +import org.photonvision.targeting.TargetCorner; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; @@ -61,6 +62,27 @@ public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider { ); } + @Override + public OptionalDouble getTagOffsetFromCameraCenterByID(int id, double cameraHalfXRes) { + PhotonPipelineResult result = camera.getLatestResult(); + + if (!result.hasTargets()) { + return OptionalDouble.empty(); + } + + Optional desiredTarget = getTargetFromList(result.getTargets(), id); + + if (desiredTarget.isEmpty()) { + return OptionalDouble.empty(); + } + + List corners = desiredTarget.get().getDetectedCorners(); + + return OptionalDouble.of( + corners.get(2).x - corners.get(3).x - cameraHalfXRes + ); + } + @Override public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) { PhotonPipelineResult result = camera.getLatestResult();