Added April Tag Class and Working Example

TeamCode/build.gradle

@@ -23,6 +23,17 @@ android {
     }
 }
 
+
+repositories {
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
+}
+
 dependencies {
     implementation project(':FtcRobotController')
+    implementation "com.acmerobotics.roadrunner:ftc:0.1.25"
+    implementation "com.acmerobotics.roadrunner:core:1.0.1"
+    implementation "com.acmerobotics.roadrunner:actions:1.0.1"
+    implementation "com.acmerobotics.dashboard:dashboard:0.5.1"
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md

@@ -13,9 +13,8 @@ The easiest way to create your own OpMode is to copy a Sample OpMode and make it
 Sample opmodes exist in the FtcRobotController module.
 To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
 
-
 Expand the following tree elements:
- FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
+FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
 
 ### Naming of Samples
 
@@ -29,21 +28,21 @@ The class names will follow a naming convention which indicates the purpose of e
 The prefix of the name will be one of the following:
 
 Basic:    This is a minimally functional OpMode used to illustrate the skeleton/structure
-            of a particular style of OpMode.  These are bare bones examples.
+of a particular style of OpMode. These are bare bones examples.
 
 Sensor:        This is a Sample OpMode that shows how to use a specific sensor.
-            It is not intended to drive a functioning robot, it is simply showing the minimal code
+It is not intended to drive a functioning robot, it is simply showing the minimal code
-            required to read and display the sensor values.
+required to read and display the sensor values.
 
 Robot:        This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
-            It may be used to provide a common baseline driving OpMode, or
+It may be used to provide a common baseline driving OpMode, or
-            to demonstrate how a particular sensor or concept can be used to navigate.
+to demonstrate how a particular sensor or concept can be used to navigate.
 
 Concept:    This is a sample OpMode that illustrates performing a specific function or concept.
-            These may be complex, but their operation should be explained clearly in the comments,
+These may be complex, but their operation should be explained clearly in the comments,
-            or the comments should reference an external doc, guide or tutorial.
+or the comments should reference an external doc, guide or tutorial.
-            Each OpMode should try to only demonstrate a single concept so they are easy to
+Each OpMode should try to only demonstrate a single concept so they are easy to
-            locate based on their name.  These OpModes may not produce a drivable robot.
+locate based on their name. These OpModes may not produce a drivable robot.
 
 After the prefix, other conventions will apply:
 
@@ -57,15 +56,15 @@ your TeamCode module to be used.
 
 This is done inside Android Studio directly, using the following steps:
 
- 1) Locate the desired sample class in the Project/Android tree.
+1) Locate the desired sample class in the Project/Android tree.
 
- 2) Right click on the sample class and select "Copy"
+2) Right click on the sample class and select "Copy"
 
- 3) Expand the  TeamCode/java folder
+3) Expand the TeamCode/java folder
 
- 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
+4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
 
- 5) You will be prompted for a class name for the copy.
+5) You will be prompted for a class name for the copy.
    Choose something meaningful based on the purpose of this class.
    Start with a capital letter, and remember that there may be more similar classes later.
 
@@ -81,16 +80,14 @@ Each OpMode sample class begins with several lines of code like the ones shown b
 ```
 
 The name that will appear on the driver station's "opmode list" is defined by the code:
- ``name="Template: Linear OpMode"``
+``name="Template: Linear OpMode"``
 You can change what appears between the quotes to better describe your opmode.
 The "group=" portion of the code can be used to help organize your list of OpModes.
 
 As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
-  ``@Disabled`` annotation which has been included.
+``@Disabled`` annotation which has been included.
 This line can simply be deleted , or commented out, to make the OpMode visible.
 
-
-
 ## ADVANCED Multi-Team App management:  Cloning the TeamCode Module
 
 In some situations, you have multiple teams in your club and you want them to all share
@@ -122,7 +119,8 @@ Note: Some names start with "Team" and others start with "team".  This is intent
 3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
    to a matching name with a lowercase 'team' eg:  "team0417".
 
-4)  In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
+4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that
+   contains
    package="org.firstinspires.ftc.teamcode"
    to be
    package="org.firstinspires.ftc.team0417"

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java

@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import android.util.Size;
+
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class AprilTagWebcam {
+
+    private AprilTagProcessor aprilTagProcessor;
+    private VisionPortal visionPortal;
+    private List<AprilTagDetection> detectedTags = new ArrayList<>();
+    private MultipleTelemetry telemetry;
+    public void init(HardwareMap hwMap, MultipleTelemetry TELE){
+
+        this.telemetry = TELE;
+
+        aprilTagProcessor = new AprilTagProcessor.Builder()
+                .setDrawTagID(true)
+                .setDrawTagOutline(true)
+                .setDrawAxes(true)
+                .setDrawCubeProjection(true)
+                .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+                .build();
+
+        VisionPortal.Builder builder = new VisionPortal.Builder();
+        builder.setCamera(hwMap.get(WebcamName.class, "Webcam 1"));
+        builder.setCameraResolution(new Size(640, 480));
+        builder.addProcessor(aprilTagProcessor);
+
+        visionPortal = builder.build();
+
+    }
+
+    public void update() {
+        detectedTags = aprilTagProcessor.getDetections();
+    }
+
+    public List<AprilTagDetection> getDetectedTags() {
+        return detectedTags;
+    }
+
+    public AprilTagDetection getTagById(int id){
+        for (AprilTagDetection detection : detectedTags) {
+            if (detection.id ==id){
+                return detection;
+            }
+        }
+        return null;
+    }
+
+    public void stop(){
+        if (visionPortal != null){
+            visionPortal.close();
+        }
+    }
+
+    //Helper Functions
+
+    public void displayDetectionTelemetry (AprilTagDetection detectedId){
+        if (detectedId ==null){return;}
+
+        if (detectedId.metadata != null) {
+            telemetry.addLine(String.format("\n==== (ID %d) %s", detectedId.id, detectedId.metadata.name));
+            telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f  (inch)", detectedId.ftcPose.x, detectedId.ftcPose.y, detectedId.ftcPose.z));
+            telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f  (deg)", detectedId.ftcPose.pitch, detectedId.ftcPose.roll, detectedId.ftcPose.yaw));
+            telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f  (inch, deg, deg)", detectedId.ftcPose.range, detectedId.ftcPose.bearing, detectedId.ftcPose.elevation));
+        } else {
+            telemetry.addLine(String.format("\n==== (ID %d) Unknown", detectedId.id));
+            telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detectedId.center.x, detectedId.center.y));
+        }
+    }
+
+    public void displayAllTelemetry (){
+        if (detectedTags ==null){return;}
+
+        telemetry.addData("# AprilTags Detected", detectedTags.size());
+
+
+        for (AprilTagDetection detection : detectedTags) {
+            if (detection.metadata != null) {
+                telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.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", detection.id));
+                telemetry.addLine(String.format("Center %6.0f %6.0f   (pixels)", detection.center.x, detection.center.y));
+            }
+        }   // end for() loop
+    }
+
+
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java

@@ -0,0 +1,37 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam;
+
+
+@Config
+@TeleOp
+public class AprilTagWebcamExample extends OpMode {
+
+    MultipleTelemetry TELE;
+    AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
+
+    @Override
+    public void init() {
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        aprilTagWebcam.init(hardwareMap, TELE);
+
+    }
+
+    @Override
+    public void loop() {
+
+        aprilTagWebcam.update();
+        aprilTagWebcam.displayAllTelemetry();
+        TELE.update();
+
+    }
+}