diff --git a/lib/publish.gradle b/lib/publish.gradle index ae17b72..2a8f4cd 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.2.0' +def pubVersion = '2023.2.1' 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 5f09614..d64f680 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 @@ -10,11 +10,14 @@ 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 { AprilTagFieldLayout layout; PhotonPoseEstimator photonPoseEstimator; + Optional frozenPose; + boolean frozen = false; /* * Example of Transform3d: @@ -35,7 +38,7 @@ public class RobotFieldPosition { layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); PhotonCamera cam = new PhotonCamera(camera); - photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException { @@ -49,11 +52,26 @@ public class RobotFieldPosition { layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); PhotonCamera cam = new PhotonCamera(camera); - photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam); } public Optional getEstimatedGlobalPose() { - return photonPoseEstimator.update(); + return frozen ? frozenPose : photonPoseEstimator.update(); } + public void setLastPose(Pose2d pose) { + photonPoseEstimator.setLastPose(pose); + } + + // For when you want it to stay the same across lines + public void freeze() { + frozen = true; + frozenPose = photonPoseEstimator.update(); + } + + public void unfreeze() { + frozen = false; + } + + } \ No newline at end of file