From f701ff884f7e2511166394621777727e887b91f1 Mon Sep 17 00:00:00 2001 From: Plano East Robotics Date: Wed, 11 Feb 2026 16:28:54 -0600 Subject: [PATCH] 2/11 changes --- .../org/firstinspires/ftc/teamcode/FlyOp.java | 21 +- .../ftc/teamcode/OUTDATED/Hware.java | 11 +- .../ftc/teamcode/TeleOpTurretLock.java | 87 +++---- .../ftc/teamcode/TurretLock.java | 238 +++++++++++------- .../drive/StandardTrackingWheelLocalizer.java | 4 +- 5 files changed, 219 insertions(+), 142 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java index bf4f32b..8b6590b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java @@ -3,39 +3,50 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.teamcode.OUTDATED.Hware; @Config -@TeleOp(name="FlyOpTuner", group="Test") +@TeleOp public class FlyOp extends LinearOpMode { private Hware robot; // Dashboard-editable - public static double TARGET = 1500.0; // ticks/sec (flip sign if needed) - public static double launchPos = 1.0; // servo position (0..1 usually) + public static double TARGET = 1500; // ticks/sec (flip sign if needed) + public static double launchPos = 0.7; // servo position (0..1 usually) + public static double intake = 0.6; @Override public void runOpMode() throws InterruptedException { robot = new Hware(); robot.initialize(hardwareMap); + robot.activeTransfer.setPosition(0.7); + + robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); waitForStart(); while (opModeIsActive() && !isStopRequested()) { + robot.bottomFly.setVelocity(TARGET); robot.topFly.setVelocity(TARGET); robot.launchHood.setPosition(launchPos); + robot.intake.set(intake); + + robot.turret.setPower(0); + if (gamepad1.dpadUpWasPressed()) { - robot.intake.set(.8); + robot.activeTransfer.setPosition(.95); } if (gamepad1.dpadDownWasPressed()) { - robot.intake.set(0); + robot.activeTransfer.setPosition(.7); } telemetry.addData("Target", (int)Math.round(TARGET)); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java index 487a25e..f12856e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.OUTDATED; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.Motor; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -16,12 +17,16 @@ public class Hware { public Motor frontRight, frontLeft, backRight, backLeft, intake; public DcMotorEx turret, bottomFly, topFly; public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood; + public Limelight3A limelight; WebcamName webcam; public void initialize(HardwareMap hwMap) { hardwareMap = hwMap; + /** MOTORS **/ + + // Chassis Motors frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435); frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435); @@ -58,10 +63,14 @@ public class Hware { backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); - PIDFCoefficients pidf = new PIDFCoefficients(0.45, 0, 0, 16.3); + PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14 + ); bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf); bottomFly.setDirection(DcMotorSimple.Direction.REVERSE); + + //Vision + limelight = hardwareMap.get(Limelight3A.class, "limelight"); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java index 53c51e1..0364d20 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java @@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -22,8 +24,16 @@ public class TeleOpTurretLock extends LinearOpMode { // Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system. // You said your start is (0,0). So measure these from that same origin. - public static double GOAL_X = 64.0; - public static double GOAL_Y = 75.0; + public static double GOAL_X = 75; + public static double GOAL_Y = 75; + + public static double p = 1, i = 0, d = 0, f = 1; + + + public static double ticksperdegree = 9.738888 * 0.29969; + + Limelight3A limelight; + // Set to a specific tag ID if you want (ex: 5). Use -1 to lock to any visible tag. public static int TARGET_TAG_ID = -1; @@ -35,13 +45,10 @@ public class TeleOpTurretLock extends LinearOpMode { public static double DRIVE_SPEED = 1.0; public static double SLOW_SPEED = 0.45; - // If your webcam name in RC config is different, change this - public static String WEBCAM_NAME = "Webcam 1"; - + LLResult result = null; + /* ----------------------------------------------------- */ - private VisionPortal visionPortal; - private AprilTagProcessor aprilTag; @Override public void runOpMode() { @@ -49,25 +56,17 @@ public class TeleOpTurretLock extends LinearOpMode { // Hardware Hware robot = new Hware(); robot.initialize(hardwareMap); + limelight = hardwareMap.get(Limelight3A.class, "limelight"); // Drive SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap); // Start pose (you said 0,0) drive.setPoseEstimate(new Pose2d(0, 0, 0)); + TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry, p, i, d, f); - // AprilTag - aprilTag = AprilTagProcessor.easyCreateWithDefaults(); - visionPortal = new VisionPortal.Builder() - .setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME)) - .addProcessor(aprilTag) - .build(); - - // TurretLock subsystem - TurretLock turretLock = new TurretLock(robot.turret); - turretLock.goalX = GOAL_X; - turretLock.goalY = GOAL_Y; - turretLock.targetTagId = TARGET_TAG_ID; + // limelight + // Optional: if you want these controlled from dashboard too, you can, // but you already have them inside TurretLock. @@ -80,8 +79,14 @@ public class TeleOpTurretLock extends LinearOpMode { waitForStart(); +// turretLock.cameraInit(); + limelight.pipelineSwitch(1); + limelight.start(); + while (opModeIsActive()) { + result = robot.limelight.getLatestResult(); + /* ---------------- DRIVE ---------------- */ double speed = DRIVE_SPEED; if (gamepad1.left_bumper || gamepad1.right_bumper) speed = SLOW_SPEED; @@ -95,49 +100,37 @@ public class TeleOpTurretLock extends LinearOpMode { drive.setWeightedDrivePower(drivePower); drive.update(); - Pose2d pose = drive.getPoseEstimate(); - /* ---------------- TOGGLE AUTOLOCK ---------------- */ - boolean xNow = gamepad2.x; - if (xNow && !xPrev) autoLockEnabled = !autoLockEnabled; - xPrev = xNow; + Pose2d pose = drive.getPoseEstimate(); /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */ turretLock.goalX = GOAL_X; turretLock.goalY = GOAL_Y; - turretLock.targetTagId = TARGET_TAG_ID; /* ---------------- MANUAL TRIM ---------------- */ // Left trigger = +trim, Right trigger = -trim (same style as your old code) double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger); if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg); - if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim - if (gamepad2.b) turretLock.resetVisionTrim(); // reset vision trim +// telemetry.update();\ +// + turretLock.update(pose, result); - /* ---------------- AUTOLOCK UPDATE ---------------- */ - List detections = aprilTag.getDetections(); + telemetry.addData("VisionAngle", turretLock.getBearing()); + telemetry.addData("Angle", turretLock.getFinalDeg()); + telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543)); + telemetry.addData("MODE", turretLock.getTYPE()); + telemetry.addData("Tag ID", turretLock.getObeliskID()); + telemetry.addData("Does Lime have Result", result.isValid()); - if (autoLockEnabled) { - turretLock.update(pose, detections); - } else { - // If autolock is off, just stop turret motor where it is - // (or you can put your own manual turret code here) - robot.turret.setPower(0); - } + telemetry.addData("Botpose", result.getBotpose()); + telemetry.addData("Yaw", result.getTx()); + + telemetry.addData("Status", turretLock.getStatus()); + + telemetry.addData("LimelightInfo", robot.limelight.getStatus()); - /* ---------------- TELEMETRY ---------------- */ - telemetry.addData("AutoLock", autoLockEnabled); - telemetry.addData("Pose", "x=%.1f y=%.1f h=%.1fdeg", - pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading())); - telemetry.addData("Goal", "x=%.1f y=%.1f", GOAL_X, GOAL_Y); - telemetry.addData("TargetTag", TARGET_TAG_ID); - telemetry.addData("TagCount", (detections == null) ? 0 : detections.size()); - telemetry.addData("TurretTicks", robot.turret.getCurrentPosition()); - telemetry.addLine("Controls: GP2 X=toggle, triggers=trim, Y=reset trim, B=reset vision"); telemetry.update(); } - - if (visionPortal != null) visionPortal.close(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java index c6908e8..cf93b5e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java @@ -1,50 +1,65 @@ package org.firstinspires.ftc.teamcode; +import static java.lang.Math.abs; + import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.teamcode.OUTDATED.Hware; +import org.firstinspires.ftc.teamcode.util.PersonalPID; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import java.util.List; public class TurretLock { + public enum MODE{ + LIME, + ODOMETRY + } + + public MODE TYPE = MODE.ODOMETRY; /* ------------------- TUNABLES ------------------- */ // Field target (in RoadRunner field units; usually inches) // Set this to the AprilTag/backdrop location you want to aim at. - public double goalX = 64.0; - public double goalY = 75.0; + public double goalX = 75; + public double goalY = 75; + + PersonalPID pid; // Turret conversion // If you already trust your TICKS_PER_DEGREE, keep it. - public double ticksPerDegree = 9.738888; + public double ticksPerDegree = 2.543; //9.738888 * 0.29969; // Mechanical limits (ticks). Keep your existing numbers. public int minTicks = (int) (ticksPerDegree * (-90)); - public int maxTicks = (int) (ticksPerDegree * ( 90)); + public int maxTicks = (int) (ticksPerDegree * ( 90 )); - // Which AprilTag to lock to (set -1 to accept any visible tag) - public int targetTagId = -1; + public double finalDeg = 0; - // Vision correction: degrees of turret change per degree of tag "bearing"/yaw error. - // Start small (0.6–1.2) and tune. - public double visionDegPerDegError = 0.9; + public static boolean limelightUsed = true; - // Deadband so it doesn’t twitch - public double visionDeadbandDeg = 0.6; + public static double visionCorrectionGain = 0.08; // Single tunable gain + public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle + public static double cameraBearingEqual = 0.5; // Deadband - // How hard we trust vision vs odom. - // 0 = ignore vision, 1 = full correction. - public double visionBlend = 1.0; + // TODO: tune these values for limelight + Hware robot; + Limelight3A webcam; + double tx = 0.0; + double ty = 0.0; + double limelightPosX = 0.0; + double limelightPosY = 0.0; + private int obeliskID = 0; - // Smooth the vision correction so it doesn’t jump - // 0.0 = no smoothing, 0.8 = heavy smoothing - public double visionLowPass = 0.65; - - // If we haven't seen a tag in this many seconds, slowly decay vision trim back to 0 - public double trimDecayPerSec = 12.0; // deg/sec back toward 0 + double limelightOffset; // Optional: flip direction if your turret sign is backwards public boolean invertTurret = false; @@ -56,29 +71,108 @@ public class TurretLock { private double visionTrimDeg = 0.0; // learned trim from camera private double filteredVisionTrimDeg = 0.0; - private final ElapsedTime tagTimer = new ElapsedTime(); + public double p, i, d, f; - public TurretLock(DcMotorEx turretMotor) { + + Telemetry tele; + + // private final ElapsedTime tagTimer = new ElapsedTime(); + + public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele, double p, double i, double d, double f) { this.turret = turretMotor; - tagTimer.reset(); + this.webcam = cam; + this.tele = tele; + this.p = p; + this.i = i; + this.d = d; + this.f = f; } - /** Call this if you want driver trim like your triggers (degrees). */ - public void addManualTrimDeg(double deltaDeg) { + public double getTurrPos() { + return turret.getCurrentPosition(); + } + + public void addManualTrimDeg(double deltaDeg){ manualTrimDeg += deltaDeg; } - public void resetManualTrim() { - manualTrimDeg = 0.0; + public void resetManualTrim(){ + manualTrimDeg = 0; + } + private void limelightRead(LLResult result) { // only for tracking purposes, not general reads + 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 void resetVisionTrim() { - visionTrimDeg = 0.0; - filteredVisionTrimDeg = 0.0; + public double getBearing() { + // tx = 1000; + LLResult result = webcam.getLatestResult(); + return result.getBotpose().getOrientation().getYaw(); } + public double getTy() { + return ty; + } + + public double getLimelightX() { + return limelightPosX; + } + + public double getLimelightY() { + return limelightPosY; + } + + public int detectObelisk() { + com.qualcomm.hardware.limelightvision.LLResult result = webcam.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + obeliskID = fiducial.getFiducialId(); + } + } + return obeliskID; + } + + public int getObeliskID() { + return obeliskID; + } + + + /* + Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading + */ + + private double bearingAlign(LLResult llResult) { + double targetTx = 0; + if(llResult != null && llResult.isValid()){ + targetTx = llResult.getTx(); // How far left or right the target is (degrees) + + } + return llResult.getBotpose().getOrientation().getYaw(); + } + + public void Init(){ + pid = new PersonalPID(p, i, d, f); + } + + public String getStatus(){ + return String.valueOf(webcam.getStatus()); + } + + /** Main update: call every loop. */ - public void update(Pose2d robotPose, List detections) { + public void update(Pose2d robotPose, LLResult result) { // 1) ODOM: compute turret angle needed to face goal (in degrees) double dx = goalX - robotPose.getX(); double dy = goalY - robotPose.getY(); @@ -98,34 +192,23 @@ public class TurretLock { if (invertTurret) turretCmdDeg = -turretCmdDeg; // 2) VISION: find tag error and update vision trim - Double tagErrorDeg = getTagErrorDeg(detections); - if (tagErrorDeg != null) { - // deadband - if (Math.abs(tagErrorDeg) < visionDeadbandDeg) tagErrorDeg = 0.0; - - // Convert tag error into turret trim change. - // Sign: if tag is to the right (+error), turret should move right or left? - // If it goes the wrong way, flip the sign here (multiply by -1). - double deltaTrim = (tagErrorDeg * visionDegPerDegError) * visionBlend; - - // We want trim to counteract the observed error, so subtract. - visionTrimDeg -= deltaTrim; - - tagTimer.reset(); - } else { - // no tag: decay vision trim toward 0 over time - double dt = 0.02; // assume ~50Hz; fine for a simple decay - double decay = trimDecayPerSec * dt; - if (visionTrimDeg > 0) visionTrimDeg = Math.max(0, visionTrimDeg - decay); - else if (visionTrimDeg < 0) visionTrimDeg = Math.min(0, visionTrimDeg + decay); + limelightRead(result); + filteredVisionTrimDeg = result.getTx(); + switch (TYPE){ + case LIME: + finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg; + if (!result.isValid()) { + TYPE = MODE.ODOMETRY; + } + break; + case ODOMETRY: + finalDeg = turretCmdDeg + manualTrimDeg; + if (finalDeg < 12 || finalDeg > -12) { + TYPE = MODE.LIME; + } + break; } - // Low-pass filter the trim so it doesn’t jerk - filteredVisionTrimDeg = filteredVisionTrimDeg * visionLowPass + visionTrimDeg * (1.0 - visionLowPass); - - // 3) Combine - double finalDeg = turretCmdDeg + manualTrimDeg + filteredVisionTrimDeg; - // Optional: keep within your allowed lock range (like ±90) finalDeg = clamp(finalDeg, -90, 90); @@ -133,42 +216,23 @@ public class TurretLock { int targetTicks = (int) Math.round(finalDeg * ticksPerDegree); targetTicks = clamp(targetTicks, minTicks, maxTicks); +// double pow = pid.calculate(targetTicks); +// turret.setPower(pow); + turret.setTargetPosition(targetTicks); turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION); turret.setPower(1.0); + } + + public MODE getTYPE() { + return TYPE; + } + + public double getFinalDeg(){ + return finalDeg; } /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */ - private Double getTagErrorDeg(List detections) { - if (detections == null || detections.isEmpty()) return null; - - AprilTagDetection best = null; - - for (AprilTagDetection d : detections) { - if (d == null || d.metadata == null) continue; // metadata can be null on some setups - if (targetTagId != -1 && d.id != targetTagId) continue; - - // pick the first good one (or you can pick closest by range if you want) - best = d; - break; - } - - if (best == null) return null; - - // FTC AprilTagDetection typically gives: - // best.ftcPose.yaw (degrees) // yaw ~ left-right rotation of tag relative to camera - // best.ftcPose.bearing (degrees) on some versions - // - // We'll prefer "bearing" if present; otherwise use yaw. - try { - // If your SDK has bearing: - // return best.ftcPose.bearing; - // Otherwise: - return best.ftcPose.yaw; - } catch (Exception e) { - return null; - } - } private static double normalizeDeg(double deg) { while (deg > 180) deg -= 360; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java index 2fd11f5..595b7c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java @@ -53,8 +53,8 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer lastEncPositions = lastTrackingEncPositions; lastEncVels = lastTrackingEncVels; - leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft")); - rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake")); + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake")); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft")); frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight")); // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)