From 7a2b275e66cfe154d3af77ab5d9c1b79a26cd169 Mon Sep 17 00:00:00 2001
From: KeshavAnandCode <keshavanandofficial@gmail.com>
Date: Mon, 23 Feb 2026 19:42:34 -0600
Subject: [PATCH] stash for dany

---
 .../teamcode/autonomous/Auto_LT_Close.java    |  8 ++---
 .../ftc/teamcode/libs/RR/MecanumDrive.java    | 30 +++++++++++--------
 .../ftc/teamcode/utils/Turret.java            | 13 ++++----
 3 files changed, 29 insertions(+), 22 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
index 3201ad3..1b05bce 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
@@ -68,9 +68,9 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double intake2TimeGate = 3;
     public static double shoot2GateTime = 1.7;
     public static double endGateTime = 22;
-    public static double waitToPickupGateWithPartner = 1;
-    public static double waitToPickupGateSolo = 0.2;
-    public static double intakeGateTime = 3;
+    public static double waitToPickupGateWithPartner = 0.7;
+    public static double waitToPickupGateSolo = 0.01;
+    public static double intakeGateTime = 8;
     public static double shootGateTime = 1.7;
     public static double shoot1GateTime = 1.7;
     public static double intake1GateTime = 3.3;
@@ -111,7 +111,7 @@ public class Auto_LT_Close extends LinearOpMode {
     int ballCycles = 3;
     int prevMotif = 0;
     boolean gateCycle = true;
-    boolean withPartner = false;
+    boolean withPartner = true;
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
index 8ddb936..9e5e180 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/libs/RR/MecanumDrive.java
@@ -201,18 +201,18 @@ public final class MecanumDrive {
         public double kA = 0.00008;
 
         // path profile parameters (in inches)
-        public double maxWheelVel = 90;
+        public double maxWheelVel = 70;
         public double minProfileAccel = -40;
-        public double maxProfileAccel = 90;
+        public double maxProfileAccel = 70;
 
         // turn profile parameters (in radians)
-        public double maxAngVel = Math.PI; // shared with path
-        public double maxAngAccel = Math.PI;
+        public double maxAngVel = 2 *Math.PI; // shared with path
+        public double maxAngAccel = 2 * Math.PI;
 
         // path controller gains
-        public double axialGain = 4;
-        public double lateralGain = 4;
-        public double headingGain = 4; // shared with turn
+        public double axialGain = 6;
+        public double lateralGain = 6;
+        public double headingGain = 6; // shared with turn
 
         public double axialVelGain = 0.0;
         public double lateralVelGain = 0.0;
@@ -345,7 +345,16 @@ public final class MecanumDrive {
                 t = Actions.now() - beginTs;
             }
 
-            if (t >= timeTrajectory.duration) {
+            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
+            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
+
+            PoseVelocity2d robotVelRobot = updatePoseEstimate();
+
+            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
+
+            if ((t >= timeTrajectory.duration && error.position.norm() < 2
+                    && robotVelRobot.linearVel.norm() < 0.5)
+                    || t >= timeTrajectory.duration + 0.5) {
                 leftFront.setPower(0);
                 leftBack.setPower(0);
                 rightBack.setPower(0);
@@ -354,10 +363,6 @@ public final class MecanumDrive {
                 return false;
             }
 
-            Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
-            targetPoseWriter.write(new PoseMessage(txWorldTarget.value()));
-
-            PoseVelocity2d robotVelRobot = updatePoseEstimate();
 
             PoseVelocity2dDual<Time> command = new HolonomicController(
                     PARAMS.axialGain, PARAMS.lateralGain, PARAMS.headingGain,
@@ -388,7 +393,6 @@ public final class MecanumDrive {
             p.put("y", localizer.getPose().position.y);
             p.put("heading (deg)", Math.toDegrees(localizer.getPose().heading.toDouble()));
 
-            Pose2d error = txWorldTarget.value().minusExp(localizer.getPose());
             p.put("xError", error.position.x);
             p.put("yError", error.position.y);
             p.put("headingError (deg)", Math.toDegrees(error.heading.toDouble()));
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 489e5ad..6907345 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
@@ -129,11 +129,14 @@ public class Turret {
 
             }
         }
-        if (xPos != null){
-            limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
-            limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
-            limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
-            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);
+            }
         }
     }