From 7eebd42ea274f075233a7e21c8821af8d219df28 Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Sat, 18 Apr 2026 21:34:08 -0500
Subject: [PATCH] limelight relocalization in progress

---
 .../ftc/teamcode/teleop/TeleopV3.java         | 19 +++++++------
 .../ftc/teamcode/utils/Turret.java            | 28 ++++++-------------
 2 files changed, 20 insertions(+), 27 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
index 937758b..c7b175e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
@@ -41,6 +41,7 @@ import java.util.List;
 @Config
 @TeleOp
 public class TeleopV3 extends LinearOpMode {
+    private double metersToInches = 39.3700787402;
     public static double manualVel = 3000;
     public static double hoodDefaultPos = 0.5;
     private double predictedResetX, predictedResetY, predictedResetH;
@@ -275,15 +276,17 @@ public class TeleopV3 extends LinearOpMode {
                 gamepad2.rumble(500);
             }
 
-            //TODO: relocalize using limelight
-//            if (relocalize){
-//                turret.relocalize();
-//                xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
-//                yOffset = (turret.getLimelightX() * 39.3701) - robY;
-//                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
-//            } else {
+            if (relocalize){
+                turret.relocalize();
+                xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX;
+                yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY;
+                hOffset = (Math.toRadians(turret.getLimelightH() + 90));
+                while (hOffset > 180) {hOffset-=360;}
+                while (hOffset < -180) {hOffset+=360;}
+                hOffset = hOffset - robH;
+            } else {
                 turret.trackGoal(deltaPose);
-            //}
+            }
 
             //VELOCITY AUTOMATIC
             if (autoVel) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
index 48969da..e8837b9 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
@@ -109,7 +109,7 @@ public class Turret {
     private void limelightRead() { // only for tracking purposes, not general reads
         Double xPos = null;
         Double yPos = null;
-        Double zPos = null;
+        double zPos;
         Double hPos = null;
         result = webcam.getLatestResult();
         if (result != null) {
@@ -117,27 +117,18 @@ public class Turret {
                 tx = result.getTx();
                 ty = result.getTy();
                 if (TeleopV3.relocalize){
-                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                        limelightTagPose = fiducial.getRobotPoseTargetSpace();
-                        if (limelightTagPose != null){
-                            xPos = limelightTagPose.getPosition().x;
-                            yPos = limelightTagPose.getPosition().y;
-                            zPos = limelightTagPose.getPosition().z;
-                            hPos = limelightTagPose.getOrientation().getYaw();
-                        }
+                    zPos = result.getBotpose().getPosition().z;
+                    if (zPos < 0.15){
+                        xPos = result.getBotpose().getPosition().x;
+                        yPos = result.getBotpose().getPosition().y;
+                        hPos = result.getBotpose().getOrientation().getYaw();
+                        limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
+                        limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
+                        limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
                     }
                 }
             }
         }
-        if (xPos != null){
-            if (zPos<0) {
-                limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
-                limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
-                limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
-                limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
-            }
-        }
     }
 
     public double getBearing() {
@@ -150,7 +141,6 @@ public class Turret {
         return ty;
     }
 
-    Pose3D limelightTagPose;
     double limelightTagX = 0.0;
     double limelightTagY = 0.0;
     double limelightTagZ = 0.0;