From b08fe5ada560c5369386d2999780832ca11b3432 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Mon, 26 Jan 2026 16:19:44 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/utils/Turret.java | 157 +++++++++++------- 1 file changed, 96 insertions(+), 61 deletions(-) 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 2e99ab1..f8945bc 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 @@ -1,4 +1,3 @@ - package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; @@ -6,7 +5,6 @@ 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.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -18,50 +16,54 @@ import java.util.List; @Config public class Turret { + public static double turretTolerance = 0.02; public static double turrPosScalar = 0.00011264432; public static double turret180Range = 0.4; public static double turrDefault = 0.4; - // TODO: tune these values for limelight -// At the top with other static variables: - public static double kP = 0.015; // Proportional gain - tune this first - public static double kI = 0.0005; // Integral gain - add slowly if needed - public static double kD = 0.002; // Derivative gain - helps prevent overshoot - - public static double kF = 0.002; // Derivative gain - helps prevent overshoot - - public static double maxOffset = 10; // degrees - safety limit - - // Add these as instance variables: - private double lastTagBearing = 0.0; - private double offsetIntegral = 0.0; - public static double cameraBearingEqual = 1; - - - public static double turrMin = 0.2; - public static double turrMax = 0.8; + public static double errorLearningRate = -0.15; + public static double turrMin = 0.15; + public static double turrMax = 0.85; public static double mult = 0.0; - private boolean lockOffset = false; + + public static double staticOffsetRate = -0.15; + public static double deltaAngleThreshold = 0.02; + public static double angleMultiplier = 0.0; + + public static double fastSeekThreshold = 10.0; // Switch to medium mode below this + public static double mediumSeekThreshold = 3.0; // Switch to fine mode below this + public static double fastCorrectionGain = 0.75; // Correction gain for large errors + public static double mediumCorrectionGain = 0.4; // Correction gain for medium errors + public static double fineCorrectionGain = 0.1; // Correction gain for small errors + public static double maxOffsetChangePerCycle = 15; // Max offset change per cycle (degrees) + public static double finalInterpolation = 0.5; // Final position interpolation factor + + + // TODO: tune these values for limelight + + public static double clampTolerance = 0.03; Robot robot; MultipleTelemetry TELE; Limelight3A webcam; - private int obeliskID = 0; - private double offset = 0.0; - private PIDFController controller = new PIDFController(kP, kI, kD, kF); double tx = 0.0; double ty = 0.0; double limelightPosX = 0.0; double limelightPosY = 0.0; - public static double clampTolerance = 0.03; + private boolean lockOffset = false; + private int obeliskID = 0; + + private double offset = 0.0; + + private double permanentOffset = 0.0; public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob; this.webcam = cam; webcam.start(); - if (redAlliance){ + if (redAlliance) { webcam.pipelineSwitch(3); } else { webcam.pipelineSwitch(2); @@ -78,17 +80,17 @@ public class Turret { } - public void manualSetTurret(double pos){ + public void manualSetTurret(double pos) { robot.turr1.setPosition(pos); - robot.turr2.setPosition(1-pos); + robot.turr2.setPosition(1 - pos); } public boolean turretEqual(double pos) { return Math.abs(pos - this.getTurrPos()) < turretTolerance; } - private void limelightRead(){ // only for tracking purposes, not general reads - if (redAlliance){ + private void limelightRead() { // only for tracking purposes, not general reads + if (redAlliance) { webcam.pipelineSwitch(3); } else { webcam.pipelineSwitch(2); @@ -101,7 +103,7 @@ public class Turret { ty = result.getTy(); // MegaTag1 code for receiving position Pose3D botpose = result.getBotpose(); - if (botpose != null){ + if (botpose != null) { limelightPosX = botpose.getPosition().x; limelightPosY = botpose.getPosition().y; } @@ -116,17 +118,17 @@ public class Turret { return tx; } - public double getTy(){ + public double getTy() { limelightRead(); return ty; } - public double getLimelightX(){ + public double getLimelightX() { limelightRead(); return limelightPosX; } - public double getLimelightY(){ + public double getLimelightY() { limelightRead(); return limelightPosY; } @@ -158,9 +160,9 @@ public class Turret { /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading */ + public void trackGoal(Pose2d deltaPos) { - controller.setPIDF(kP, kI, kD, kF); /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ // Angle from robot to goal in robot frame @@ -173,55 +175,88 @@ 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 ---------------- */ -// + + /* ---------------- LIMELIGHT VISION CORRECTION ---------------- */ + double tagBearingDeg = getBearing(); // + = target is to the left + boolean hasValidTarget = (tagBearingDeg != 1000.0); - turretAngleDeg += offset; - /* ---------------- ANGLE → SERVO ---------------- */ + turretAngleDeg += permanentOffset; - double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); - // Clamp to servo range - double currentEncoderPos = this.getTurrPos(); + // Active correction if we see the target + if (hasValidTarget && !lockOffset) { + double bearingError = Math.abs(tagBearingDeg); - 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 { // TODO: add so it only adds error when standstill - if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) { - // PID-based offset correction for faster, smoother tracking - - // Proportional: respond to current error - - offset = -controller.calculate(tagBearingDeg); + if (bearingError > cameraBearingEqual) { + // Dual-mode correction: fast when far, gentle when close + double correctionGain; + if (bearingError > fastSeekThreshold) { + correctionGain = fastCorrectionGain; + } else if (bearingError > mediumSeekThreshold) { + correctionGain = mediumCorrectionGain; + } else { + correctionGain = fineCorrectionGain; + } + offset = correctionGain*tagBearingDeg; + + + TELE.addData("offset", offset); + TELE.addData("offsetChange", offsetChange); + + + TELE.addData("Correction Mode", bearingError > fastSeekThreshold ? "FAST" : + bearingError > mediumSeekThreshold ? "MEDIUM" : "FINE"); + } else { + permanentOffset = offset; } } + + // Apply persistent offset from previous corrections + turretAngleDeg += offset; + + + /* ---------------- ANGLE → SERVO POSITION ---------------- */ + + double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + + // Clamp to physical servo limits + targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax)); + + // Interpolate towards target position + double currentPos = getTurrPos(); + double turretPos = targetTurretPos; + + if (targetTurretPos == turrMin){ + turretPos = turrMin; + } else if (targetTurretPos == turrMax){ + turretPos = turrMax; + } + + // Set servo positions robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos); + /* ---------------- TELEMETRY ---------------- */ - TELE.addData("Turret Angle", turretAngleDeg); - TELE.addData("Bearing", tagBearingDeg); - TELE.addData("Offset", offset); + TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); + TELE.addData("Target Pos", "%.3f", targetTurretPos); + TELE.addData("Current Pos", "%.3f", currentPos); + TELE.addData("Commanded Pos", "%.3f", turretPos); + TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); + TELE.addData("Learned Offset", "%.2f", offset); } }