From 4386980eff398f2a55f05bee0866d878122d2f16 Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Mon, 27 Oct 2025 21:34:33 -0400 Subject: [PATCH 1/8] Limelight Testing https://www.youtube.com/watch?v=-EfOzB_A00Q --- .../limelight/AprilTagLimelightTest.java | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/AprilTagLimelightTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/AprilTagLimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/AprilTagLimelightTest.java new file mode 100644 index 000000000000..79ab2771cf3a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/AprilTagLimelightTest.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.vision.limelight; + +import com.qualcomm.hardware.bosch.BHI260IMU; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +public class AprilTagLimelightTest extends OpMode { + + private Limelight3A limelight; + private IMU imu; + + public void init(){ + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); + imu = hardwareMap.get(BHI260IMU.class, "imu"); + imu.initialize( + new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP + ) + ) + ); + imu.resetYaw(); + } + + public void start(){ + limelight.start(); + } + + public void loop(){ + YawPitchRollAngles orientations = imu.getRobotYawPitchRollAngles(); + limelight.updateRobotOrientation(orientations.getYaw()); + LLResult llResult = limelight.getLatestResult(); + if (llResult != null && llResult.isValid()) { + Pose3D botPose = llResult.getBotpose_MT2(); + telemetry.addData("Tag Pose X", llResult.getTx()); + telemetry.addData("Tag Pose Y", llResult.getTy()); + telemetry.addData("Tag Pose Area", llResult.getTa()); + } else { + telemetry.addData("Tag Pose", "No target found"); + } + } +} From b17fb29acea7078a6dcd6f699221ce9dca65d487 Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Tue, 28 Oct 2025 18:28:03 -0400 Subject: [PATCH 2/8] Estimate distance --- TeamCode/build.gradle | 2 +- .../vision/limelight/EstimateDistance.java | 64 +++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index a989cf69d276..bceafadbc922 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -49,5 +49,5 @@ dependencies { implementation project(':FtcRobotController') implementation("dev.frozenmilk.sinister:Sloth:0.2.4") implementation("com.acmerobotics.slothboard:dashboard:0.2.4+0.4.17") - implementation 'com.acmerobotics.dashboard:dashboard:0.5.0' + //implementation 'com.acmerobotics.dashboard:dashboard:0.5.0' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java new file mode 100644 index 000000000000..8404e06fb5dc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.vision.limelight; + +import com.qualcomm.hardware.bosch.BHI260IMU; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +@TeleOp(name = "Estimate Distance to Target", group = "Vision") +public class EstimateDistance extends OpMode { + + private Limelight3A limelight; + private IMU imu; + + private static double h1 = 4.0; //height of limelight in inches + private static double h2 = 26.0; //height of target in inches + private static double a1 = 20.0; //angle of limelight + private double a2; + + public void init(){ + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); + limelight.setPollRateHz(100); + imu = hardwareMap.get(BHI260IMU.class, "imu"); + imu.initialize( + new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP + ) + ) + ); + imu.resetYaw(); + } + + public void start(){ + limelight.start(); + } + + public void loop(){ + YawPitchRollAngles orientations = imu.getRobotYawPitchRollAngles(); + limelight.updateRobotOrientation(orientations.getYaw()); + + LLResult llResult = limelight.getLatestResult(); + if (llResult != null && llResult.isValid()) { + a2 = llResult.getTy(); + double distance = (h2 - h1) / Math.tan(Math.toRadians(a1 + a2)); + + double botposeDistance = llResult.getBotposeAvgDist(); + telemetry.addData("Estimated Distance (inches)", distance); + telemetry.addData("Botpose Average Distance (inches)", botposeDistance); + telemetry.addData("Tag X Angle", llResult.getTx()); + telemetry.addData("Tag Pose Area", llResult.getTa()); + } else { + telemetry.addData("Estimated Distance", "No target found"); + + } + } +} From 4cb35d90a816d2c4d031e6c30e246b0761b5134e Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Tue, 11 Nov 2025 18:46:46 -0500 Subject: [PATCH 3/8] Pose coordinate conversion --- .../vision/limelight/EstimateDistance.java | 12 +-- .../vision/limelight/PoseTesting.java | 79 +++++++++++++++++++ 2 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java index 8404e06fb5dc..7c43d73608f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java @@ -17,9 +17,9 @@ public class EstimateDistance extends OpMode { private Limelight3A limelight; private IMU imu; - private static double h1 = 4.0; //height of limelight in inches + private static double h1 = 13.5; //height of limelight in inches private static double h2 = 26.0; //height of target in inches - private static double a1 = 20.0; //angle of limelight + private static double a1 = 33.0; //angle of limelight private double a2; public void init(){ @@ -30,8 +30,8 @@ public void init(){ imu.initialize( new IMU.Parameters( new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, - RevHubOrientationOnRobot.UsbFacingDirection.UP + RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.RIGHT ) ) ); @@ -54,11 +54,13 @@ public void loop(){ double botposeDistance = llResult.getBotposeAvgDist(); telemetry.addData("Estimated Distance (inches)", distance); telemetry.addData("Botpose Average Distance (inches)", botposeDistance); + telemetry.addData("Botpose", llResult.getBotpose_MT2()); telemetry.addData("Tag X Angle", llResult.getTx()); telemetry.addData("Tag Pose Area", llResult.getTa()); + } else { telemetry.addData("Estimated Distance", "No target found"); - } + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java new file mode 100644 index 000000000000..8d9eb1e5296f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java @@ -0,0 +1,79 @@ +package org.firstinspires.ftc.teamcode.vision.limelight; + +import com.qualcomm.hardware.bosch.BHI260IMU; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +@TeleOp(name = "Pose Testing", group = "Vision") +public class PoseTesting extends OpMode { + + private Limelight3A limelight; + private IMU imu; + + public void init(){ + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); + imu = hardwareMap.get(BHI260IMU.class, "imu"); + imu.initialize( + new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP + ) + ) + ); + imu.resetYaw(); + } + + public void start(){ + limelight.start(); + } + + public void loop() { + YawPitchRollAngles orientations = imu.getRobotYawPitchRollAngles(); + limelight.updateRobotOrientation(orientations.getYaw()); + LLResult llResult = limelight.getLatestResult(); + + if (llResult != null && llResult.isValid()) { + Pose3D botPose = llResult.getBotpose_MT2(); + + final double METERS_TO_INCHES = 39.3701; + final double FIELD_LENGTH_METERS = 3.6576; + final double HALF_FIELD = FIELD_LENGTH_METERS / 2.0; + + double x_m = botPose.getPosition().x; + double y_m = botPose.getPosition().y; + + // Shift origin from field center → bottom-left + double pedroX_m = x_m; + double pedroY_m = y_m + HALF_FIELD; + + // Convert to inches + double pedroX_in = pedroX_m * METERS_TO_INCHES; + double pedroY_in = pedroY_m * METERS_TO_INCHES; + + // Optional: flip Y if Limelight’s +Y points opposite Pedro’s + //pedroY_in = (FIELD_LENGTH_METERS - pedroY_m) * METERS_TO_INCHES; + + // Offset correction (if Limelight not centered) + double LL_OFFSET_X = 0.0; + double LL_OFFSET_Y = 0.0; + pedroX_in += LL_OFFSET_X; + pedroY_in += LL_OFFSET_Y; + + telemetry.addData("Pedro X (in)", pedroX_in); + telemetry.addData("Pedro Y (in)", pedroY_in); + telemetry.addData("Heading (deg)", orientations.getYaw()); + } else { + telemetry.addData("Tag Pose", "No target found"); + } + } + +} \ No newline at end of file From 21a1d47c94f5dcb4a81d10c3046cc07027793a61 Mon Sep 17 00:00:00 2001 From: jasonbottino Date: Tue, 11 Nov 2025 20:38:31 -0500 Subject: [PATCH 4/8] refactor: Update IMU orientation and enhance Limelight telemetry This commit updates the IMU's `RevHubOrientationOnRobot` configuration to match a new physical mounting orientation. It also enhances the `PoseTesting` OpMode to display the raw X and Y position data from the Limelight's `botpose_MT2` (MegaTag2) pipeline. ### `PoseTesting.java` - **IMU Orientation:** The IMU initialization has been updated: - `LogoFacingDirection` changed from `RIGHT` to `BACKWARD`. - `UsbFacingDirection` changed from `UP` to `RIGHT`. - **Limelight Telemetry:** Added telemetry output to display the raw X and Y position (in meters) from the Limelight's MegaTag2 `botpose`. --- .../ftc/teamcode/vision/limelight/PoseTesting.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java index 8d9eb1e5296f..9dc4d9a79eda 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java @@ -24,8 +24,8 @@ public void init(){ imu.initialize( new IMU.Parameters( new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, - RevHubOrientationOnRobot.UsbFacingDirection.UP + RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.RIGHT ) ) ); @@ -71,6 +71,9 @@ public void loop() { telemetry.addData("Pedro X (in)", pedroX_in); telemetry.addData("Pedro Y (in)", pedroY_in); telemetry.addData("Heading (deg)", orientations.getYaw()); + + telemetry.addData("Limelight X (meters)", llResult.getBotpose_MT2().getPosition().x); + telemetry.addData("Limelight Y (meters)", llResult.getBotpose_MT2().getPosition().y); } else { telemetry.addData("Tag Pose", "No target found"); } From 5279cad72ad7b71ae79124d42f366752301a200e Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Mon, 24 Nov 2025 18:04:43 -0500 Subject: [PATCH 5/8] built in conversion --- .../vision/limelight/PoseTesting.java | 36 ++++--------------- 1 file changed, 7 insertions(+), 29 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java index 9dc4d9a79eda..b9983123422d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.vision.limelight; +import com.pedropathing.ftc.FTCCoordinates; +import com.pedropathing.geometry.PedroCoordinates; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.bosch.BHI260IMU; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -43,37 +46,12 @@ public void loop() { if (llResult != null && llResult.isValid()) { Pose3D botPose = llResult.getBotpose_MT2(); + Pose pedroPose = new Pose(botPose.getPosition().x, botPose.getPosition().y, botPose.getOrientation().getYaw(), FTCCoordinates.INSTANCE).getAsCoordinateSystem(PedroCoordinates.INSTANCE); - final double METERS_TO_INCHES = 39.3701; - final double FIELD_LENGTH_METERS = 3.6576; - final double HALF_FIELD = FIELD_LENGTH_METERS / 2.0; + telemetry.addData("Tag Pose X", pedroPose.getX()); + telemetry.addData("Tag Pose Y", pedroPose.getY()); + telemetry.addData("Tag Pose Heading", pedroPose.getHeading()); - double x_m = botPose.getPosition().x; - double y_m = botPose.getPosition().y; - - // Shift origin from field center → bottom-left - double pedroX_m = x_m; - double pedroY_m = y_m + HALF_FIELD; - - // Convert to inches - double pedroX_in = pedroX_m * METERS_TO_INCHES; - double pedroY_in = pedroY_m * METERS_TO_INCHES; - - // Optional: flip Y if Limelight’s +Y points opposite Pedro’s - //pedroY_in = (FIELD_LENGTH_METERS - pedroY_m) * METERS_TO_INCHES; - - // Offset correction (if Limelight not centered) - double LL_OFFSET_X = 0.0; - double LL_OFFSET_Y = 0.0; - pedroX_in += LL_OFFSET_X; - pedroY_in += LL_OFFSET_Y; - - telemetry.addData("Pedro X (in)", pedroX_in); - telemetry.addData("Pedro Y (in)", pedroY_in); - telemetry.addData("Heading (deg)", orientations.getYaw()); - - telemetry.addData("Limelight X (meters)", llResult.getBotpose_MT2().getPosition().x); - telemetry.addData("Limelight Y (meters)", llResult.getBotpose_MT2().getPosition().y); } else { telemetry.addData("Tag Pose", "No target found"); } From edd967f5f2e7024ca94fea3750b4d61363679e90 Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Sat, 29 Nov 2025 16:58:48 -0500 Subject: [PATCH 6/8] Ball Detection --- .../vision/opencv/BallDetectionPipeline.java | 141 ++++++++++++++++++ .../vision/opencv/BallDetectionTest.java | 88 +++++++++++ 2 files changed, 229 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java new file mode 100644 index 000000000000..4695041aa76d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java @@ -0,0 +1,141 @@ +package org.firstinspires.ftc.teamcode.vision.opencv; + +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvPipeline; + +import java.util.ArrayList; +import java.util.List; + +public class BallDetectionPipeline extends OpenCvPipeline { + + public static class BallResult { + public String color; + public double area; + public double radius; + public Point center; + public double distanceFocal; + public double distanceRegression; + } + + private final double realBallDiameterCM = 12.7; + private Double focalLengthPX = null; + private final double knownDistanceCM = 45; + private final double minArea = 300; + + public List detections = new ArrayList<>(); + + private final Scalar P_L = new Scalar(120, 43, 45); + private final Scalar P_U = new Scalar(179, 255, 255); + + private final Scalar G_L = new Scalar(30, 75, 93); + private final Scalar G_U = new Scalar(88, 255, 255); + + @Override + public Mat processFrame(Mat frame) { + + detections.clear(); + + Mat resized = new Mat(); + Imgproc.resize(frame, resized, new Size(600, (600.0/frame.cols())*frame.rows())); + + Imgproc.GaussianBlur(resized, resized, new Size(11, 11), 0); + + Mat hsv = new Mat(); + Imgproc.cvtColor(resized, hsv, Imgproc.COLOR_BGR2HSV); + + Mat maskPurple = new Mat(); + Core.inRange(hsv, P_L, P_U, maskPurple); + + Imgproc.erode(maskPurple, maskPurple, new Mat(), new Point(-1,-1), 2); + Imgproc.dilate(maskPurple, maskPurple, new Mat(), new Point(-1,-1), 2); + + detectColor(maskPurple, resized, "Purple"); + + Mat maskGreenHSV = new Mat(); + Core.inRange(hsv, G_L, G_U, maskGreenHSV); + + Mat lab = new Mat(); + Imgproc.cvtColor(resized, lab, Imgproc.COLOR_BGR2Lab); + List labChannels = new ArrayList<>(); + Core.split(lab, labChannels); + Mat A = labChannels.get(1); + + Mat maskLAB = new Mat(); + Imgproc.threshold(A, maskLAB, 135, 255, Imgproc.THRESH_BINARY_INV); + + Mat maskGreen = new Mat(); + Core.bitwise_and(maskGreenHSV, maskLAB, maskGreen); + + Imgproc.morphologyEx(maskGreen, maskGreen, Imgproc.MORPH_OPEN, + Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5))); + + Imgproc.morphologyEx(maskGreen, maskGreen, Imgproc.MORPH_CLOSE, + Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5))); + + detectColor(maskGreen, resized, "Green"); + + return resized; + } + + private void detectColor(Mat mask, Mat display, String colorName) { + + List contours = new ArrayList<>(); + Mat hierarchy = new Mat(); + Imgproc.findContours(mask, contours, hierarchy, + Imgproc.RETR_CCOMP, Imgproc.CHAIN_APPROX_SIMPLE); + + if (hierarchy.empty()) return; + + for (int i = 0; i < contours.size(); i++) { + double parent = hierarchy.get(0, i)[3]; + if (parent != -1) continue; + double area = Imgproc.contourArea(contours.get(i)); + if (area < minArea) continue; + + Point center = new Point(); + float[] radiusArr = new float[1]; + Imgproc.minEnclosingCircle(new MatOfPoint2f(contours.get(i).toArray()), center, radiusArr); + + double radius = radiusArr[0]; + if (radius < 10) continue; + + Scalar color = colorName.equals("Purple") ? + new Scalar(255, 0, 255) : new Scalar(0, 255, 0); + + Imgproc.circle(display, center, (int) radius, color, 2); + + BallResult result = new BallResult(); + result.color = colorName; + result.area = area; + result.center = center; + result.radius = radius; + + if (focalLengthPX == null) + focalLengthPX = calibrateFocalLength(radius); + + result.distanceFocal = computeDistanceFocal(radius); + + double areaNorm = (area / 10000.0); + result.distanceRegression = + (-0.0732667 * Math.pow(areaNorm, 3)) + + (0.402269 * Math.pow(areaNorm, 2)) + - (0.807345 * areaNorm) + + 0.984157; + + detections.add(result); + } + } + + private double calibrateFocalLength(double pixelRadius) { + double pixelDiameter = 2 * pixelRadius; + return (pixelDiameter * knownDistanceCM) / realBallDiameterCM; + } + + private double computeDistanceFocal(double pixelRadius) { + if (focalLengthPX == null || pixelRadius <= 0) + return -1; + double pixelDiameter = 2 * pixelRadius; + return (realBallDiameterCM * focalLengthPX) / pixelDiameter; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionTest.java new file mode 100644 index 000000000000..f7cf4442ad39 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionTest.java @@ -0,0 +1,88 @@ +package org.firstinspires.ftc.teamcode.vision.opencv; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; +import org.openftc.easyopencv.OpenCvCameraRotation; +import java.util.Locale; + +@TeleOp(name = "Ball Detection Test", group = "Vision") +public class BallDetectionTest extends LinearOpMode { + + private OpenCvCamera webcam; + private BallDetectionPipeline pipeline; + + @Override + public void runOpMode() throws InterruptedException { + + telemetry.addLine("Initializing camera..."); + telemetry.update(); + + int cameraMonitorViewId = hardwareMap + .appContext + .getResources() + .getIdentifier("cameraMonitorViewId", "id", + hardwareMap.appContext.getPackageName()); + + webcam = OpenCvCameraFactory.getInstance().createWebcam( + hardwareMap.get(WebcamName.class, "Webcam 1"), cameraMonitorViewId + ); + + pipeline = new BallDetectionPipeline(); + webcam.setPipeline(pipeline); + + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + webcam.startStreaming(640, 480, OpenCvCameraRotation.UPRIGHT); + } + + @Override + public void onError(int errorCode) { + telemetry.addData("CameraError", errorCode); + telemetry.update(); + } + }); + + telemetry.addLine("Camera ready."); + telemetry.addLine("Press ▶ to start detecting."); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) { + + BallDetectionPipeline.BallResult[] balls = pipeline.detections.toArray(new BallDetectionPipeline.BallResult[0]); + + if (balls.length > 0) { + telemetry.addData("Ball Count", balls.length); + for (int i = 0; i < balls.length; i++) { + BallDetectionPipeline.BallResult ball = balls[i]; + telemetry.addLine("=== Ball " + (i + 1) + " ==="); + telemetry.addData("Color", ball.color); + telemetry.addData("Radius (px)", String.format(Locale.US, "%.2f", ball.radius)); + telemetry.addData("Area (px)", String.format(Locale.US, "%.2f", ball.area)); + + if (ball.center != null) { + telemetry.addData("Center", String.format(Locale.US, "(%.1f, %.1f)", ball.center.x, ball.center.y)); + } else { + telemetry.addData("Center", "(null)"); + } + + telemetry.addData("Distance (focal)", String.format(Locale.US, "%.2f cm", ball.distanceFocal)); + telemetry.addData("Distance (regression)", String.format(Locale.US, "%.2f m", ball.distanceRegression)); + } + } else { + telemetry.addLine("No ball detected."); + } + + telemetry.update(); + + sleep(50); + } + + webcam.closeCameraDevice(); + } +} From 46e991c88d5346ca142cd1a8b8b38639019d211b Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Sat, 20 Dec 2025 12:37:35 -0500 Subject: [PATCH 7/8] Units --- .../ftc/teamcode/vision/opencv/BallDetectionPipeline.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java index 4695041aa76d..9eb8dc2267c3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/opencv/BallDetectionPipeline.java @@ -14,8 +14,8 @@ public static class BallResult { public double area; public double radius; public Point center; - public double distanceFocal; - public double distanceRegression; + public double distanceFocal; //cm + public double distanceRegression; //m } private final double realBallDiameterCM = 12.7; @@ -138,4 +138,4 @@ private double computeDistanceFocal(double pixelRadius) { double pixelDiameter = 2 * pixelRadius; return (realBallDiameterCM * focalLengthPX) / pixelDiameter; } -} +} \ No newline at end of file From 2ddfa58ab8d2b5a1ab55f6179b268cd6a6ec2160 Mon Sep 17 00:00:00 2001 From: AddyVeerendra Date: Sat, 27 Dec 2025 18:10:19 -0500 Subject: [PATCH 8/8] Create TurretFarZoneAlign.java --- .../vision/limelight/TurretFarZoneAlign.java | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/TurretFarZoneAlign.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/TurretFarZoneAlign.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/TurretFarZoneAlign.java new file mode 100644 index 000000000000..3d4962046d6e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/TurretFarZoneAlign.java @@ -0,0 +1,71 @@ +package org.firstinspires.ftc.teamcode.vision.limelight; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.pedropathing.ftc.FTCCoordinates; +import com.pedropathing.geometry.PedroCoordinates; +import com.pedropathing.geometry.Pose; +import com.qualcomm.hardware.bosch.BHI260IMU; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.Turret; + +public class TurretFarZoneAlign { + private Limelight3A limelight; + private IMU imu; + private Turret turret; + + public void init(){ + limelight = hardwareMap.get(Limelight3A.class, "Limelight"); + limelight.pipelineSwitch(0); + imu = hardwareMap.get(BHI260IMU.class, "imu"); + imu.initialize( + new IMU.Parameters( + new RevHubOrientationOnRobot( + RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.RIGHT + ) + ) + ); + imu.resetYaw(); + + turret = new Turret(); + turret.init(hardwareMap, "turretMotor", DcMotorSimple.Direction.FORWARD); + } + + public void start(){ + limelight.start(); + } + + public void loop(){ + YawPitchRollAngles orientations = imu.getRobotYawPitchRollAngles(); + limelight.updateRobotOrientation(orientations.getYaw()); + LLResult llResult = limelight.getLatestResult(); + + if (llResult != null && llResult.isValid()) { + Pose3D botPose = llResult.getBotpose_MT2(); + Pose pedroPose = new Pose(botPose.getPosition().x, botPose.getPosition().y, botPose.getOrientation().getYaw(), FTCCoordinates.INSTANCE).getAsCoordinateSystem(PedroCoordinates.INSTANCE); + + telemetry.addData("Tag Pose X", pedroPose.getX()); + telemetry.addData("Tag Pose Y", pedroPose.getY()); + telemetry.addData("Tag Pose Heading", pedroPose.getHeading()); + + } else { + telemetry.addData("Tag Pose", "No target found"); + } + + double currentAngle = turret.getCurrentAngle(); + + //ts does not work rn + double targetAngle = 10; + turret.setAngle(targetAngle); + + } +}