Instead of manually duplicating logic for each camera inside VisionSubsystem, we can make a helper class to handle everything that's specific to a camera and make two instances of that.
For example, inside VisionSubsystem, define this:
private class Camera {
private final PhotonCamera photonCamera;
private final PhotonPoseEstimator photonPoseEstimator;
private final StructPublisher<Pose3d> rawFieldPose3dEntry;
public Camera(String photonVisionName, String name, Transform3d robotToCamera) {
// Initialize stuff here...
}
public void update() {
// Loop over updates here
}
private void process(PhotonPipelineResult result) {
// Process result here... Use the estimator and entry in the Camera class
}
}
Instead of manually duplicating logic for each camera inside VisionSubsystem, we can make a helper class to handle everything that's specific to a camera and make two instances of that.
For example, inside
VisionSubsystem, define this: