Pretty please [BALL DETECTION PIPELINE]

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");
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/EstimateDistance.java

@@ -0,0 +1,66 @@
+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 = 13.5; //height of limelight in inches
+    private static double h2 = 26.0;  //height of target in inches
+    private static double a1 = 33.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.BACKWARD,
+                                RevHubOrientationOnRobot.UsbFacingDirection.RIGHT
+                        )
+                )
+        );
+        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("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");
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/limelight/PoseTesting.java

@@ -0,0 +1,60 @@
+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;
+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.BACKWARD,
+                                RevHubOrientationOnRobot.UsbFacingDirection.RIGHT
+                        )
+                )
+        );
+        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();
+            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");
+        }
+    }
+
+}

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);
+
+    }
+}

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; //cm
+        public double distanceRegression; //m
+    }
+
+    private final double realBallDiameterCM = 12.7;
+    private Double focalLengthPX = null;
+    private final double knownDistanceCM = 45;
+    private final double minArea = 300;
+
+    public List<BallResult> 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<Mat> 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<MatOfPoint> 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;
+    }
+}