Update TelopV3 and Targeting for merge conflicts.

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -12,7 +12,7 @@ public class Poses {
 
     public static double relativeGoalHeight = goalHeight - turretHeight;
 
-    public static Pose2d goalPose = new Pose2d(-15, 0, 0);
+    public static Pose2d goalPose = new Pose2d(-10, 0, 0);
 
     public static double rx1 = 40, ry1 = -7, rh1 = 0;
     public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
@@ -38,6 +38,6 @@ public class Poses {
     public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
 
-    public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1);
+    public static Pose2d teleStart = new Pose2d(0, 0, 0);
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -1,11 +1,9 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
-import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
 import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
@@ -21,7 +19,6 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
 import com.arcrobotics.ftclib.controller.PIDFController;
-import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -29,11 +26,13 @@ import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
 import org.firstinspires.ftc.teamcode.utils.Spindexer;
 import org.firstinspires.ftc.teamcode.utils.Targeting;
+import org.firstinspires.ftc.teamcode.utils.Turret;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -50,8 +49,6 @@ public class TeleopV3 extends LinearOpMode {
     public static double spindexPos = spindexer_intakePos1;
     public static double spinPow = 0.09;
     public static double bMult = 1, bDiv = 2200;
-    public static double limelightKp = 0.001; // Proportional gain for limelight auto-aim
-    public static double limelightDeadband = 0.5; // Ignore tx values smaller than this
     public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
     public static boolean manualTurret = true;
     public double vel = 3000;
@@ -123,7 +120,7 @@ public class TeleopV3 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
-    boolean turretInterpolate = true;
+    boolean turretInterpolate = false;
 
     public static double velPrediction(double distance) {
         if (distance < 30) {
@@ -169,6 +166,12 @@ public class TeleopV3 extends LinearOpMode {
 
 //        robot.limelight.start();
 
+        AprilTagWebcam webcam = new AprilTagWebcam();
+        webcam.init(robot, TELE);
+
+        Turret turret = new Turret(robot, TELE, webcam);
+        waitForStart();
+
         waitForStart();
         if (isStopRequested()) return;
 
@@ -382,40 +385,22 @@ public class TeleopV3 extends LinearOpMode {
             double robotY = robY - yOffset;
             double robotHeading = drive.localizer.getPose().heading.toDouble();
 
-            double goalX = -10;
+            double goalX = -15;
             double goalY = 0;
 
-            double dx = goalX - robotX;  // delta x from robot to goal
+            double dx = robotX - goalX;  // delta x from robot to goal
-            double dy = goalY - robotY;  // delta y from robot to goal
+            double dy = robotY - goalY;  // delta y from robot to goal
+            Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
 
             double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
 
-            desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
-
-            desiredTurretAngle += manualOffset + error;
-
-            offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
-
-            if (offset > 135) {
-                offset -= 360;
-            }
-
-            double pos = turrDefault;
-
-            TELE.addData("offset", offset);
-
-            pos -= offset * ((double) 1 / 360);
-
-            if (pos < 0.13) {
-                pos = 0.13;
-            } else if (pos > 0.83) {
-                pos = 0.83;
-            }
-
-
             targetingSettings = targeting.calculateSettings
                     (robotX,robotY,robotHeading,0.0, turretInterpolate);
 
+            turret.trackGoal(deltaPose);
+
+            webcam.update();
+
             //VELOCITY AUTOMATIC
             if (targetingVel) {
                 vel = targetingSettings.flywheelRPM;
@@ -446,105 +431,28 @@ public class TeleopV3 extends LinearOpMode {
 
             //TODO: test the camera teleop code
 
-            // TODO: TEST THIS CODE
+//            if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
-
+//                double bearing;
-            TELE.addData("posS2", pos);
+//
-
+//                LLResult result = robot.light.getLatestResult();
-            LLResult result = robot.limelight.getLatestResult();
+//                if (result != null) {
-            boolean limelightActive = false;
+//                    if (result.isValid()) {
-            
+//                        bearing = result.getTx() * bMult;
-            double turretMin = 0.13;
+//
-            double turretMax = 0.83;
+//                        double bearingCorrection = bearing / bDiv;
-            
+//
-            if (result != null && result.isValid()) {
+//                        error += bearingCorrection;
-                double tx = result.getTx();
+//
-                double ty = result.getTy();
+//                        camTicker++;
-                
+//                        TELE.addData("tx", bearingCorrection);
-                if (Math.abs(tx) > limelightDeadband) {
+//                        TELE.addData("ty", result.getTy());
-                    limelightActive = true;
+//                    }
-                    overrideTurr = true;
+//                }
-                    
+//
-                    double currentTurretPos = servo.getTurrPos();
+//            } else {
-                    
+//                camTicker = 0;
-                    // + tx means tag is right, so rotate right
+//                overrideTurr = false;
-                    double adjustment = -tx * limelightKp;
+//            }
-                    
-                    // calculate new position
-                    double newTurretPos = currentTurretPos + adjustment;
-                    
-                    if (newTurretPos < turretMin) {
-                        double forwardDist = turretMin - newTurretPos;
-                        double backwardDist = (currentTurretPos - turretMin) + (turretMax - newTurretPos);
-                        // check path distance
-                        if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
-                            newTurretPos = turretMax - (turretMin - newTurretPos);
-                        } else {
-                            newTurretPos = turretMin;
-                        }
-                    } else if (newTurretPos > turretMax) {
-                        double forwardDist = newTurretPos - turretMax;
-                        double backwardDist = (turretMax - currentTurretPos) + (newTurretPos - turretMin);
-                        if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
-                            newTurretPos = turretMin + (newTurretPos - turretMax);
-                        } else {
-                            newTurretPos = turretMax;
-                        }
-                    }
-                    
-                    // Final clamp 
-                    if (newTurretPos < turretMin) {
-                        newTurretPos = turretMin;
-                    } else if (newTurretPos > turretMax) {
-                        newTurretPos = turretMax;
-                    }
-                    
-                    pos = newTurretPos;
-                    turretPos = pos;
-                    
-                    camTicker++;
-                    TELE.addData("tx", tx);
-                    TELE.addData("ty", ty);
-                    TELE.addData("limelightAdjustment", adjustment);
-                    TELE.addData("limelightActive", true);
-                } else {
-                    limelightActive = true;
-                    overrideTurr = true;
-                    TELE.addData("tx", tx);
-                    TELE.addData("ty", ty);
-                    TELE.addData("limelightActive", true);
-                    TELE.addData("limelightStatus", "Centered");
-                }
-            } else {
-                if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) {
-                    TELE.addData("limelightActive", false);
-                    TELE.addData("limelightStatus", "No target");
-                } else {
-                    camTicker = 0;
-                    overrideTurr = false;
-                    limelightActive = false;
-                }
-            }
-
-            if (!limelightActive && !overrideTurr) {
-                turretPos = pos;
-            }
-
-            TELE.addData("posS3", turretPos);
-
-            if (manualTurret && !limelightActive) {
-                pos = turrDefault + (manualOffset / 100) + error;
-            }
-
-            if (!overrideTurr && !limelightActive) {
-                turretPos = pos;
-            }
-
-            if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) {
-                manualOffset += 1.35 * gamepad2.left_stick_x;
-            }
-
-            robot.turr1.setPosition(pos);
-            robot.turr2.setPosition(1 - pos);
 
             //HOOD:
 

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

@@ -1,12 +1,9 @@
 package org.firstinspires.ftc.teamcode.tests;
 
-import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
@@ -33,33 +30,19 @@ public class TurretTest extends LinearOpMode {
         Turret turret = new Turret(robot, TELE, webcam);
         waitForStart();
 
-        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(10, 0,0));
+
+        MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
 
         while(opModeIsActive()){
 
             drive.updatePoseEstimate();
-            Pose2d robotPose = drive.localizer.getPose();
+            turret.trackGoal(drive.localizer.getPose());
 
-            double dx = goalPose.position.x - robotPose.position.x;
+            webcam.update();
-            double dy = goalPose.position.y - robotPose.position.y;
+            webcam.displayAllTelemetry();
-
-            double heading = robotPose.heading.toDouble();
-
-            // field vector -> robot frame... avoids double calculation
-            double relX =  dx * Math.cos(-heading) - dy * Math.sin(-heading);
-            double relY =  dx * Math.sin(-heading) + dy * Math.cos(-heading);
-
-            Pose2d deltaPos = new Pose2d(
-                    new Vector2d(relX, relY),
-                    robotPose.heading
-            );
 
 
-            turret.trackGoal(deltaPos);
 
-            TELE.addData("Robot Pose", robotPose);
-            TELE.addData("Goal Pose", goalPose);
-            TELE.addData("Delta Pos", deltaPos);
             TELE.update();
         }
 

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

@@ -79,8 +79,10 @@ public class Robot {
         shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
         shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        shooter1.setVelocity(1400);
         shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
+        shooter2.setVelocity(1400);
 
         hood = hardwareMap.get(Servo.class, "hood");
 

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

@@ -97,8 +97,8 @@ public class Targeting {
         robotInchesY = rotatedY * unitConversionFactor;
 
         // Find approximate location in the grid
-        robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
+        int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
-        robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+        int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
 
         //clamp
         robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));

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

@@ -5,61 +5,83 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.util.Range;
 
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
 @Config
 public class Turret {
 
+    public static double turretTolerance = 0.02;
+    public static double turrPosScalar = 1.009;
+    public static double turret180Range = 0.4;
     public static double turrDefault = 0.4;
-    public static double turretRange = 0.6;
+    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 turretTolerance = 0.02;
+    public static double angleMultiplier = 0.0;
-
+    Robot robot;
-    public static double cameraBearingEqual = 1.5;
+    MultipleTelemetry TELE;
-    public static double errorLearningRate = 0.02; // must be low
+    AprilTagWebcam webcam;
-    public static double maxOffsetDeg = 30.0;
-
-    private final Robot robot;
-    private final MultipleTelemetry TELE;
-    private final AprilTagWebcam webcam;
-
     private int obeliskID = 0;
-    private double offsetDeg = 0.0;
+    private double turrPos = 0.0;
+    private double offset = 0.0;
+    private double bearing = 0.0;
+
+
 
     public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
-        this.robot = rob;
         this.TELE = tele;
+        this.robot = rob;
         this.webcam = cam;
     }
 
-    public double getTurretPos() {
+    public double getTurrPos() {
-        return robot.turr1Pos.getVoltage() / 3.3;
+        return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
+
     }
 
     public void manualSetTurret(double pos){
-        pos = Range.clip(pos, turrMin, turrMax);
         robot.turr1.setPosition(pos);
-        robot.turr2.setPosition(1.0 - pos);
+        robot.turr2.setPosition(1-pos);
     }
 
-    public boolean turretAt(double pos) {
+    public boolean turretEqual(double pos) {
-        return Math.abs(pos - getTurretPos()) < turretTolerance;
+        return Math.abs(pos - this.getTurrPos()) < turretTolerance;
     }
 
-    public double getBearingDeg() {
+    public double getBearing() {
-        AprilTagDetection tag =
+        if (redAlliance) {
-                redAlliance ? webcam.getTagById(24) : webcam.getTagById(20);
+            AprilTagDetection d24 = webcam.getTagById(24);
-        return (tag != null) ? tag.ftcPose.bearing : Double.NaN;
+            if (d24 != null) {
+                bearing = d24.ftcPose.bearing;
+                return bearing;
+            } else {
+                return 1000.0;
+            }
+        } else {
+            AprilTagDetection d20 = webcam.getTagById(20);
+            if (d20 != null) {
+                bearing = d20.ftcPose.bearing;
+                return bearing;
+            } else {
+                return 1000.0;
+            }
+        }
     }
 
     public int detectObelisk() {
-        if (webcam.getTagById(21) != null) obeliskID = 21;
+        AprilTagDetection id21 = webcam.getTagById(21);
-        else if (webcam.getTagById(22) != null) obeliskID = 22;
+        AprilTagDetection id22 = webcam.getTagById(22);
-        else if (webcam.getTagById(23) != null) obeliskID = 23;
+        AprilTagDetection id23 = webcam.getTagById(23);
+        if (id21 != null) {
+            obeliskID = 21;
+        } else if (id22 != null) {
+            obeliskID = 22;
+        } else if (id23 != null) {
+            obeliskID = 23;
+        }
         return obeliskID;
     }
 
@@ -67,34 +89,59 @@ public class Turret {
         return obeliskID;
     }
 
+
+
+    /*
+        Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
+     */
     public void trackGoal(Pose2d deltaPos) {
 
-        double turretAngleDeg = Math.toDegrees(
+        /* ---------------- FIELD → TURRET GEOMETRY ---------------- */
+
+        // Angle from robot to goal in robot frame
+        double desiredTurretAngleDeg = Math.toDegrees(
                 Math.atan2(deltaPos.position.y, deltaPos.position.x)
         );
 
-        double bearingDeg = getBearingDeg();
+        // Robot heading (field → robot)
+        double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble());
 
-        if (!Double.isNaN(bearingDeg) &&
+        // Turret angle needed relative to robot
-                Math.abs(bearingDeg) < cameraBearingEqual) {
+        double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
 
-            offsetDeg -= bearingDeg * errorLearningRate;
+        turretAngleDeg = -turretAngleDeg;
-            offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg);
+
+        // Normalize to [-180, 180]
+        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 += offsetDeg;
+        turretAngleDeg += offset;
 
-        double turretPos =
+        /* ---------------- ANGLE → SERVO ---------------- */
-                turrDefault + (turretAngleDeg / 180.0) * turretRange;
 
-        turretPos = Range.clip(turretPos, turrMin, turrMax);
+        double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
+
+        // Clamp to servo range
+        turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
 
         robot.turr1.setPosition(turretPos);
         robot.turr2.setPosition(1.0 - turretPos);
 
-        TELE.addData("Turret Angle (deg)", turretAngleDeg);
+        /* ---------------- TELEMETRY ---------------- */
-        TELE.addData("Offset (deg)", offsetDeg);
+
-        TELE.addData("Tag Bearing (deg)", bearingDeg);
+        TELE.addData("Turret Angle", turretAngleDeg);
-        TELE.addData("Turret Servo", turretPos);
+        TELE.addData("Bearing", tagBearingDeg);
+        TELE.addData("Offset", offset);
     }
+
 }