limelight works

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

@@ -3,66 +3,75 @@ 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.Autonomous;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.variables.HardwareConfig;
+import java.util.List;
-import org.firstinspires.ftc.teamcode.utils.Robot;
-import org.firstinspires.ftc.teamcode.utils.subsystems.Limelight;
 
-@Autonomous
+//TODO: fix to get the apriltag that it is reading
 @Config
-
+@TeleOp
 public class LimelightTest extends LinearOpMode {
-
-    public static String MODE = "AT";
-    public static int pipe = 1;
-
-
     MultipleTelemetry TELE;
-
+    public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
-
+    public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
-
-
-
-
-    Robot robot;
-
     @Override
     public void runOpMode() throws InterruptedException {
-
+        Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
-
-
-        HardwareConfig.USING_LL= true;
-
-
-        robot = new Robot(hardwareMap);
-
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
+        limelight.pipelineSwitch(pipeline);
-        Limelight limelight = new Limelight(robot, TELE);
-
-        limelight.setMode(MODE);
-
-        limelight.setTelemetryOn(true);
-
         waitForStart();
-
+        if (isStopRequested()) return;
-        if(isStopRequested()) return;
+        limelight.start();
-
+        while (opModeIsActive()){
-        while(opModeIsActive()){
+            if (mode == 0){
-
+                limelight.pipelineSwitch(pipeline);
-            limelight.setPipeline(pipe);
+                LLResult result = limelight.getLatestResult();
-
+                if (result != null) {
-            limelight.setMode(MODE);
+                    if (result.isValid()) {
-
+                        TELE.addData("tx", result.getTx());
-            limelight.update();
+                        TELE.addData("ty", result.getTy());
-
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 1){
+                limelight.pipelineSwitch(1);
+                LLResult result = limelight.getLatestResult();
+                if (result != null && result.isValid()) {
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        int id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
                         TELE.update();
-
-
-
                     }
 
                 }
+            } else if (mode == 2){
+                limelight.pipelineSwitch(2);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else if (mode == 3){
+                limelight.pipelineSwitch(3);
+                LLResult result = limelight.getLatestResult();
+                if (result != null) {
+                    if (result.isValid()) {
+                        TELE.addData("tx", result.getTx());
+                        TELE.addData("ty", result.getTy());
+                        TELE.update();
+                    }
+                }
+            } else {
+                limelight.pipelineSwitch(0);
+            }
+        }
+    }
 }