need to add zero code to properly test

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

@@ -19,6 +19,8 @@ public class LimelightTest extends LinearOpMode {
     Robot robot;
     public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
     public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
+    public static boolean turretMode = false;
+    public static double turretPos = 0.501;
     @Override
     public void runOpMode() throws InterruptedException {
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -55,6 +57,13 @@ public class LimelightTest extends LinearOpMode {
             } else {
                 robot.limelight.pipelineSwitch(0);
             }
+
+            if (turretMode){
+                if (turretPos != 0.501){
+                    turret.manualSetTurret(turretPos);
+                }
+            }
+
         }
     }
 }

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

@@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
 @Autonomous
 @Config
 public class TurretTest extends LinearOpMode {
+    public static boolean zeroTurr = false;
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -25,7 +26,6 @@ public class TurretTest extends LinearOpMode {
         Turret turret = new Turret(robot, TELE, robot.limelight);
         waitForStart();
 
-
         MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
 
         while(opModeIsActive()){
@@ -33,6 +33,16 @@ public class TurretTest extends LinearOpMode {
             drive.updatePoseEstimate();
             turret.trackGoal(drive.localizer.getPose());
 
+            TELE.addData("tpos", turret.getTurrPos());
+            TELE.addData("Limelight tx", turret.getBearing());
+            TELE.addData("Limelight ty", turret.getTy());
+            TELE.addData("Limelight X", turret.getLimelightX());
+            TELE.addData("Limelight Y", turret.getLimelightY());
+
+            if(zeroTurr){
+                turret.zeroTurretEncoder();
+            }
+
             TELE.update();
         }
 

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

@@ -8,6 +8,7 @@ import com.acmerobotics.roadrunner.Pose2d;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
 
@@ -17,38 +18,45 @@ import java.util.List;
 public class Turret {
 
     public static double turretTolerance = 0.02;
-    public static double turrPosScalar = 1.009;
+    public static double turrPosScalar = 0.00011264432;
     public static double turret180Range = 0.4;
     public static double turrDefault = 0.4;
     public static double cameraBearingEqual = 1;
     public static double errorLearningRate = 0.15;
     public static double turrMin = 0.2;
     public static double turrMax = 0.8;
-    public static double deltaAngleThreshold = 0.02;
+    public static double mult = 0.0;
-    public static double angleMultiplier = 0.0;
+    private boolean lockOffset = false;
     Robot robot;
     MultipleTelemetry TELE;
     Limelight3A webcam;
     private int obeliskID = 0;
-    private double turrPos = 0.0;
     private double offset = 0.0;
-    private double bearing = 0.0;
     double tx = 0.0;
     double ty = 0.0;
     double limelightPosX = 0.0;
     double limelightPosY = 0.0;
-
+    public static double clampTolerance = 0.03;
-
 
     public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
         this.TELE = tele;
         this.robot = rob;
         this.webcam = cam;
         webcam.start();
+        if (redAlliance){
+            webcam.pipelineSwitch(3);
+        } else {
+            webcam.pipelineSwitch(2);
+        }
+    }
+
+    public void zeroTurretEncoder() {
+        robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
     }
 
     public double getTurrPos() {
-        return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
+        return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
 
     }
 
@@ -121,11 +129,18 @@ public class Turret {
         return obeliskID;
     }
 
+    public void zeroOffset() {
+        offset = 0.0;
+    }
+
+    public void lockOffset(boolean lock) {
+        lockOffset = lock;
+    }
+
     /*
         Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
      */
     public void trackGoal(Pose2d deltaPos) {
-
         /* ---------------- FIELD → TURRET GEOMETRY ---------------- */
 
         // Angle from robot to goal in robot frame
@@ -145,16 +160,10 @@ public class Turret {
         while (turretAngleDeg > 180) turretAngleDeg -= 360;
         while (turretAngleDeg < -180) turretAngleDeg += 360;
 
-
         /* ---------------- APRILTAG CORRECTION ---------------- */
 //
         double tagBearingDeg = getBearing();  // + = target is to the left
 
-        if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
-            // Slowly learn turret offset (persistent calibration)
-            offset -= tagBearingDeg * errorLearningRate;
-        }
-
         turretAngleDeg += offset;
 
         /* ---------------- ANGLE → SERVO ---------------- */
@@ -162,7 +171,22 @@ public class Turret {
         double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
 
         // Clamp to servo range
-        turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
+        double currentEncoderPos = this.getTurrPos();
+
+        if (!turretEqual(turretPos)) {
+            double diff = turretPos - currentEncoderPos;
+            turretPos = turretPos + diff * mult;
+        }
+
+        if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) {
+            // Clamp to servo range
+            turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
+        } else {
+            if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
+                // Slowly learn turret offset (persistent calibration)
+                offset -= tagBearingDeg * errorLearningRate;
+            }
+        }
 
         robot.turr1.setPosition(turretPos);
         robot.turr2.setPosition(1.0 - turretPos);