From 5fe83594885c93efed27ba045c87c2592936480e Mon Sep 17 00:00:00 2001
From: Yash Karandikar <yash@karx.xyz>
Date: Wed, 16 Nov 2022 18:23:46 -0600
Subject: [PATCH] o, pali e tawa nilo lon poka nanpa tu

---
 .../enhancedautos/AutoCameraLeft.java         | 93 +++++++++++++++++++
 1 file changed, 93 insertions(+)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoCameraLeft.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoCameraLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoCameraLeft.java
new file mode 100644
index 0000000..f11a658
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoCameraLeft.java
@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode.createdcode.enhancedautos;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.teamcode.createdcode.enhancedautos.vendor.AprilTagDetectionPipeline;
+import org.openftc.apriltag.AprilTagDetection;
+import org.openftc.easyopencv.OpenCvCamera;
+import org.openftc.easyopencv.OpenCvCameraFactory;
+import org.openftc.easyopencv.OpenCvCameraRotation;
+
+import java.util.ArrayList;
+
+@Autonomous
+public class AutoCameraLeft extends LinearOpMode {
+    OpenCvCamera camera;
+    AprilTagDetectionPipeline aprilTagDetectionPipeline;
+
+    int detected_id = 0;
+
+    @Override
+    public void runOpMode() {
+        int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
+                "cameraMonitorViewId",
+                "id",
+                hardwareMap.appContext.getPackageName());
+        camera = OpenCvCameraFactory.getInstance().createWebcam(
+                hardwareMap.get(WebcamName.class, "Webcam 1"));
+        aprilTagDetectionPipeline = new AprilTagDetectionPipeline(
+                CameraConstants.TAGSIZE,
+                CameraConstants.FX,
+                CameraConstants.FY,
+                CameraConstants.CX,
+                CameraConstants.CY);
+        API api = new API(this);
+        MovementAPI movementAPI = new MovementAPI(api);
+
+        camera.setPipeline(aprilTagDetectionPipeline);
+        camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
+            @Override
+            public void onOpened() {
+                camera.startStreaming(800, 448, OpenCvCameraRotation.UPRIGHT);
+            }
+
+            @Override
+            public void onError(int errorCode) {
+
+            }
+        });
+
+        waitForStart();
+
+        // let the camera settle
+        api.pause(5);
+
+        ArrayList<AprilTagDetection> currentDetections = aprilTagDetectionPipeline.getLatestDetections();
+        if (currentDetections.size() != 0) {
+            detected_id = currentDetections.get(0).id;
+            telemetry.addData("Tag found", String.valueOf(detected_id));
+            telemetry.update();
+        }
+
+        movementAPI.move(0, 0.5);
+        api.pause(0.1);
+        movementAPI.stop();
+
+        switch (detected_id) {
+            case 1:
+                movementAPI.move(-90, 0.7);
+                api.pause(1);
+                movementAPI.move(0, 0.5);
+                api.pause(1.4);
+                break;
+            case 2:
+                movementAPI.move(0, 0.5);
+                api.pause(1.4);
+                break;
+            case 3:
+                movementAPI.move(90, 0.7);
+                api.pause(0.85);
+                movementAPI.move(0, 0.5);
+                api.pause(1.4);
+                break;
+            default:
+                movementAPI.move(-90, 0.5);
+                api.pause(1.75);
+                break;
+        }
+
+        movementAPI.stop();
+    }
+}