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 4be2f4a..fce535b 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; @@ -396,8 +396,6 @@ public class TeleopV3 extends LinearOpMode { turret.trackGoal(deltaPose); - - //VELOCITY AUTOMATIC if (targetingVel) { vel = targetingSettings.flywheelRPM; @@ -634,7 +632,6 @@ public class TeleopV3 extends LinearOpMode { } } - // // if (shootAll) { // @@ -806,7 +803,6 @@ public class TeleopV3 extends LinearOpMode { for (LynxModule hub : allHubs) { hub.clearBulkCache(); } - // TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); @@ -833,7 +829,7 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("shootall commanded", shootAll); // Targeting Debug TELE.addData("robotX", robotX); - TELE.addData( "robotY", robotY); + TELE.addData("robotY", robotY); TELE.addData("robotInchesX", targeting.robotInchesX); TELE.addData( "robotInchesY", targeting.robotInchesY); TELE.addData("Targeting Interpolate", turretInterpolate); 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..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 @@ -4,32 +4,35 @@ 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 + public static boolean turretMode = false; + public static double turretPos = 0.501; @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,40 +41,29 @@ 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); } + + 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 b42fcc8..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 @@ -8,15 +8,12 @@ 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 public class TurretTest extends LinearOpMode { - public static boolean zeroTurr = false; @Override public void runOpMode() throws InterruptedException { @@ -26,11 +23,9 @@ public class TurretTest extends LinearOpMode { telemetry, FtcDashboard.getInstance().getTelemetry() ); - Turret turret = new Turret(robot, TELE, robot.limelight); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); while(opModeIsActive()){ @@ -38,16 +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/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 347b850..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 @@ -21,7 +21,7 @@ public class Robot { //Initialize Public Components public static boolean usingLimelight = true; - public static boolean usingCamera = false; + public static boolean usingCamera = true; public DcMotorEx frontLeft; public DcMotorEx frontRight; public DcMotorEx backLeft; 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 ced735b..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; @@ -16,52 +18,50 @@ 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 errorLearningRate = -0.15; + + public static double turrMin = 0.2; public static double turrMax = 0.8; public static double mult = 0.0; - - public static double staticOffsetRate = -0.15; - public static double deltaAngleThreshold = 0.02; - public static double angleMultiplier = 0.0; - - public static double fastSeekThreshold = 5.0; // Switch to medium mode below this - public static double mediumSeekThreshold = 2.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.2; // Correction gain for small errors - public static double maxOffsetChangePerCycle = 0.3; // Max offset change per cycle (degrees) - public static double finalInterpolation = 0.1; // Final position interpolation factor - - - // TODO: tune these values for limelight - - public static double clampTolerance = 0.03; + private boolean lockOffset = false; 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; - private boolean lockOffset = false; - private int obeliskID = 0; - - private double offset = 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) { + if (redAlliance){ webcam.pipelineSwitch(3); } else { webcam.pipelineSwitch(2); @@ -78,17 +78,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 +101,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 +116,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 +158,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,75 +173,55 @@ 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; - - /* ---------------- LIMELIGHT VISION CORRECTION ---------------- */ - + /* ---------------- APRILTAG CORRECTION ---------------- */ +// double tagBearingDeg = getBearing(); // + = target is to the left - boolean hasValidTarget = (tagBearingDeg != 1000.0); - // Apply persistent offset from previous corrections turretAngleDeg += offset; - // Active correction if we see the target - if (hasValidTarget && !lockOffset) { - double bearingError = Math.abs(tagBearingDeg); + /* ---------------- ANGLE → SERVO ---------------- */ - 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; - } + double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); + + // Clamp to servo range + 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 { // 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); - // Immediate correction to turret angle - turretAngleDeg -= tagBearingDeg * correctionGain; - // Learn offset slowly for persistent calibration - double offsetChange = -tagBearingDeg * errorLearningRate; - // Rate limit to prevent oscillation - offsetChange = Math.max(-maxOffsetChangePerCycle, Math.min(offsetChange, maxOffsetChangePerCycle)); - offset += offsetChange; - TELE.addData("Correction Mode", bearingError > fastSeekThreshold ? "FAST" : - bearingError > mediumSeekThreshold ? "MEDIUM" : "FINE"); } } - - /* ---------------- 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 = currentPos + (targetTurretPos - currentPos) * finalInterpolation; - - // Set servo positions robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1.0 - turretPos); - /* ---------------- TELEMETRY ---------------- */ - 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); + TELE.addData("Turret Angle", turretAngleDeg); + TELE.addData("Bearing", tagBearingDeg); + TELE.addData("Offset", offset); } } 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' } + } }