CommonAprilTag.java

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
import java.util.concurrent.TimeUnit;
import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;

import java.util.List;

public class CommonAprilTag {

    private HardwareMap hardwareMap = null;
    private LinearOpMode opMode = null;
    private AprilTagProcessor aprilTagProcessor;
    private VisionPortal visionPortal;

    public void initialize (LinearOpMode opMode, HardwareMap hardwareMap)
    {
        this.hardwareMap = hardwareMap;
        this.opMode = opMode;

        // Create the AprilTag processor
        aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();

        visionPortal = VisionPortal.easyCreateWithDefaults(
            hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTagProcessor);

    }
    
    public void waitForCameraReady ()
    {
        while (!opMode.isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING))
        {
            opMode.sleep(20);
        }
    }
    
    /*  Manually set the camera gain and exposure.
        Can only be called AFTER calling waitForCameraReady();
        Returns true if controls are set.
     */
    public boolean setManualExposure(int exposureMS, int gain)
    {
        // Ensure Vision Portal has been setup.
        if (visionPortal == null)
        {
            return false;
        }

        if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)
        {
            return false;
        }

        // Set exposure.  Make sure we are in Manual Mode for these values to take effect.
        ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
        if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
            exposureControl.setMode(ExposureControl.Mode.Manual);
            opMode.sleep(50);
        }
        exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
        opMode.sleep(20);

        // Set Gain.
        GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
        gainControl.setGain(gain);
        opMode.sleep(20);
        return (true);
    }

    // This function returns the first ID found. Make sure the camera can only
    // see 1 AprilTag at this time.
    public int getId ()
    {
        int id = -1;

        List<AprilTagDetection> currentDetections = aprilTagProcessor.getDetections();
 
        if (currentDetections.size() > 0)
        {
            id = currentDetections.get(0).id;
        }
 
        return id;     
    }
}