diff --git a/lib/publish.gradle b/lib/publish.gradle index bd49a07..94f7526 100644 --- a/lib/publish.gradle +++ b/lib/publish.gradle @@ -4,7 +4,7 @@ apply plugin: 'maven-publish' ext.licenseFile = files("$rootDir/LICENSE.txt") -def pubVersion = '2023.1.0' +def pubVersion = '2023.1.1-SNAPSHOT' def outputsFolder = file("$buildDir/allOutputs") diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java index 2c62ef5..7ce0ffc 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java @@ -9,30 +9,52 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy; import org.photonvision.EstimatedRobotPose; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; -public class RobotFieldPosition { +public class RobotFieldPosition { AprilTagFieldLayout layout; PhotonPoseEstimator photonPoseEstimator; /* - Example of Transform3d: - - Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); - Cam mounted facing forward, half a meter forward of center, half a meter up from center. - */ + * Example of Transform3d: + * + * Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), + * new Rotation3d(0,0,0)); + * Cam mounted facing forward, half a meter forward of center, half a meter up + * from center. + */ public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException { - layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); - + PhotonCamera cam = new PhotonCamera(camera); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); - } + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields, PoseStrategy strategy) throws IOException { + layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } public Optional getEstimatedGlobalPose() { return photonPoseEstimator.update(); } - -} \ No newline at end of file +} \ No newline at end of file