BeaverPhotonVision

Beaverlib includes a class that can be used to quickly set up a vision subsystem, using PhotonVision.

---

Using the AprilTag Pipeline

1. To do this, first follow Photon Vision's docs and make sure you are able to detect 3D AprilTags.

2. Then, make a new file in your subsystems folder (Or wherever you keep your subsystems), named "Vision".

3. Set val Vision = BeaverPhotonVision()

• This will create a new subsystem named Vision. Every periodic, it will check each of the cameras for new results, and will call listener.update for each new result.

4. Create a new BeaverVisionCamera() for each of your connected PhotonVision cameras.

5. Set the name of the BeaverVisionCamera() to what it is on the PhotonVision Dashboard.

6. Set the robotToCamera to the offset of the camera to the center of the robot in real life (Or in CAD if you are confident).

7. Set layout to AprilTagFieldLayout.loadField(AprilTagFields.[insert this years field]).

8. Set strategy to the apriltage estimation strategy you would like to use.

• (PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR with fallbackStrategy = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE is recommended)

---

//Vision Subsystem
val Vision =
    BeaverPhotonVision(
        BeaverVisionCamera(
            "HD_USB_Camera",
            Transform3d(0.18,-0.33,0.2, Rotation3d()),
            layout = AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded),
            strategy = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
            fallbackStrategy = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
        )
    )

---

Now, you can use this data by doing Vision.listeners.add("InsertListenerName", {result, camera -> Insert Your Code here}), and the code in the lambda will run every frame a camera sees an AprilTag. You can stop the function from running again by doing Vision.listeners.remove("InsertListenerName").

Updating Odometry

If you want to estimate the robots position from the AprilTags, you can copy the following code into your Drivetrain's init function.

// Updates odometry whenever vision sees AprilTag
Vision.listeners.add(
    "UpdateOdometry",
    { result, camera ->
        if (result.targets.isEmpty()) return@add
        if (
            !result.multitagResult.isPresent && (result.targets.first().poseAmbiguity > 0.3)
        ) return@add
        val newPose = camera.getEstimatedRobotPose(result) ?: return@add
        addVisionMeasurement(newPose.toPose2d(), result.timestampSeconds)
    },
)
setVisionMeasurementStdDevs(3.0, 4.0, 5.0)

addVisionMeasurement adds a vision measurement to the SwervePoseEstimator

• (swerveDrive.addVisionMeasurement(robotPose, timestamp) for YAGSL)

setVisionMeasurementStdDevs sets the vision standard deviations for the SwervePoseEstimator

• (swerveDrive.swerveDrivePoseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(stdDevX, stdDevY, stdDevTheta)) for YAGSL)