From 78d38481a76dce43a6a5c6e3018f59bb19b94318 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 23 Jan 2026 21:44:29 -0600 Subject: [PATCH 1/5] stash --- .../org/firstinspires/ftc/teamcode/teleop/TeleopV3.java | 9 ++------- .../org/firstinspires/ftc/teamcode/tests/TurretTest.java | 9 +-------- .../java/org/firstinspires/ftc/teamcode/utils/Robot.java | 2 +- .../org/firstinspires/ftc/teamcode/utils/Turret.java | 5 +++-- 4 files changed, 7 insertions(+), 18 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 91f0f24..c2223cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -19,6 +19,7 @@ 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.Limelight3A; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -26,7 +27,6 @@ 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; @@ -165,10 +165,7 @@ public class TeleopV3 extends LinearOpMode { // robot.limelight.start(); - AprilTagWebcam webcam = new AprilTagWebcam(); - webcam.init(robot, TELE); - - Turret turret = new Turret(robot, TELE, webcam); + Turret turret = new Turret(robot, TELE, robot.limelight); waitForStart(); waitForStart(); @@ -398,8 +395,6 @@ public class TeleopV3 extends LinearOpMode { turret.trackGoal(deltaPose); - webcam.update(); - //VELOCITY AUTOMATIC if (targetingVel) { vel = targetingSettings.flywheelRPM; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 2542488..6f17fef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -24,10 +24,7 @@ public class TurretTest extends LinearOpMode { telemetry, FtcDashboard.getInstance().getTelemetry() ); - AprilTagWebcam webcam = new AprilTagWebcam(); - webcam.init(robot, TELE); - - Turret turret = new Turret(robot, TELE, webcam); + Turret turret = new Turret(robot, TELE, robot.limelight); waitForStart(); @@ -38,10 +35,6 @@ public class TurretTest extends LinearOpMode { drive.updatePoseEstimate(); turret.trackGoal(drive.localizer.getPose()); - webcam.update(); - webcam.displayAllTelemetry(); - - TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index e4345b1..bb11edf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -20,7 +20,7 @@ public class Robot { //Initialize Public Components - public static boolean usingLimelight = false; + public static boolean usingLimelight = true; public static boolean usingCamera = true; public DcMotorEx frontLeft; public DcMotorEx frontRight; 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 202293c..39cb80a 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 @@ -5,6 +5,7 @@ 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.hardware.limelightvision.Limelight3A; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @@ -23,7 +24,7 @@ public class Turret { public static double angleMultiplier = 0.0; Robot robot; MultipleTelemetry TELE; - AprilTagWebcam webcam; + Limelight3A webcam; private int obeliskID = 0; private double turrPos = 0.0; private double offset = 0.0; @@ -31,7 +32,7 @@ public class Turret { - public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { + public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) { this.TELE = tele; this.robot = rob; this.webcam = cam; From 5922f4e9357afbdcddd948e7ea30eb3b448b52cc Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 23 Jan 2026 22:50:33 -0600 Subject: [PATCH 2/5] need to add zero code to properly test --- .../ftc/teamcode/tests/LimelightTest.java | 63 +++++--------- .../ftc/teamcode/tests/TurretTest.java | 3 - .../ftc/teamcode/utils/Robot.java | 4 +- .../ftc/teamcode/utils/Turret.java | 83 +++++++++++++------ build.gradle | 6 +- 5 files changed, 86 insertions(+), 73 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index 2ab1b16..c8e698d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -4,32 +4,33 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.hardware.limelightvision.LLResult; -import com.qualcomm.hardware.limelightvision.LLResultTypes; -import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import java.util.List; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Turret; @Config @TeleOp //TODO: fix to get the apriltag that it is reading public class LimelightTest extends LinearOpMode { MultipleTelemetry TELE; + Turret turret; + 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 @Override public void runOpMode() throws InterruptedException { - Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight"); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - limelight.pipelineSwitch(pipeline); + robot = new Robot(hardwareMap); + turret = new Turret(robot, TELE, robot.limelight); + robot.limelight.pipelineSwitch(pipeline); waitForStart(); if (isStopRequested()) return; - limelight.start(); while (opModeIsActive()){ if (mode == 0){ - limelight.pipelineSwitch(pipeline); - LLResult result = limelight.getLatestResult(); + robot.limelight.pipelineSwitch(pipeline); + LLResult result = robot.limelight.getLatestResult(); if (result != null) { if (result.isValid()) { TELE.addData("tx", result.getTx()); @@ -38,39 +39,21 @@ public class LimelightTest extends LinearOpMode { } } } else if (mode == 1){ - limelight.pipelineSwitch(1); - LLResult result = limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - int id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } - - } - } else if (mode == 2){ - limelight.pipelineSwitch(4); - LLResult result = limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - TELE.addData("tx", result.getTx()); - TELE.addData("ty", result.getTy()); - TELE.update(); - } - } - } else if (mode == 3){ - limelight.pipelineSwitch(5); - LLResult result = limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - TELE.addData("tx", result.getTx()); - TELE.addData("ty", result.getTy()); - TELE.update(); - } - } + int obeliskID = turret.detectObelisk(); + TELE.addData("Limelight ID", obeliskID); + TELE.update(); + } else if (mode == 2 || mode == 3){ // Use redAlliance variable to switch between red and blue + double tx = turret.getBearing(); + double ty = turret.getTy(); + double x = turret.getLimelightX(); + double y = turret.getLimelightY(); + TELE.addData("tx", tx); + TELE.addData("ty", ty); + TELE.addData("x", x); + TELE.addData("y", y); + TELE.update(); } else { - limelight.pipelineSwitch(0); + robot.limelight.pipelineSwitch(0); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 6f17fef..781f1a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -8,10 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Turret; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Autonomous @Config @@ -35,7 +33,6 @@ public class TurretTest extends LinearOpMode { drive.updatePoseEstimate(); turret.trackGoal(drive.localizer.getPose()); - TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index bb11edf..9dcc104 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -79,10 +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); + shooter1.setVelocity(0); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter2.setVelocity(1400); + shooter2.setVelocity(0); hood = hardwareMap.get(Servo.class, "hood"); 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 39cb80a..75cf6d1 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 @@ -5,9 +5,13 @@ 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.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; @Config public class Turret { @@ -29,6 +33,10 @@ public class Turret { 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; @@ -36,6 +44,7 @@ public class Turret { this.TELE = tele; this.robot = rob; this.webcam = cam; + webcam.start(); } public double getTurrPos() { @@ -52,36 +61,58 @@ public class Turret { return Math.abs(pos - this.getTurrPos()) < turretTolerance; } - public double getBearing() { - if (redAlliance) { - AprilTagDetection d24 = webcam.getTagById(24); - if (d24 != null) { - bearing = d24.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } + private void limelightRead(){ // only for tracking purposes, not general reads + if (redAlliance){ + webcam.pipelineSwitch(3); } else { - AprilTagDetection d20 = webcam.getTagById(20); - if (d20 != null) { - bearing = d20.ftcPose.bearing; - return bearing; - } else { - return 1000.0; + webcam.pipelineSwitch(2); + } + + LLResult result = webcam.getLatestResult(); + if (result != null) { + if (result.isValid()) { + tx = result.getTx(); + ty = result.getTy(); + // MegaTag1 code for receiving position + Pose3D botpose = result.getBotpose(); + if (botpose != null){ + limelightPosX = botpose.getPosition().x; + limelightPosY = botpose.getPosition().y; + } + } } } + public double getBearing() { + tx = 1000; + limelightRead(); + return tx; + } + + public double getTy(){ + limelightRead(); + return ty; + } + + public double getLimelightX(){ + limelightRead(); + return limelightPosX; + } + + public double getLimelightY(){ + limelightRead(); + return limelightPosY; + } + public int detectObelisk() { - AprilTagDetection id21 = webcam.getTagById(21); - AprilTagDetection id22 = webcam.getTagById(22); - AprilTagDetection id23 = webcam.getTagById(23); - if (id21 != null) { - obeliskID = 21; - } else if (id22 != null) { - obeliskID = 22; - } else if (id23 != null) { - obeliskID = 23; + webcam.pipelineSwitch(1); + LLResult result = webcam.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + obeliskID = fiducial.getFiducialId(); + } } return obeliskID; } @@ -90,8 +121,6 @@ public class Turret { return obeliskID; } - - /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading */ diff --git a/build.gradle b/build.gradle index e70f209..02aa304 100644 --- a/build.gradle +++ b/build.gradle @@ -25,5 +25,9 @@ allprojects { } repositories { - mavenCentral() + repositories { + mavenCentral() + google() + maven { url 'https://maven.pedropathing.com' } + } } From 8d29a806966cf3fc7905905a8af1d6744fd651a7 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 24 Jan 2026 14:45:35 -0600 Subject: [PATCH 3/5] need to add zero code to properly test --- .../ftc/teamcode/tests/LimelightTest.java | 9 +++ .../ftc/teamcode/tests/TurretTest.java | 12 +++- .../ftc/teamcode/utils/Turret.java | 56 +++++++++++++------ 3 files changed, 60 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index c8e698d..1ef325a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/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); + } + } + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 781f1a6..8823522 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/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(); } 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 75cf6d1..3ddb14a 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 @@ -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 angleMultiplier = 0.0; + public static double mult = 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); From b5a31afe526cc300c255e0e971ae66e231e611fa Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 24 Jan 2026 15:42:32 -0600 Subject: [PATCH 4/5] i need you @KeshavAnandCode --- .../java/org/firstinspires/ftc/teamcode/utils/Turret.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 3ddb14a..c830e89 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 @@ -16,11 +16,11 @@ 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 public static double cameraBearingEqual = 1; public static double errorLearningRate = 0.15; public static double turrMin = 0.2; @@ -181,7 +181,7 @@ public class Turret { if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) { // Clamp to servo range turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); - } else { + } else { // TODO: add so it only adds error when standstill if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) { // Slowly learn turret offset (persistent calibration) offset -= tagBearingDeg * errorLearningRate; From fefeeb1f2effe8fa90281761679213fb293a4a73 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 24 Jan 2026 17:18:57 -0600 Subject: [PATCH 5/5] i need you @KeshavAnandCode --- .../ftc/teamcode/utils/Turret.java | 32 +++++++++++++++++-- 1 file changed, 29 insertions(+), 3 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 c830e89..2e99ab1 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,3 +1,4 @@ + package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; @@ -5,6 +6,7 @@ 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; @@ -21,8 +23,22 @@ public class Turret { 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 errorLearningRate = 0.15; + + public static double turrMin = 0.2; public static double turrMax = 0.8; public static double mult = 0.0; @@ -32,6 +48,8 @@ public class Turret { 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; @@ -141,6 +159,8 @@ 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 @@ -183,8 +203,14 @@ public class Turret { 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) { - // Slowly learn turret offset (persistent calibration) - offset -= tagBearingDeg * errorLearningRate; + // PID-based offset correction for faster, smoother tracking + + // Proportional: respond to current error + + offset = -controller.calculate(tagBearingDeg); + + + } }