From e06508496405610d602911533551362772da009c Mon Sep 17 00:00:00 2001
From: DanTheMan-byte <danibarrf@gmail.com>
Date: Fri, 5 Jun 2026 18:51:54 -0500
Subject: [PATCH] tele pose transfer

---
 .../teamcode/autonomous/Auto12Ball_Back_Sorted.java   | 11 +++++------
 .../ftc/teamcode/constants/TeleStart.java             |  7 +++++++
 .../firstinspires/ftc/teamcode/teleop/TeleopV4.java   | 10 ++++++++--
 3 files changed, 20 insertions(+), 8 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
index 77d4d41..65bf966 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java
@@ -19,6 +19,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+import org.firstinspires.ftc.teamcode.constants.TeleStart;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
@@ -388,9 +389,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
             follower.update();
             pathStateMachine();
             Pose currentPose = follower.getPose();
-            teleStartPoseX = currentPose.getX();
-            teleStartPoseY = currentPose.getY();
-            teleStartPoseH = Math.toDegrees(currentPose.getHeading());
+            TeleStart.teleStart = currentPose;
 
             spindexer.update();
 
@@ -402,9 +401,9 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
             TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
             TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
             TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
-            TELE.addData("X:", currentPose.getX());
-            TELE.addData("Y:", currentPose.getY());
-            TELE.addData("H:", currentPose.getHeading());
+            TELE.addData("X:", teleStartPoseX);
+            TELE.addData("Y:", teleStartPoseY);
+            TELE.addData("H:", teleStartPoseH);
             TELE.update();
         }
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java
new file mode 100644
index 0000000..c6d9801
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/TeleStart.java
@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+import com.pedropathing.geometry.Pose;
+
+public class TeleStart {
+    public static Pose teleStart = new Pose(72,72,0);
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
index 80459f3..b19990e 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java
@@ -13,6 +13,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.constants.Color;
+import org.firstinspires.ftc.teamcode.constants.TeleStart;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
 import org.firstinspires.ftc.teamcode.utils.MeasuringLoopTimes;
 import org.firstinspires.ftc.teamcode.utilsv2.*;
@@ -50,8 +51,10 @@ public class TeleopV4 extends LinearOpMode {
         commander = new VelocityCommander();
         drivetrain = new Drivetrain(robot, TELE);
         follower = Constants.createFollower(hardwareMap);
-        Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH));
-        follower.setStartingPose(start);
+        follower.setStartingPose(TeleStart.teleStart);
+        sleep(500);
+        follower.setPose(TeleStart.teleStart);
+        follower.update();
 
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
@@ -73,6 +76,9 @@ public class TeleopV4 extends LinearOpMode {
 
         limelightUsed = true;
 
+        TELE.addLine("Initialization is done");
+        TELE.update();
+
         waitForStart();
 
         if (isStopRequested()) return;