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;
@@ -121,6 +120,7 @@ public class TeleopV3 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
+    boolean turretInterpolate = false;
 
     public static double velPrediction(double distance) {
         if (distance < 30) {
@@ -166,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;
 
@@ -379,39 +385,21 @@ 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 dy = goalY - robotY;  // delta y from robot to goal
+            double dx = robotX - goalX;  // delta x 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);
+                    (robotX,robotY,robotHeading,0.0, turretInterpolate);
+
+            turret.trackGoal(deltaPose);
+
+            webcam.update();
 
             //VELOCITY AUTOMATIC
             if (targetingVel) {
@@ -443,8 +431,6 @@ public class TeleopV3 extends LinearOpMode {
 
             //TODO: test the camera teleop code
 
-            TELE.addData("posS2", pos);
-
 //            if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
 //                double bearing;
 //
@@ -468,27 +454,6 @@ public class TeleopV3 extends LinearOpMode {
 //                overrideTurr = false;
 //            }
 
-            if (!overrideTurr) {
-                turretPos = pos;
-            }
-
-            TELE.addData("posS3", turretPos);
-
-            if (manualTurret) {
-                pos = turrDefault + (manualOffset / 100) + error;
-            }
-
-            if (!overrideTurr) {
-                turretPos = pos;
-            }
-
-            if (Math.abs(gamepad2.left_stick_x)>0.2) {
-                manualOffset += 1.35 * gamepad2.left_stick_x;
-            }
-
-            robot.turr1.setPosition(pos);
-            robot.turr2.setPosition(1 - pos);
-
             //HOOD:
 
             if (targetingHood) {
@@ -873,6 +838,7 @@ public class TeleopV3 extends LinearOpMode {
             TELE.addData( "robotY", robotY);
             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);

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

@@ -30,12 +30,20 @@ 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();
             turret.trackGoal(drive.localizer.getPose());
+
+            webcam.update();
+            webcam.displayAllTelemetry();
+
+
+
+            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

@@ -84,7 +84,7 @@ public class Targeting {
     {
     }
 
-    public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) {
+    public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
         Settings recommendedSettings = new Settings(0.0, 0.0);
 
         double cos45 = Math.cos(Math.toRadians(-45));
@@ -97,15 +97,45 @@ public class Targeting {
         robotInchesY = rotatedY * unitConversionFactor;
 
         // Find approximate location in the grid
-        robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1);
-        robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
+        int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
+        int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
 
-        // Use Grid Location to perform lookup
-        // Keep it simple for now but may want to interpolate results
+        //clamp
+        robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
+        robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
+
+        // basic search
+        if(!interpolate) {
             if ((robotGridY < 6) && (robotGridX <6)) {
                 recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM;
                 recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle;
             }
+            return recommendedSettings;
+        } else {
+
+            // bilinear interpolation
+            int x0 = robotGridX;
+            int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
+            int y0 = gridY;
+            int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
+
+            double x = (robotInchesX - (x0 * tileSize)) / tileSize;
+            double y = (robotInchesY - (y0 * tileSize)) / tileSize;
+
+            double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
+            double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
+            double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
+            double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
+
+            double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
+            double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
+            double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
+            double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
+
+            recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
+            recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
+
             return recommendedSettings;
         }
     }
+}

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

@@ -13,12 +13,14 @@ public class Turret {
 
     public static double turretTolerance = 0.02;
     public static double turrPosScalar = 1.009;
-    public static double turret180Range = 0.6;
+    public static double turret180Range = 0.4;
     public static double turrDefault = 0.4;
-    public static double cameraBearingEqual = 1.5;
-    public static double errorLearningRate = 2;
+    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 angleMultiplier = 0.0;
     Robot robot;
     MultipleTelemetry TELE;
     AprilTagWebcam webcam;
@@ -27,6 +29,8 @@ public class Turret {
     private double offset = 0.0;
     private double bearing = 0.0;
 
+
+
     public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
         this.TELE = tele;
         this.robot = rob;
@@ -85,9 +89,7 @@ public class Turret {
         return obeliskID;
     }
 
-    public void update() {
 
-    }
 
     /*
         Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
@@ -107,15 +109,18 @@ public class Turret {
         // Turret angle needed relative to robot
         double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
 
+        turretAngleDeg = -turretAngleDeg;
+
         // Normalize to [-180, 180]
         while (turretAngleDeg > 180) turretAngleDeg -= 360;
         while (turretAngleDeg < -180) turretAngleDeg += 360;
 
-        /* ---------------- APRILTAG CORRECTION ---------------- */
 
+        /* ---------------- APRILTAG CORRECTION ---------------- */
+//
         double tagBearingDeg = getBearing();  // + = target is to the left
 
-        if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) < cameraBearingEqual) {
+        if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
             // Slowly learn turret offset (persistent calibration)
             offset -= tagBearingDeg * errorLearningRate;
         }
@@ -124,7 +129,7 @@ public class Turret {
 
         /* ---------------- ANGLE → SERVO ---------------- */
 
-        double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0));
+        double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
 
         // Clamp to servo range
         turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));