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

Merge pull request #7 from FairportRobotics/tyler 

Add Some Extra RobotFieldPosition Stuff
This commit is contained in:
Tyler Wilcox 2023-03-14 22:07:24 -04:00 committed by GitHub
parent c42663d215 71eb89fadf
commit 8c1aa27634
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 22 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/publish.gradle
View File

@ -4,7 +4,7 @@ apply plugin: 'maven-publish'
ext.licenseFile = files("$rootDir/LICENSE.txt") ext.licenseFile = files("$rootDir/LICENSE.txt")
def pubVersion = '2023.2.0' def pubVersion = '2023.2.1'
def outputsFolder = file("$buildDir/allOutputs") def outputsFolder = file("$buildDir/allOutputs")

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;
}
} }