Merge remote-tracking branch 'origin/Targeting' into Targeting

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

@@ -50,6 +50,8 @@ 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;
@@ -121,6 +123,7 @@ public class TeleopV3 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
+    boolean turretInterpolate = true;
 
     public static double velPrediction(double distance) {
         if (distance < 30) {
@@ -411,7 +414,7 @@ public class TeleopV3 extends LinearOpMode {
 
 
             targetingSettings = targeting.calculateSettings
-                    (robotX,robotY,robotHeading,0.0);
+                    (robotX,robotY,robotHeading,0.0, turretInterpolate);
 
             //VELOCITY AUTOMATIC
             if (targetingVel) {
@@ -443,46 +446,100 @@ public class TeleopV3 extends LinearOpMode {
 
             //TODO: test the camera teleop code
 
+            // TODO: TEST THIS 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
+            LLResult result = robot.limelight.getLatestResult();
-//                double bearing;
+            boolean limelightActive = false;
-//
-//                LLResult result = robot.light.getLatestResult();
-//                if (result != null) {
-//                    if (result.isValid()) {
-//                        bearing = result.getTx() * bMult;
-//
-//                        double bearingCorrection = bearing / bDiv;
-//
-//                        error += bearingCorrection;
-//
-//                        camTicker++;
-//                        TELE.addData("tx", bearingCorrection);
-//                        TELE.addData("ty", result.getTy());
-//                    }
-//                }
-//
-//            } else {
-//                camTicker = 0;
-//                overrideTurr = false;
-//            }
             
-            if (!overrideTurr) {
+            double turretMin = 0.13;
+            double turretMax = 0.83;
+            
+            if (result != null && result.isValid()) {
+                double tx = result.getTx();
+                double ty = result.getTy();
+                
+                if (Math.abs(tx) > limelightDeadband) {
+                    limelightActive = true;
+                    overrideTurr = true;
+                    
+                    double currentTurretPos = servo.getTurrPos();
+                    
+                    // + tx means tag is right, so rotate right
+                    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) {
+            if (manualTurret && !limelightActive) {
                 pos = turrDefault + (manualOffset / 100) + error;
             }
 
-            if (!overrideTurr) {
+            if (!overrideTurr && !limelightActive) {
                 turretPos = pos;
             }
 
-            if (Math.abs(gamepad2.left_stick_x)>0.2) {
+            if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) {
                 manualOffset += 1.35 * gamepad2.left_stick_x;
             }
 
@@ -873,6 +930,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

@@ -1,9 +1,12 @@
 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;
 
@@ -35,7 +38,29 @@ public class TurretTest extends LinearOpMode {
         while(opModeIsActive()){
 
             drive.updatePoseEstimate();
-            turret.trackGoal(drive.localizer.getPose());
+            Pose2d robotPose = drive.localizer.getPose();
+            
+            double dx = goalPose.position.x - robotPose.position.x;
+            double dy = goalPose.position.y - robotPose.position.y;
+
+            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/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));
@@ -100,12 +100,42 @@ public class Targeting {
         robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
         robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
 
-        // Use Grid Location to perform lookup
+        //clamp
-        // Keep it simple for now but may want to interpolate results
+        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

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