From 0264cf2c776661091f3420374adc048259d5fdff Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Sun, 22 Feb 2026 17:55:56 -0600
Subject: [PATCH] heading relocalization done, need to test for flipping and
 consistency

---
 .../ftc/teamcode/teleop/TeleopV3.java           | 17 +++++++++++------
 .../ftc/teamcode/utils/Turret.java              |  5 +++++
 2 files changed, 16 insertions(+), 6 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 04fa836..46b7949 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
@@ -65,6 +65,7 @@ public class TeleopV3 extends LinearOpMode {
     boolean reject = false;
     double xOffset = 0.0;
     double yOffset = 0.0;
+    double hOffset = 0.0;
 //    double headingOffset = 0.0;
     int ticker = 0;
 
@@ -166,10 +167,11 @@ public class TeleopV3 extends LinearOpMode {
 
             double robX = drive.localizer.getPose().position.x;
             double robY = drive.localizer.getPose().position.y;
+            double robH = drive.localizer.getPose().heading.toDouble();
 
             double robotX = robX + xOffset;
             double robotY = robY + yOffset;
-            double robotHeading = drive.localizer.getPose().heading.toDouble();
+            double robotHeading = robH + hOffset;
 
             double goalX = -15;
             double goalY = 0;
@@ -194,6 +196,7 @@ public class TeleopV3 extends LinearOpMode {
                 turret.relocalize();
                 xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
                 yOffset = (turret.getLimelightX() * 39.3701) - robY;
+                hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
             } else {
                 turret.trackGoal(deltaPose);
             }
@@ -353,13 +356,14 @@ public class TeleopV3 extends LinearOpMode {
             // Targeting Debug
             TELE.addData("robotX", robotX);
             TELE.addData("robotY", robotY);
-            TELE.addData("robotInchesX", targeting.robotInchesX);
-            TELE.addData("robotInchesY", targeting.robotInchesY);
-            TELE.addData("Targeting Interpolate", turretInterpolate);
+            TELE.addData("robot H", robotHeading);
+//            TELE.addData("robotInchesX", targeting.robotInchesX);
+//            TELE.addData("robotInchesY", targeting.robotInchesY);
+//            TELE.addData("Targeting Interpolate", turretInterpolate);
             TELE.addData("Targeting GridX", targeting.robotGridX);
             TELE.addData("Targeting GridY", targeting.robotGridY);
-            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
-            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
+//            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
+//            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
 //            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
 //            TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
             TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
@@ -367,6 +371,7 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
             TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
             TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
+            TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
 
             TELE.update();
 
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 b247708..489e5ad 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
@@ -104,6 +104,7 @@ public class Turret {
         Double xPos = null;
         Double yPos = null;
         Double zPos = null;
+        Double hPos = null;
         result = webcam.getLatestResult();
         if (result != null) {
             if (result.isValid()) {
@@ -122,6 +123,7 @@ public class Turret {
                         xPos = limelightTagPose.getPosition().x;
                         yPos = limelightTagPose.getPosition().y;
                         zPos = limelightTagPose.getPosition().z;
+                        hPos = limelightTagPose.getOrientation().getYaw();
                     }
                 }
 
@@ -131,6 +133,7 @@ public class Turret {
             limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
             limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
             limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
+            limelightTagH = (alphaPosConstant*hPos) + ((1-alphaPosConstant)*limelightTagH);
         }
     }
 
@@ -148,11 +151,13 @@ public class Turret {
     double limelightTagX = 0.0;
     double limelightTagY = 0.0;
     double limelightTagZ = 0.0;
+    double limelightTagH = 0.0;
     public double getLimelightX() {
         return limelightTagX;
     }
     public double getLimelightY() {return limelightTagY;}
     public double getLimelightZ(){return limelightTagZ;}
+    public double getLimelightH(){return limelightTagH;}
 
     public void relocalize(){
         setTurret(turrDefault);