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

quick fix

This commit is contained in:
Declipsonator 2023-01-21 11:31:27 -08:00
parent 9e37df4f0e
commit ff0b1f258c
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java
View File

@ -6,6 +6,8 @@ import java.util.List;
import org.photonvision.PhotonCamera; import org.photonvision.PhotonCamera;
import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.math.geometry.Transform3d;
public class AprilTags { public class AprilTags {
// an ArrayList to store the cameras // an ArrayList to store the cameras
public final ArrayList<PhotonCamera> cameras = new ArrayList<>(); public final ArrayList<PhotonCamera> cameras = new ArrayList<>();
@ -59,7 +61,7 @@ public class AprilTags {
if(!hasTargets()) return null; if(!hasTargets()) return null;
PhotonTrackedTarget closestTarget = null; PhotonTrackedTarget closestTarget = null;
for(PhotonTrackedTarget target : getTargets()) { for(PhotonTrackedTarget target : getTargets()) {
if(closestTarget == null || Math.abs(target.getBestCameraToTarget().getY()) > Math.abs(target.getBestCameraToTarget().getY()) ) closestTarget = target; if(closestTarget == null || calculateDistance(target.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget()) ) closestTarget = target;
} }
return closestTarget; return closestTarget;
} }
@ -75,4 +77,7 @@ public class AprilTags {
return lowestAmbiguity; return lowestAmbiguity;
} }
private double calculateDistance(Transform3d t) {
return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY() + t.getZ()*t.getZ());
}
} }