fixed ll class

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

@@ -61,6 +61,7 @@ public class LimelightTest extends LinearOpMode {
             TELE.update();
 
 
+
         }
 
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -3,6 +3,7 @@ package org.firstinspires.ftc.teamcode.utils;
 import static org.firstinspires.ftc.teamcode.variables.HardwareConfig.*;
 
 import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
 
@@ -12,6 +13,8 @@ public class Robot {
 
     public Limelight3A limelight3A;
 
+    public IMU imu;
+
     public Robot (HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -25,5 +28,20 @@ public class Robot {
 
 
 
+
+        imu = hardwareMap.get(IMU.class, "imu");
+
+        RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+
+        RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+        RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoFacingDirection, usbFacingDirection);
+
+        imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+
+
+
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/functions/DistanceToTarget.java

@@ -1,8 +0,0 @@
-package org.firstinspires.ftc.teamcode.utils.functions;
-
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-public class DistanceToTarget {
-
-
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/subsystems/Limelight.java

@@ -6,6 +6,7 @@ import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.LLStatus;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.hardware.IMU;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
@@ -43,12 +44,16 @@ public class Limelight implements Subsystem {
     private double ty = 0.0;
     private double tync = 0.0;
 
+    private double ta = 0.0;
+
     private List<LLResultTypes.BarcodeResult> barcodeResults = new ArrayList<>();
     private List<LLResultTypes.ClassifierResult> classifierResults = new ArrayList<>();
     private List<LLResultTypes.DetectorResult> detectorResults = new ArrayList<>();
     private List<LLResultTypes.FiducialResult> fiducialResults = new ArrayList<>();
     private List<LLResultTypes.ColorResult> colorResults = new ArrayList<>();
 
+    private IMU imu;
+
     public Limelight(Robot robot, MultipleTelemetry tele) {
 
         HardwareConfig.USING_LL= true;
@@ -57,6 +62,10 @@ public class Limelight implements Subsystem {
         this.telemetry = tele;
         limelight.pipelineSwitch(1);
 
+        this.imu = robot.imu;
+
+        this.imu.resetYaw();
+
 
     }
 
@@ -66,15 +75,19 @@ public class Limelight implements Subsystem {
 
     public void setMode(String newMode) { this.mode = newMode; }
 
+
+
     /** ✅ MAIN UPDATE LOOP */
     @Override
     public void update() {
 
-        limelight.updateRobotOrientation();
+
 
         result = limelight.getLatestResult();
         status = limelight.getStatus();
 
+
+
         if (result != null && (Objects.equals(status.getPipelineType(), "pipe_python") || result.isValid())){
             // Refresh all cached values
             botpose         = result.getBotpose();
@@ -86,6 +99,7 @@ public class Limelight implements Subsystem {
             txnc            = result.getTxNC();
             ty              = result.getTy();
             tync            = result.getTyNC();
+            ta              = result.getTa();
             barcodeResults  = result.getBarcodeResults();
             classifierResults = result.getClassifierResults();
             detectorResults   = result.getDetectorResults();
@@ -112,6 +126,7 @@ public class Limelight implements Subsystem {
 
 
 
+
         if (result != null && result.isValid()) {
             telemetry.addData("LL Latency", getTotalLatency());
             telemetry.addData("Capture Latency", getCaptureLatency());
@@ -122,7 +137,17 @@ public class Limelight implements Subsystem {
             telemetry.addData("txnc", getTxNC());
             telemetry.addData("ty", getTy());
             telemetry.addData("tync", getTyNC());
-            telemetry.addData("Botpose", getBotPose().toString());
+            telemetry.addData("ta", getTa());
+
+
+
+
+            telemetry.addData("BotX", getBotX());
+
+            telemetry.addData("BotY", getBotY());
+
+
+
 
 
             if (Objects.equals(mode, "BR"))
@@ -177,6 +202,15 @@ public class Limelight implements Subsystem {
     public double getTy() { return ty; }
     public double getTyNC() { return tync;}
 
+    public double getTa() {return ta;}
+
+    public double getBotX() {return getBotPose().getPosition().x;}
+
+    public double getBotY() {return getBotPose().getPosition().y;}
+
+
+
+
     public List<LLResultTypes.BarcodeResult> getBarcodeResults() { return barcodeResults; }
     public List<LLResultTypes.ClassifierResult> getClassifierResults() { return classifierResults; }
     public List<LLResultTypes.DetectorResult> getDetectorResults() { return detectorResults; }
@@ -185,4 +219,6 @@ public class Limelight implements Subsystem {
 
     public LLStatus getStatus() { return status; }
     public LLResult getRawResult() { return result; }
+
+
 }