diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml index 8ff3b6e..02da1ca 100644 --- a/FtcRobotController/src/main/AndroidManifest.xml +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -2,7 +2,7 @@ + android:versionName="12.0"> diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java index 44abd97..f8d754c 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -38,8 +38,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCompatibil import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.internal.usb.UsbConstants; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -150,12 +152,12 @@ public class ConceptAprilTag extends LinearOpMode { aprilTag = new AprilTagProcessor.Builder() // The following default settings are available to un-comment and edit as needed. - //.setDrawAxes(false) - //.setDrawCubeProjection(false) + //.setDrawAxes(true) // Changed in V12.0 //.setDrawTagOutline(true) //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + //.setDrawCubeProjection(false) // == CAMERA CALIBRATION == // If you do not manually specify calibration parameters, the SDK will attempt @@ -220,14 +222,25 @@ public class ConceptAprilTag extends LinearOpMode { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index 76ace3c..085f133 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -36,8 +36,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDir import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCompatibilityManager; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -45,6 +47,8 @@ import java.util.List; * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using * the easy way. * + * Note: See ConceptAprilTag.java for how to add a camera compatibility quirk + * * For an introduction to AprilTags, see the FTC-DOCS link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * @@ -78,33 +82,10 @@ public class ConceptAprilTagEasy extends LinearOpMode { */ private VisionPortal visionPortal; - // To find the VID/PID for a camera: - // - // Linux: open a terminal, run "lsusb", locate the line for your camera, - // and find the section that resembles "ID 1d6b:0002"; this is VID:PID - // - // OSX: open a terminal, run "system_profiler SPUSBDataType", locate the - // section for your camera, and find the "Product ID:" and "Vendor ID:" - // listings in the output - // - // Windows: open a PowerShell, run: - // Get-PnpDevice -PresentOnly | Where-Object { $_.InstanceId -like 'USB*' } | Select-Object FriendlyName, InstanceId - // and locate the line for your camera. The VID and PID is listed directly in the line. - static final int VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY = 0x1BCF; - static final int PRODUCT_ID_ARDUCAM_OV5648 = 0x284C; - @Override public void runOpMode() { - // Demonstrate how to add a camera compatibility quirk - // these can sometimes be needed if a camera behaves poorly. - // Quirks have no effect unless the camera you are using matches the specified VID/PID - CameraCompatibilityManager.getInstance() - .addQuirk( - VENDOR_ID_SUNPLUS_INNOVATION_TECHNOLOGY, - PRODUCT_ID_ARDUCAM_OV5648, - CameraCompatibilityManager.Quirk.AVOID_LIB_USB_RESET_DEVICE); - + // See ConceptAprilTag.java for how to add a camera compatibility quirk initAprilTag(); // Wait for the DS start button to be touched. @@ -165,14 +146,25 @@ public class ConceptAprilTagEasy extends LinearOpMode { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java index a14b971..53bed1a 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -43,6 +43,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -177,7 +178,7 @@ public class ConceptAprilTagLocalization extends LinearOpMode { aprilTag = new AprilTagProcessor.Builder() // The following default settings are available to un-comment and edit as needed. - //.setDrawAxes(false) + //.setDrawAxes(true) // changed in V12.0 //.setDrawCubeProjection(false) //.setDrawTagOutline(true) //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) @@ -247,22 +248,23 @@ public class ConceptAprilTagLocalization extends LinearOpMode { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - // Only use tags that don't have Obelisk in them - if (!detection.metadata.name.contains("Obelisk")) { - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("Robot XYZ %6.1f %6.1f %6.1f (inch)", detection.robotPose.getPosition().x, detection.robotPose.getPosition().y, detection.robotPose.getPosition().z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + telemetry.addLine(String.format("Robot PRY %6.1f %6.1f %6.1f (deg)", detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); } - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java index 02e83d3..06068b3 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -37,8 +37,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.VisionPortal.CameraState; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; @@ -153,14 +155,25 @@ public class ConceptAprilTagSwitchableCameras extends LinearOpMode { // Step through the list of detections and display info for each one. for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + + if (singleDet.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", singleDet.id, singleDet.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", singleDet.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", singleDet.center.x, singleDet.center.y)); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + telemetry.addLine(String.format("\n==== Tag Cluster (%s)", clusterDet.metadata.name)); + telemetry.addLine(String.format("Percent tags found: %d", clusterDet.percentClusterFound)); telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } } // end for() loop diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java index 4b777e2..af58d40 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -39,25 +39,32 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; import java.util.concurrent.TimeUnit; /* - * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster + * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name. * The code assumes a Holonomic (Mecanum or X Drive) Robot. * * For an introduction to AprilTags, see the ftc-docs link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * - * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera. * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. * https://ftc-docs.firstinspires.org/apriltag-detection-values * - * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and - * driving towards the tag to achieve the desired distance. + * For a single tag, the "Drive Target" is the center of the Tag. For a cluster of tags, the "Drive Target" will be the + * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order + * to help to locate a game objective. + * + * The driving goal is to rotate to keep the Target centered in the camera, while strafing to be directly in front of the target, + * and driving towards the target to achieve the desired distance. * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. * @@ -73,9 +80,9 @@ import java.util.concurrent.TimeUnit; * Release the Left Bumper to return to manual driving mode. * * Under "Drive To Target" mode, the robot has three goals: - * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) - * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) - * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Strafe the robot towards the centerline of the Target, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) + * 3) Drive towards the Target to get to the desired distance. (Use TargetRange to drive the robot forward/backward) * * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants. @@ -90,7 +97,7 @@ import java.util.concurrent.TimeUnit; public class RobotAutoDriveToAprilTagOmni extends LinearOpMode { // Adjust these numbers to suit your robot. - final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + final double DESIRED_DISTANCE = 30.0; // this is how close the camera should get to the target (inches) // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. @@ -99,24 +106,31 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0) final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) - final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) - final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) - final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) private DcMotor frontLeftDrive = null; // Used to control the left front drive wheel - private DcMotor frontRightDrive = null; // Used to control the right front drive wheel - private DcMotor backLeftDrive = null; // Used to control the left back drive wheel + private DcMotor frontRightDrive = null; // Used to control the right front drive wheel + private DcMotor backLeftDrive = null; // Used to control the left back drive wheel private DcMotor backRightDrive = null; // Used to control the right back drive wheel - private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera - private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private final int DESIRED_TAG_ID = -1; // The tag you want to approach, or set to -1 for ANY tag. + private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster. + private VisionPortal visionPortal; // Used to manage the video source. private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. - private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + private boolean targetFound = false; // Set to true when an AprilTag/Cluster target is detected + private String targetName = "none"; + private int targetID = 0; + private double targetRange = 0; + private double targetBearing = 0; + private double targetYaw = 0; @Override public void runOpMode() { - boolean targetFound = false; // Set to true when an AprilTag target is detected double drive = 0; // Desired forward power/speed (-1 to +1) double strafe = 0; // Desired strafe power/speed (-1 to +1) double turn = 0; // Desired turning power/speed (-1 to +1) @@ -152,36 +166,57 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode while (opModeIsActive()) { targetFound = false; - desiredTag = null; // Step through the list of detected tags and look for a matching tag List currentDetections = aprilTag.getDetections(); for (AprilTagDetection detection : currentDetections) { - // Look to see if we have size info on this tag. - if (detection.metadata != null) { - // Check to see if we want to track towards this tag. - if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { - // Yes, we want to use this tag. - targetFound = true; - desiredTag = detection; - break; // don't look any further. + + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection; + + // Look to see if we have size info on this tag. + if (singleDetection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetName = singleDetection.metadata.name; + targetID = singleDetection.id; + targetRange = singleDetection.ftcPose.range; + targetBearing = singleDetection.ftcPose.bearing; + targetYaw = singleDetection.ftcPose.yaw; + targetFound = true; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id); + } } else { - // This tag is in the library, but we do not want to track it right now. - telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id); } } else { - // This tag is NOT in the library, so we don't have enough information to track to it. - telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + + if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) { + // Yes, we want to use this tag. + targetName = clusterDet.metadata.shortName; + targetID = -1; + targetRange = clusterDet.ftcPose.range; + targetBearing = clusterDet.ftcPose.bearing; + targetYaw = clusterDet.ftcPose.yaw; + targetFound = true; + break; // don't look any further. + } } } // Tell the driver what we see, and what to do. if (targetFound) { telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); - telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); - telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); - telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); - telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw); + telemetry.addData("Found", "ID %d (%s)", targetID, targetName); + telemetry.addData("Range", "%5.1f inches", targetRange); + telemetry.addData("Bearing","%3.0f degrees", targetBearing); + telemetry.addData("Yaw","%3.0f degrees", targetYaw); } else { telemetry.addData("\n>","Drive using joysticks to find valid target\n"); } @@ -190,9 +225,9 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode if (gamepad1.left_bumper && targetFound) { // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically. - double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); - double headingError = desiredTag.ftcPose.bearing; - double yawError = desiredTag.ftcPose.yaw; + double rangeError = targetRange - DESIRED_DISTANCE; + double headingError = targetBearing; + double yawError = targetYaw; // Use the speed and turn "gains" to calculate how we want the robot to move. drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); @@ -201,7 +236,6 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); } else { - // drive using manual POV Joystick mode. Slow things down to make the robot more controlable. drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%. @@ -218,11 +252,8 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode /** * Move robot according to desired axes motions - *

* Positive X is forward - *

* Positive Y is strafe left - *

* Positive Yaw is counter-clockwise */ public void moveRobot(double x, double y, double yaw) { diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java index ba3eb4f..09cad83 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java @@ -39,44 +39,50 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagClusterDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagSingleDetection; import java.util.List; import java.util.concurrent.TimeUnit; /* - * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. - * The code assumes a basic two-wheel (Tank) Robot Drivetrain + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag or AprilTag Cluster + * A "Cluster" is a group of Apriltags that share a common origin, and are identified by name. + * The code assumes a basic two-motor Tank (differential) drive robot. * * For an introduction to AprilTags, see the ftc-docs link below: * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html * - * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * When an AprilTag/Cluster in the TagLibrary is detected, the SDK provides location and orientation of the target, relative to the camera. * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. * https://ftc-docs.firstinspires.org/apriltag-detection-values * - * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance. - * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) - * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * For a single tag, the "Drive Target" is the center of the Tag. For a cluster of tags, the "Drive Target" will be the + * (0,0,0) ORIGIN of the cluster, which may have been positioned somewhere other than the center of the cluster in order + * to help to locate a game objective. * - * The code assumes a Robot Configuration with motors named left_drive and right_drive. - * The motor directions must be set so a positive power goes forward on both wheels; - * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default + * The driving goal is to rotate to keep the Target centered in the camera, while driving towards the target to achieve the desired distance. + * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) + * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * + * The code assumes a Robot Configuration with motors named: left_drive and right_drive. + * The motor directions must be set so a positive power goes forward on all wheels. + * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, * so you should choose to approach a valid tag ID. * - * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot. - * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel). - * + * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot. * Manually drive the robot until it displays Target data on the Driver Station. + * * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. * Release the Left Bumper to return to manual driving mode. * - * Under "Drive To Target" mode, the robot has two goals: - * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) - * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * Under "Drive To Target" mode, the robot has two goals: + * 1) Turn the robot to always keep the Target centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Drive towards the Target to get to the desired distance. (Use TargetRange to drive the robot forward/backward) * - * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants. * * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. @@ -89,7 +95,7 @@ import java.util.concurrent.TimeUnit; public class RobotAutoDriveToAprilTagTank extends LinearOpMode { // Adjust these numbers to suit your robot. - final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + final double DESIRED_DISTANCE = 30.0; // this is how close the camera should get to the target (inches) // Set the GAIN constants to control the relationship between the measured position error, and how much power is // applied to the drive motors to correct the error. @@ -103,15 +109,22 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode private DcMotor leftDrive = null; // Used to control the left drive wheel private DcMotor rightDrive = null; // Used to control the right drive wheel - private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera - private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private final int DESIRED_TAG_ID = -1; // The tag you want to approach, or set to -1 for ANY tag. + private final String DESIRED_CLUSTER_NAME = null; // The cluster name you want to approach, or set null for ANY cluster. + private VisionPortal visionPortal; // Used to manage the video source. private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. - private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + private boolean targetFound = false; // Set to true when an AprilTag/Cluster target is detected + private String targetName = "none"; + private int targetID = 0; + private double targetRange = 0; + private double targetBearing = 0; + private double targetYaw = 0; @Override public void runOpMode() { - boolean targetFound = false; // Set to true when an AprilTag target is detected double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise @@ -142,35 +155,57 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode while (opModeIsActive()) { targetFound = false; - desiredTag = null; // Step through the list of detected tags and look for a matching tag List currentDetections = aprilTag.getDetections(); for (AprilTagDetection detection : currentDetections) { - // Look to see if we have size info on this tag. - if (detection.metadata != null) { - // Check to see if we want to track towards this tag. - if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { - // Yes, we want to use this tag. - targetFound = true; - desiredTag = detection; - break; // don't look any further. + + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDetection = (AprilTagSingleDetection) detection; + + // Look to see if we have size info on this tag. + if (singleDetection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (singleDetection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetName = singleDetection.metadata.name; + targetID = singleDetection.id; + targetRange = singleDetection.ftcPose.range; + targetBearing = singleDetection.ftcPose.bearing; + targetYaw = singleDetection.ftcPose.yaw; + targetFound = true; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", singleDetection.id); + } } else { - // This tag is in the library, but we do not want to track it right now. - telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", singleDetection.id); + } + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + + if (DESIRED_CLUSTER_NAME == null || clusterDet.metadata.shortName.equals(DESIRED_CLUSTER_NAME) ) { + // Yes, we want to use this tag. + targetName = clusterDet.metadata.shortName; + targetID = -1; + targetRange = clusterDet.ftcPose.range; + targetBearing = clusterDet.ftcPose.bearing; + targetYaw = clusterDet.ftcPose.yaw; + targetFound = true; + break; // don't look any further. } - } else { - // This tag is NOT in the library, so we don't have enough information to track to it. - telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); } } // Tell the driver what we see, and what to do. if (targetFound) { telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); - telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); - telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); - telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + telemetry.addData("Found", "ID %d (%s)", targetID, targetName); + telemetry.addData("Range", "%5.1f inches", targetRange); + telemetry.addData("Bearing","%3.0f degrees", targetBearing); + telemetry.addData("Yaw","%3.0f degrees", targetYaw); } else { telemetry.addData("\n>","Drive using joysticks to find valid target\n"); } @@ -179,8 +214,8 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode if (gamepad1.left_bumper && targetFound) { // Determine heading and range error so we can use them to control the robot automatically. - double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); - double headingError = desiredTag.ftcPose.bearing; + double rangeError = targetRange - DESIRED_DISTANCE; + double headingError = targetBearing; // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); @@ -188,7 +223,6 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); } else { - // drive using manual POV Joystick mode. drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%. @@ -204,9 +238,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode /** * Move robot according to desired axes motions - *

* Positive X is forward - *

* Positive Yaw is counter-clockwise */ public void moveRobot(double x, double yaw) { diff --git a/README.md b/README.md index 1209be9..317b54c 100644 --- a/README.md +++ b/README.md @@ -1,12 +1,12 @@ ## NOTICE -This repository contains the public FTC SDK for the DECODE (2025-2026) competition season. +This repository contains the public FTC SDK for the BIOBUZZ (2026-2027) competition season. ## Welcome! This GitHub repository contains the source code that is used to build an Android app to control a *FIRST* Tech Challenge competition robot. To use this SDK, download/clone the entire project to your local computer. ## Requirements -To use this Android Studio project, you will need Android Studio Ladybug (2024.2) or later. +To use this Android Studio project, you will need Android Studio Narwhal 3 Feature Drop or later. To program your robot in Blocks or OnBot Java, you do not need Android Studio. @@ -59,6 +59,54 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc # Release Information +## Version 12.0 (20260907-090034) + +### Breaking Changes +* The new AprilTag Cluster capability breaks legacy AprilTag OpModes resulting in compile errors for software that uses AprilTagDetection objects in both Android Studio and OnBot Java. + * Legacy AprilTag OpModes must be updated to check whether the returned AprilTag is a cluster or singleton, + and cast the returned detection into the correct type to access its elements. See below: + + **Old method for AprilTag processing** + ``` + for (AprilTagDetection detection : currentDetections) { + // Do single Tag processing here + } + ``` + + **New method for AprilTag Singleton/Cluster processing** + + ``` + for (AprilTagDetection detection : currentDetections) { + if (detection instanceof AprilTagSingleDetection) { + AprilTagSingleDetection singleDet = (AprilTagSingleDetection) detection; + // Do single Tag processing here + } else { + AprilTagClusterDetection clusterDet = (AprilTagClusterDetection) detection; + // Do cluster Tag processing here + } + } + ``` + For more information about how to update your OpModes to fix the breaking change see: https://ftc-docs.firstinspires.org/apriltag-clusters + + * About AprilTag clusters: + * Clusters are co-planar groups of two or more AprilTags wherein the position of each member tag is defined relative to a common origin + * This origin may be placed outside the bounds of the tags themselves to provide a more suitable "aiming" target + * Clusters are resilient to partial occlusion. Full 6DOF pose can be estimated from a cluster even if only a single member tag is visible. Of course, the more tags that are visible, the better and more stable the pose estimate will be + * All AprilTag samples have been updated to differentiate between standalone tags and clusters + +### Enhancements +* Adds a tree view for robot configurations [issue 1821](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/1821) +* Gamepad indicators on the Driver Station are now colored orange if the respective gamepads are connected to the Android generic gamepad driver instead of the Driver Station's usermode USB driver +* Updated AprilTag Library for BIOBUZZ. Notably, getCurrentGameTagLibrary() now returns BIOBUZZ tags. + * In BIOBUZZ, the Origin of each cluster is located in the center of the Cell opening for easy aiming. + * The Origin X,Y & Z Axes are now displayed by default on the preview image. + * Unfortunately, since BIOBUZZ AprilTags move, they are not suitable for absolute Field Localization. +* Supports OctoQuad MK2 firmware v3.1.0, which adds diagnostics parameters for the IMU and MCU uptime + +### Bug Fixes +* Fixes issue [2078](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/2078) where battery +voltage was not updated on driver station if OpMode did not send any telemetry. + ## Version 11.2.1 (20260724-093406) ### Bug Fixes diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 8989a4c..390cc64 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -4,14 +4,14 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:11.2.1' - implementation 'org.firstinspires.ftc:Blocks:11.2.1' - implementation 'org.firstinspires.ftc:RobotCore:11.2.1' - implementation 'org.firstinspires.ftc:RobotServer:11.2.1' - implementation 'org.firstinspires.ftc:OnBotJava:11.2.1' - implementation 'org.firstinspires.ftc:Hardware:11.2.1' - implementation 'org.firstinspires.ftc:FtcCommon:11.2.1' - implementation 'org.firstinspires.ftc:Vision:11.2.1' + implementation 'org.firstinspires.ftc:Inspection:12.0.0' + implementation 'org.firstinspires.ftc:Blocks:12.0.0' + implementation 'org.firstinspires.ftc:RobotCore:12.0.0' + implementation 'org.firstinspires.ftc:RobotServer:12.0.0' + implementation 'org.firstinspires.ftc:OnBotJava:12.0.0' + implementation 'org.firstinspires.ftc:Hardware:12.0.0' + implementation 'org.firstinspires.ftc:FtcCommon:12.0.0' + implementation 'org.firstinspires.ftc:Vision:12.0.0' implementation 'androidx.appcompat:appcompat:1.2.0' }