upgraded versions for sdk
This commit is contained in:
@@ -2,7 +2,7 @@
|
||||
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
|
||||
xmlns:tools="http://schemas.android.com/tools"
|
||||
android:versionCode="63"
|
||||
android:versionName="11.2.1">
|
||||
android:versionName="12.0">
|
||||
|
||||
<uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />
|
||||
|
||||
|
||||
+20
-7
@@ -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
|
||||
|
||||
|
||||
+21
-29
@@ -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
|
||||
|
||||
|
||||
+12
-10
@@ -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
|
||||
|
||||
|
||||
+18
-5
@@ -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
|
||||
|
||||
|
||||
+72
-41
@@ -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<AprilTagDetection> 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
|
||||
* <p>
|
||||
* Positive X is forward
|
||||
* <p>
|
||||
* Positive Y is strafe left
|
||||
* <p>
|
||||
* Positive Yaw is counter-clockwise
|
||||
*/
|
||||
public void moveRobot(double x, double y, double yaw) {
|
||||
|
||||
+75
-43
@@ -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<AprilTagDetection> 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
|
||||
* <p>
|
||||
* Positive X is forward
|
||||
* <p>
|
||||
* Positive Yaw is counter-clockwise
|
||||
*/
|
||||
public void moveRobot(double x, double yaw) {
|
||||
|
||||
@@ -6,20 +6,17 @@ repositories {
|
||||
}
|
||||
|
||||
dependencies {
|
||||
implementation 'org.firstinspires.ftc:Inspection:11.2.1'
|
||||
implementation 'org.firstinspires.ftc:Blocks:11.2.1'
|
||||
//noinspection Aligned16KB
|
||||
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'
|
||||
//noinspection GradleDependency
|
||||
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'
|
||||
|
||||
implementation 'com.bylazar:fullpanels:1.0.12'
|
||||
implementation 'com.pedropathing:revhub:3.0.0'
|
||||
implementation 'com.pedropathing:tuning:1.0.0'
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user