FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix Repeated PoseStrategy and Added Some Extra Stuff

This commit is contained in:
Declipsonator 2023-03-09 16:27:57 -08:00
parent da4f21ffbb
commit b51d407822
1 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java
View File

@ -10,11 +10,14 @@ import org.photonvision.EstimatedRobotPose;
import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Transform3d;
public class RobotFieldPosition { public class RobotFieldPosition {
AprilTagFieldLayout layout; AprilTagFieldLayout layout;
PhotonPoseEstimator photonPoseEstimator; PhotonPoseEstimator photonPoseEstimator;
Optional<EstimatedRobotPose> frozenPose;
boolean frozen = false;
/* /*
* Example of Transform3d: * Example of Transform3d:
@ -35,7 +38,7 @@ public class RobotFieldPosition {
layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
PhotonCamera cam = new PhotonCamera(camera); 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 { public RobotFieldPosition(String camera, Transform3d robotToCam, AprilTagFields fields) throws IOException {
@ -49,11 +52,26 @@ public class RobotFieldPosition {
layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile); layout = AprilTagFieldLayout.loadFromResource(fields.m_resourceFile);
PhotonCamera cam = new PhotonCamera(camera); PhotonCamera cam = new PhotonCamera(camera);
photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); photonPoseEstimator = new PhotonPoseEstimator(layout, strategy, cam, robotToCam);
} }
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() { public Optional<EstimatedRobotPose> 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;
}
} }