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"); + } + } +} 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..7c43d73608f6 --- /dev/null +++ b/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"); + } + + } +} 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..b9983123422d --- /dev/null +++ b/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"); + } + } + +} \ No newline at end of file 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); + + } +} 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..9eb8dc2267c3 --- /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; //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 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; + } +} \ No newline at end of file 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(); + } +}