From fda9a113c5f2bfad4f49d5e47bab3337513b645c Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 12 Sep 2025 20:18:06 -0700 Subject: [PATCH 01/19] VISION WORKS!!! --- .../DummyClasses/WebcamInitNotes.java | 106 ++-- .../Modules/AprilTagCustomDatabase.java | 29 ++ .../ftc/teamcode/Modules/Camera.java | 485 ++---------------- .../ftc/teamcode/Modules/CameraHardware.java | 77 --- .../ftc/teamcode/Modules/CameraPipeline.java | 254 +++++++++ .../ftc/teamcode/Modules/Drivetrain.java | 3 +- .../org/firstinspires/ftc/teamcode/Robot.java | 1 + .../ftc/teamcode/Tuning/SlidesPIDFTuner.java | 4 +- .../ftc/teamcode/Tuning/TestVision.java | 45 ++ 9 files changed, 428 insertions(+), 576 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java index e2ad3f297782..9f83c2eaea99 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DummyClasses/WebcamInitNotes.java @@ -1,53 +1,53 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; - -@Autonomous -public class WebcamInitNotes extends OpMode -{ - - // why does Android studio have a spell checker - // webcamdude is the webcam. I'm not naming it anything else - OpenCvWebcam webcamdude = null; - - @Override // This stuff happens when you click the init button - public void init() - { - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - // Acquire the camera ID - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); - //set the cam name and id to the webcam. - webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - //set webcam Pipeline - webcamdude.setPipeline(new WebcamPipelineNotes()); - - //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. - webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); - } - - @Override - public void onError(int errorCode) { - telemetry.addLine("Webcam not working"); - } - }); - } - - @Override // This stuff happens when you click the play button - public void loop() - { - telemetry.addLine("Why did you click this lmao"); - } -} - +//package org.firstinspires.ftc.teamcode; +// +//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +//import com.qualcomm.robotcore.eventloop.opmode.OpMode; +// +//import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +////import org.firstinspires.ftc.teamcode.VisionPipelines.WebcamPipelineNotes; +//import org.openftc.easyopencv.OpenCvCamera; +//import org.openftc.easyopencv.OpenCvCameraFactory; +//import org.openftc.easyopencv.OpenCvCameraRotation; +//import org.openftc.easyopencv.OpenCvWebcam; +// +//@Autonomous +//public class WebcamInitNotes extends OpMode +//{ +// +// // why does Android studio have a spell checker +// // webcamdude is the webcam. I'm not naming it anything else +// OpenCvWebcam webcamdude = null; +// +// @Override // This stuff happens when you click the init button +// public void init() +// { +// WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); +// // Acquire the camera ID +// int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName() ); +// //set the cam name and id to the webcam. +// webcamdude = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); +// +// //set webcam Pipeline +// webcamdude.setPipeline(new WebcamPipelineNotes()); +// +// //You're creating an instance of the AsyncCameraOpenListener class. The class contains the two methods, onOpened and onError, which you are overriding with your code. That instance is passed to the openCameraDeviceAsync method as a parameter. +// webcamdude.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { +// @Override +// public void onOpened() { +// webcamdude.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); +// } +// +// @Override +// public void onError(int errorCode) { +// telemetry.addLine("Webcam not working"); +// } +// }); +// } +// +// @Override // This stuff happens when you click the play button +// public void loop() +// { +// telemetry.addLine("Why did you click this lmao"); +// } +//} +// diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java new file mode 100644 index 000000000000..9058e43f83ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; + +public class AprilTagCustomDatabase { + public static AprilTagLibrary getLibrary() { + return new AprilTagLibrary.Builder() + .addTag( + 20, + "Blue Goal", + 6.5, + //new VectorF(), + DistanceUnit.INCH + //Quaternion.identityQuaternion() + ) + .addTag( + 24, + "Red Goal", + 6.5, + //new VectorF(), + DistanceUnit.INCH + //Quaternion.identityQuaternion() + ) + .build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 29e8dcd68635..1bb8edb40dae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,29 +1,24 @@ package org.firstinspires.ftc.teamcode.Modules; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; -//opencv for vision stuff -import org.opencv.calib3d.Calib3d; -import org.opencv.core.*; -import org.opencv.imgproc.Imgproc; -import org.openftc.apriltag.AprilTagDetection; -import org.openftc.apriltag.AprilTagDetectorJNI; -import org.openftc.easyopencv.OpenCvPipeline; -import java.util.ArrayList; -import java.util.List; +// vision stuff +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 org.openftc.easyopencv.OpenCvWebcam; /* - * This is the pipeline which will be processing the cameras feed - * It should be able to: - * - detect and recognize april tags - * - detect and locate (if I will we able to) artifacts - * - get the current state of the ramp + * We have to have two classes because java does not support multiple inheritance + * + * This class is the hardware aspect of the camera, the camera class will be used for the pipeline (what processes the feed) * - * welp lets see how well I make this */ -// omg this is going to need so much cleaning up -public class Camera extends OpenCvPipeline { +public class Camera { /////////////////////////////////////////////// //// ///// @@ -31,71 +26,8 @@ public class Camera extends OpenCvPipeline { //// ///// ////////////////////////////////////////////// - // -------------- ARTIFACTS -------------- // - - /* - Purple lower: (0, 136.0, 120.4) - Purple upper: (255, 255, 255) - */ - public Scalar lower = new Scalar(0, 136.0, 120.4); - public Scalar upper = new Scalar(255, 255, 255); - - - private final Mat ycrcbmat = new Mat(); - private final Mat binaryMat = new Mat(); - private final Mat maskedInputMat = new Mat(); - private final Mat grayscaleMat = new Mat(); - - // Contour Vars - - List contours = new ArrayList<>(); - public double lowerContourThreshold = 0; - public double upperContourThreshold = 300; - public Scalar contourColors = new Scalar(0,255,0); - public double noiseSensitivity = 153; - public int contourSize = 1; - - private final Mat edgeDetectorFrame = new Mat(); - private int onlyContours = 1; - - public static double currentLargest = 0; - - Mat activeMat; - Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); - int index = 0; - double contourArea; - double largestArea = 0; - - // -------------- APRIL TAGS ------------- // - - private long nativeApriltagPtr; - private Mat grey = new Mat(); - private ArrayList detections = new ArrayList<>(); - - - private ArrayList detectionsUpdate = new ArrayList<>(); - private final Object detectionsUpdateSync = new Object(); - - Mat cameraMatrix; - - Scalar blue = new Scalar(7,197,235,255); - Scalar red = new Scalar(255,0,0,255); - Scalar green = new Scalar(0,255,0,255); - Scalar white = new Scalar(255,255,255,255); - - double fx; - double fy; - double cx; - double cy; - - // UNITS ARE METERS - double tagsize; - double tagsizeX; - double tagsizeY; - - private float decimation; - private boolean needToSetDecimation; - private final Object decimationSync = new Object(); + // this used to be called webcamdude, I'm looking at you Aaron (actually Aroon) + OpenCvWebcam webcam = null; /////////////////////////////////////////////// //// ///// @@ -103,373 +35,42 @@ public class Camera extends OpenCvPipeline { //// ///// ////////////////////////////////////////////// - public Camera(double tagsize, double fx, double fy, double cx, double cy) { - this.tagsize = tagsize; - this.tagsizeX = tagsize; - this.tagsizeY = tagsize; - this.fx = fx; - this.fy = fy; - this.cx = cx; - this.cy = cy; - - constructMatrix(); - - // Allocate a native context object. See the corresponding deletion in the finalizer - nativeApriltagPtr = AprilTagDetectorJNI.createApriltagDetector(AprilTagDetectorJNI.TagFamily.TAG_36h11.string, 3, 3); - - } - - @Override - public Mat processFrame(Mat input) { - - // Convert to greyscale - Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY); - - synchronized (decimationSync) - { - if(needToSetDecimation) - { - AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation); - needToSetDecimation = false; - } - } - - // Run AprilTag - detections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy); - - synchronized (detectionsUpdateSync) - { - detectionsUpdate = detections; - } - - // For fun, use OpenCV to draw 6DOF markers on the image. We actually recompute the pose using - // OpenCV because I haven't yet figured out how to re-use AprilTag's pose in OpenCV. - for(AprilTagDetection detection : detections) - { - Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY); - drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix); - draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix); - } - - //Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" - Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); - //Order is source, lowerBound, upperbound, dst. - Core.inRange(ycrcbmat, lower, upper, binaryMat); - /* - * Release the reusable Mat so that old data doesn't - * affect the next step in the current processing - */ - maskedInputMat.release(); - - //Order: src1, src2, dst, mask. - Core.bitwise_and(input, input, maskedInputMat, binaryMat); - - // now the masked input mat is the filtered image with colors and shit. - // Sorry Alok I ripped u off ;P - - // filter maskedinputmat to grey. - Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); + public Camera(HardwareMap hardwareMap) { - // Order: input image, output edges(Array), lowerthreshold, upperthreshold - Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); + // get webcam name + WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - contours.clear(); - //Order : input image, the contours from output, hierarchy, mode, and method + // get camera ID + // this is to display the camera feed on the robot controller screen + int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + // set webcam to the camera name and id + // can omit cameraMonitorViewId if you don't want to display the live feed + webcam = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; + // set webcam pipeline + // the arguments passed in are the camera intrinsics in pixels for the Logitech HD Webcam C270 + webcam.setPipeline(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap)); - Rect[] boundRect = new Rect[contours.size()]; - //Might be useful later, idk so I am going to leave this here - //Point[] centers = new Point[contours.size()]; - - - for (int i = 0; i < contours.size(); i++) { - contoursPoly[i] = new MatOfPoint2f(); - Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); - boundRect[i] = Imgproc.boundingRect(contours.get(i)); - //centers[i] = new Point(); - } - - - List contoursPolyList = new ArrayList<>(contoursPoly.length); - for (MatOfPoint2f poly : contoursPoly) { - //Just to make sure that no problems really come up. - if(poly == null) - { - break; + // this is the asyncrounous camera open listener class + // it has the methods : onOpened and onError + // we are overriding these methods + webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + // starts the webcam when click play (?) + @Override + public void onOpened() { + webcam.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); +// telemetry.addLine("Webcam started streaming..."); } - contoursPolyList.add(new MatOfPoint(poly.toArray())); - } - - - // now this can be removed during the meet. - if (onlyContours == 1) - { - activeMat = maskedInputMat; - } - else if (onlyContours == 2) - { - activeMat = emptyMat; - } - else { - activeMat = input; - } - - largestArea = 0; - - for (int i = 0; i < contours.size(); i++) { - - MatOfPoint contour = contours.get(i); - - contourArea = Imgproc.contourArea(contour); - // A simple filter I cam up with ;P - if (contourArea < noiseSensitivity) - { - continue; - } + // called if the camera could not be opened + @Override + public void onError(int errorCode) { +// telemetry.addLine("Welp you screwed up."); +// telemetry.addLine("Or maybe I\'m stupid."); +// telemetry.addLine("Or maybe there\'s just an error with the camera."); - if (contourArea > largestArea) - { - largestArea = contourArea; - index = i; } - - } - - currentLargest = largestArea; - - - // pretty sure that we actually don't need this for the meet either, - // but is good for demonstration purposes and debugging - if (contours.size() != 0 && largestArea != 0) - { - // Adding Text3 - Imgproc.putText ( - activeMat, // Matrix obj of the image - largestArea + "", // Text to be added - new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point - Imgproc.FONT_HERSHEY_SIMPLEX , // front face - 1, // front scale - contourColors, // Scalar object for color - 2 // Thickness - ); - Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); - Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); - } - - return activeMat; - } - - // I think we could have just returned a Mat frame and have the FSM and Auton do the computing, but that is like - // not smart - public static double getLargestSize() - { - return currentLargest; - } - - // boilerplate code if there is a better way to do this, please tell me - @Override - public void onViewportTapped() - { - if (onlyContours == 1 || onlyContours == 2) - onlyContours++; - else - onlyContours = 1; - } - - @Override - public void finalize() - { - // Might be null if createApriltagDetector() threw an exception - if(nativeApriltagPtr != 0) - { - // Delete the native context we created in the constructor - AprilTagDetectorJNI.releaseApriltagDetector(nativeApriltagPtr); - nativeApriltagPtr = 0; - } - else - { - System.out.println("AprilTagDetectionPipeline.finalize(): nativeApriltagPtr was NULL"); - } - } - - public void setDecimation(float decimation) - { - synchronized (decimationSync) - { - this.decimation = decimation; - needToSetDecimation = true; - } - } - - public ArrayList getLatestDetections() - { - return detections; - } - - public ArrayList getDetectionsUpdate() - { - synchronized (detectionsUpdateSync) - { - ArrayList ret = detectionsUpdate; - detectionsUpdate = null; - return ret; - } - } - - void constructMatrix() - { - // Construct the camera matrix. - // - // -- -- - // | fx 0 cx | - // | 0 fy cy | - // | 0 0 1 | - // -- -- - // - - cameraMatrix = new Mat(3,3, CvType.CV_32FC1); - - cameraMatrix.put(0,0, fx); - cameraMatrix.put(0,1,0); - cameraMatrix.put(0,2, cx); - - cameraMatrix.put(1,0,0); - cameraMatrix.put(1,1,fy); - cameraMatrix.put(1,2,cy); - - cameraMatrix.put(2, 0, 0); - cameraMatrix.put(2,1,0); - cameraMatrix.put(2,2,1); + }); } - - /** - * Draw a 3D axis marker on a detection. (Similar to what Vuforia does) - * - * @param buf the RGB buffer on which to draw the marker - * @param length the length of each of the marker 'poles' - * @param rvec the rotation vector of the detection - * @param tvec the translation vector of the detection - * @param cameraMatrix the camera matrix used when finding the detection - */ - void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) - { - // The points in 3D space we wish to project onto the 2D image plane. - // The origin of the coordinate space is assumed to be in the center of the detection. - MatOfPoint3f axis = new MatOfPoint3f( - new Point3(0,0,0), - new Point3(length,0,0), - new Point3(0,length,0), - new Point3(0,0,-length) - ); - - // Project those points - MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); - Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); - Point[] projectedPoints = matProjectedPoints.toArray(); - - // Draw the marker! - Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness); - Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness); - Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness); - - Imgproc.circle(buf, projectedPoints[0], thickness, white, -1); - } - - void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix) - { - //axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0], - // [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ]) - - // The points in 3D space we wish to project onto the 2D image plane. - // The origin of the coordinate space is assumed to be in the center of the detection. - MatOfPoint3f axis = new MatOfPoint3f( - new Point3(-tagWidth/2, tagHeight/2,0), - new Point3( tagWidth/2, tagHeight/2,0), - new Point3( tagWidth/2,-tagHeight/2,0), - new Point3(-tagWidth/2,-tagHeight/2,0), - new Point3(-tagWidth/2, tagHeight/2,-length), - new Point3( tagWidth/2, tagHeight/2,-length), - new Point3( tagWidth/2,-tagHeight/2,-length), - new Point3(-tagWidth/2,-tagHeight/2,-length)); - - // Project those points - MatOfPoint2f matProjectedPoints = new MatOfPoint2f(); - Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints); - Point[] projectedPoints = matProjectedPoints.toArray(); - - // Pillars - for(int i = 0; i < 4; i++) - { - Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness); - } - - // Base lines - //Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness); - //Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness); - //Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness); - //Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness); - - // Top lines - Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness); - Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness); - Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness); - Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness); - } - - /** - * Extracts 6DOF pose from a trapezoid, using a camera intrinsics matrix and the - * original size of the tag. - * - * @param points the points which form the trapezoid - * @param cameraMatrix the camera intrinsics matrix - * @param tagsizeX the original width of the tag - * @param tagsizeY the original height of the tag - * @return the 6DOF pose of the camera relative to the tag - */ - Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY) - { - // The actual 2d points of the tag detected in the image - MatOfPoint2f points2d = new MatOfPoint2f(points); - - // The 3d points of the tag in an 'ideal projection' - Point3[] arrayPoints3d = new Point3[4]; - arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0); - arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0); - arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0); - arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0); - MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d); - - // Using this information, actually solve for pose - Pose pose = new Pose(); - Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false); - - return pose; - } - - /* - * A simple container to hold both rotation and translation - * vectors, which together form a 6DOF pose. - */ - class Pose - { - Mat rvec; - Mat tvec; - - public Pose() - { - rvec = new Mat(); - tvec = new Mat(); - } - - public Pose(Mat rvec, Mat tvec) - { - this.rvec = rvec; - this.tvec = tvec; - } - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java deleted file mode 100644 index 9248622829a7..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraHardware.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.firstinspires.ftc.teamcode.Modules; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.HardwareMap; - -// vision stuff -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 org.openftc.easyopencv.OpenCvWebcam; - -import org.firstinspires.ftc.teamcode.Modules.Camera; - -/* - * We have to have two classes because java does not support multiple inheritance - * - * This class is the hardware aspect of the camera, the camera class will be used for the pipeline (what processes the feed) - * - */ -@Autonomous -public class CameraHardware { - - /////////////////////////////////////////////// - //// ///// - //// VARIABLES ///// - //// ///// - ////////////////////////////////////////////// - - // this used to be called webcamdude, I'm looking at you Aaron - OpenCvWebcam webcam = null; - - /////////////////////////////////////////////// - //// ///// - //// FUNCTIONS ///// - //// ///// - ////////////////////////////////////////////// - - public CameraHardware(HardwareMap hardwareMap) { - - // get webcam name - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - - // get camera ID - // this is to display the camera feed on the robot controller screen - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - - // set webcam to the camera name and id - // can omit cameraMonitorViewId if you don't want to display the live feed - webcam = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - // set webcam pipeline - webcam.setPipeline(new Camera(0.047, 578.272, 578.272, 402.145, 221.506)); - - // this is the asyncrounous camera open listener class - // it has the methods : onOpened and onError - // we are overriding these methods - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - // starts the webcam when click play (?) - @Override - public void onOpened() { - webcam.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); -// telemetry.addLine("Webcam started streaming..."); - } - - // called if the camera could not be opened - @Override - public void onError(int errorCode) { -// telemetry.addLine("Welp you screwed up."); -// telemetry.addLine("Or maybe I\'m stupid."); -// telemetry.addLine("Or maybe there\'s just an error with the camera."); - - } - }); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java new file mode 100644 index 000000000000..8d3a982b590a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java @@ -0,0 +1,254 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import android.util.Size; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +//opencv for vision stuff +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.opencv.core.*; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvPipeline; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.VisionPortal; + +// idk why we need this +import java.util.ArrayList; +import java.util.List; + +/* + * This is the pipeline which will be processing the cameras feed + * It should be able to: + * - detect and recognize april tags + * - detect and locate (if I will we able to) artifacts + * - get the current state of the ramp + * + * welp lets see how well I make this + */ + +// omg this is going to need so much cleaning up +// https://www.youtube.com/watch?v=aWPIsqjMoDI&t=740s +// https://stackoverflow.com/questions/60867638/methods-for-detecting-a-known-shape-object-in-an-image-using-opencv +public class CameraPipeline extends OpenCvPipeline { + + /////////////////////////////////////////////// + //// ///// + //// VARIABLES ///// + //// ///// + ////////////////////////////////////////////// + + // -------------- APRIL TAGS ------------- // + + AprilTagProcessor tag_processor; + VisionPortal vision_portal; + List detections; + + // -------------- ARTIFACTS -------------- // + + /* + Purple lower: (0, 136.0, 120.4) + Purple upper: (255, 255, 255) + */ +// public Scalar lower = new Scalar(0, 136.0, 120.4); +// public Scalar upper = new Scalar(255, 255, 255); +// +// +// private final Mat ycrcbmat = new Mat(); +// private final Mat binaryMat = new Mat(); +// private final Mat maskedInputMat = new Mat(); +// private final Mat grayscaleMat = new Mat(); +// +// // Contour Vars +// +// List contours = new ArrayList<>(); +// public double lowerContourThreshold = 0; +// public double upperContourThreshold = 300; +// public Scalar contourColors = new Scalar(0,255,0); +// public double noiseSensitivity = 153; +// public int contourSize = 1; +// +// private final Mat edgeDetectorFrame = new Mat(); +// private int onlyContours = 1; +// +// public static double currentLargest = 0; +// +// Mat activeMat; +// Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); +// int index = 0; +// double contourArea; +// double largestArea = 0; + + + + /////////////////////////////////////////////// + //// ///// + //// FUNCTIONS ///// + //// ///// + ////////////////////////////////////////////// + + public CameraPipeline(double tagsize, double fx, double fy, double cx, double cy, HardwareMap hardwareMap) { + tag_processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagCustomDatabase.getLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .setDrawTagID(true) + .setDrawTagOutline(true) + .build(); + // can also be 640 and 488 + // there is also YUY2 + vision_portal = new VisionPortal.Builder() + .addProcessor(tag_processor) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setCameraResolution(new Size(640, 480)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + //.enableCameraMonitoring(true) + .setAutoStopLiveView(true) + .build(); + } + + @Override + public Mat processFrame(Mat input) { + + // --- April Tags --- // + + detections = tag_processor.getDetections(); + + // --- Artifacts --- // + +// // Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" +// // converts the image from the BGR color space to the YCrCB color space +// // the YCrCb color space has the channels [Luminance or Y, Red - Y, Blue - Y] +// Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); +// +// // Order is source, lowerBound, upperbound, dst. +// // filters the image to the lower and upper bounds +// Core.inRange(ycrcbmat, lower, upper, binaryMat); +// /* +// * Release the reusable Mat so that old data doesn't +// * affect the next step in the current processing +// */ +// maskedInputMat.release(); +// +// //Order: src1, src2, dst, mask. +// Core.bitwise_and(input, input, maskedInputMat, binaryMat); +// +// // filter maskedinputmat to grey. +// Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); +// +// // Order: input image, output edges(Array), lowerthreshold, upperthreshold +// Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); +// +// contours.clear(); +// +// //Order : input image, the contours from output, hierarchy, mode, and method +// Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); +// +// MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; +// +// Rect[] boundRect = new Rect[contours.size()]; +// //Might be useful later, idk so I am going to leave this here +// //Point[] centers = new Point[contours.size()]; +// +// +// for (int i = 0; i < contours.size(); i++) { +// contoursPoly[i] = new MatOfPoint2f(); +// Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); +// boundRect[i] = Imgproc.boundingRect(contours.get(i)); +// //centers[i] = new Point(); +// } +// +// +// List contoursPolyList = new ArrayList<>(contoursPoly.length); +// for (MatOfPoint2f poly : contoursPoly) { +// //Just to make sure that no problems really come up. +// if(poly == null) +// { +// break; +// } +// +// contoursPolyList.add(new MatOfPoint(poly.toArray())); +// } +// +// +// // now this can be removed during the meet. +// if (onlyContours == 1) +// { +// activeMat = maskedInputMat; +// } +// else if (onlyContours == 2) +// { +// activeMat = emptyMat; +// } +// else { +// activeMat = input; +// } +// +// largestArea = 0; +// +// for (int i = 0; i < contours.size(); i++) { +// +// MatOfPoint contour = contours.get(i); +// +// contourArea = Imgproc.contourArea(contour); +// // A simple filter I cam up with ;P +// if (contourArea < noiseSensitivity) +// { +// continue; +// } +// +// if (contourArea > largestArea) +// { +// largestArea = contourArea; +// index = i; +// } +// +// } +// +// currentLargest = largestArea; +// +// +// // pretty sure that we actually don't need this for the meet either, +// // but is good for demonstration purposes and debugging +// if (contours.size() != 0 && largestArea != 0) +// { +// // Adding Text3 +// Imgproc.putText ( +// activeMat, // Matrix obj of the image +// largestArea + "", // Text to be added +// new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point +// Imgproc.FONT_HERSHEY_SIMPLEX , // front face +// 1, // front scale +// contourColors, // Scalar object for color +// 2 // Thickness +// ); +// Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); +// Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); +// } +// +// return activeMat; + return input; + } + + // I think we could have just returned a Mat frame and have the FSM and Auton do the computing, but that is like + // not smart +// public static double getLargestSize() +// { +// return currentLargest; +// } +// +// // boilerplate code if there is a better way to do this, please tell me +// @Override +// public void onViewportTapped() +// { +// if (onlyContours == 1 || onlyContours == 2) +// onlyContours++; +// else +// onlyContours = 1; +// } + + public List getDetections() { + return detections; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index 7049813f49f9..d69cb433e7ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -14,8 +14,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.Modules.EditablePose2D; -import org.firstinspires.ftc.teamcode.Modules.OdometryLocalizer; + // ----- READY TO TRANSFER ----- // diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 045d73cc46ce..e92de83b1e36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; import org.firstinspires.ftc.teamcode.Modules.Slides; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java index 2c9163c99b17..3beb208d464f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/SlidesPIDFTuner.java @@ -49,9 +49,9 @@ public void runOpMode() throws InterruptedException { } slideController.setPID(kp, ki, kd); - currentPosition = robot.getSlides().getSlidePosition(); +// currentPosition = robot.getSlides().getSlidePosition(); - robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); + //robot.getSlides().setSlidePower(slideController.calculate(currentPosition, goal) + kf); t.addData("Current Position", currentPosition); t.addData("Goal Position", goal); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java new file mode 100644 index 000000000000..038789e696b8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.teamcode.Tuning; + +import android.util.Size; + +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.firstinspires.ftc.teamcode.Modules.AprilTagCustomDatabase; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +@TeleOp +public class TestVision extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + + AprilTagProcessor tag_processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagCustomDatabase.getLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .setDrawTagID(true) + .setDrawTagOutline(true) + .build(); + + // can also be 640 and 488 + // there is also YUY2 + VisionPortal vision_portal = new VisionPortal.Builder() + .addProcessor(tag_processor) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setCameraResolution(new Size(640, 480)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + .enableLiveView(true) + .setAutoStopLiveView(true) + .build(); + + waitForStart(); + while (!isStopRequested() && opModeIsActive()) { + if (!tag_processor.getDetections().isEmpty()) { + AprilTagDetection tag = tag_processor.getDetections().get(0); + } + } + } +} From ce5c813208dda5b544af7c0785e2ddd9a6dce4a6 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 12 Sep 2025 20:58:42 -0700 Subject: [PATCH 02/19] added some changes to camera and camerapipeline classes --- .../ftc/teamcode/Modules/Camera.java | 69 ++-- .../ftc/teamcode/Modules/CameraPipeline.java | 361 ++++++++---------- .../ftc/teamcode/Tuning/TestVision.java | 30 +- 3 files changed, 212 insertions(+), 248 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 1bb8edb40dae..352bf7913828 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,16 +1,24 @@ package org.firstinspires.ftc.teamcode.Modules; +import android.util.Size; + import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.HardwareMap; // vision stuff +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.openftc.easyopencv.OpenCvCamera; import org.openftc.easyopencv.OpenCvCameraFactory; import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvWebcam; +import java.util.List; + /* * We have to have two classes because java does not support multiple inheritance * @@ -29,48 +37,39 @@ public class Camera { // this used to be called webcamdude, I'm looking at you Aaron (actually Aroon) OpenCvWebcam webcam = null; + AprilTagProcessor tag_processor; + VisionPortal vision_portal; + List detections; + /////////////////////////////////////////////// //// ///// //// FUNCTIONS ///// //// ///// ////////////////////////////////////////////// - public Camera(HardwareMap hardwareMap) { - - // get webcam name - WebcamName webcamname = hardwareMap.get(WebcamName.class, "Webcam 1"); - - // get camera ID - // this is to display the camera feed on the robot controller screen - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + public Camera(CameraName cameraname) { - // set webcam to the camera name and id - // can omit cameraMonitorViewId if you don't want to display the live feed - webcam = OpenCvCameraFactory.getInstance().createWebcam(webcamname, cameraMonitorViewId); - - // set webcam pipeline - // the arguments passed in are the camera intrinsics in pixels for the Logitech HD Webcam C270 - webcam.setPipeline(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap)); - - // this is the asyncrounous camera open listener class - // it has the methods : onOpened and onError - // we are overriding these methods - webcam.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - // starts the webcam when click play (?) - @Override - public void onOpened() { - webcam.startStreaming(640, 360, OpenCvCameraRotation.UPRIGHT); -// telemetry.addLine("Webcam started streaming..."); - } - - // called if the camera could not be opened - @Override - public void onError(int errorCode) { -// telemetry.addLine("Welp you screwed up."); -// telemetry.addLine("Or maybe I\'m stupid."); -// telemetry.addLine("Or maybe there\'s just an error with the camera."); + tag_processor = new AprilTagProcessor.Builder() + .setTagLibrary(AprilTagCustomDatabase.getLibrary()) + .setDrawAxes(true) + .setDrawCubeProjection(true) + .setDrawTagID(true) + .setDrawTagOutline(true) + .build(); + // can also be 640 and 488 + // there is also YUY2 + vision_portal = new VisionPortal.Builder() + .addProcessor(tag_processor) + //.addProcessor(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap.get(WebcamName.class, "Webcam 1"))) + .setCamera(cameraname) + .setCameraResolution(new Size(640, 480)) + .setStreamFormat(VisionPortal.StreamFormat.MJPEG) + //.enableCameraMonitoring(true) + .setAutoStopLiveView(true) + .build(); + } - } - }); + public List getDetections() { + return detections; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java index 8d3a982b590a..362fb671abe2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java @@ -1,11 +1,14 @@ package org.firstinspires.ftc.teamcode.Modules; +import android.graphics.Canvas; import android.util.Size; import com.qualcomm.robotcore.hardware.HardwareMap; //opencv for vision stuff import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; @@ -30,7 +33,7 @@ // omg this is going to need so much cleaning up // https://www.youtube.com/watch?v=aWPIsqjMoDI&t=740s // https://stackoverflow.com/questions/60867638/methods-for-detecting-a-known-shape-object-in-an-image-using-opencv -public class CameraPipeline extends OpenCvPipeline { +public class CameraPipeline implements VisionProcessor { /////////////////////////////////////////////// //// ///// @@ -38,217 +41,189 @@ public class CameraPipeline extends OpenCvPipeline { //// ///// ////////////////////////////////////////////// - // -------------- APRIL TAGS ------------- // - - AprilTagProcessor tag_processor; - VisionPortal vision_portal; - List detections; - - // -------------- ARTIFACTS -------------- // - /* Purple lower: (0, 136.0, 120.4) Purple upper: (255, 255, 255) */ -// public Scalar lower = new Scalar(0, 136.0, 120.4); -// public Scalar upper = new Scalar(255, 255, 255); -// -// -// private final Mat ycrcbmat = new Mat(); -// private final Mat binaryMat = new Mat(); -// private final Mat maskedInputMat = new Mat(); -// private final Mat grayscaleMat = new Mat(); -// -// // Contour Vars -// -// List contours = new ArrayList<>(); -// public double lowerContourThreshold = 0; -// public double upperContourThreshold = 300; -// public Scalar contourColors = new Scalar(0,255,0); -// public double noiseSensitivity = 153; -// public int contourSize = 1; -// -// private final Mat edgeDetectorFrame = new Mat(); -// private int onlyContours = 1; -// -// public static double currentLargest = 0; -// -// Mat activeMat; -// Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); -// int index = 0; -// double contourArea; -// double largestArea = 0; + public Scalar lower = new Scalar(0, 136.0, 120.4); + public Scalar upper = new Scalar(255, 255, 255); + private final Mat ycrcbmat = new Mat(); + private final Mat binaryMat = new Mat(); + private final Mat maskedInputMat = new Mat(); + private final Mat grayscaleMat = new Mat(); - /////////////////////////////////////////////// + // Contour Vars + + List contours = new ArrayList<>(); + public double lowerContourThreshold = 0; + public double upperContourThreshold = 300; + public Scalar contourColors = new Scalar(0,255,0); + public double noiseSensitivity = 153; + public int contourSize = 1; + + private final Mat edgeDetectorFrame = new Mat(); + private int onlyContours = 1; + + public static double currentLargest = 0; + + Mat activeMat; + Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); + int index = 0; + double contourArea; + double largestArea = 0; + + ////////////////////////////////////////////// //// ///// //// FUNCTIONS ///// //// ///// ////////////////////////////////////////////// - public CameraPipeline(double tagsize, double fx, double fy, double cx, double cy, HardwareMap hardwareMap) { - tag_processor = new AprilTagProcessor.Builder() - .setTagLibrary(AprilTagCustomDatabase.getLibrary()) - .setDrawAxes(true) - .setDrawCubeProjection(true) - .setDrawTagID(true) - .setDrawTagOutline(true) - .build(); - // can also be 640 and 488 - // there is also YUY2 - vision_portal = new VisionPortal.Builder() - .addProcessor(tag_processor) - .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) - .setCameraResolution(new Size(640, 480)) - .setStreamFormat(VisionPortal.StreamFormat.MJPEG) - //.enableCameraMonitoring(true) - .setAutoStopLiveView(true) - .build(); + @Override + public void init(int a, int b, CameraCalibration c) { + + } + + public CameraPipeline(double tagsize, double fx, double fy, double cx, double cy, WebcamName camera) { + } @Override - public Mat processFrame(Mat input) { - - // --- April Tags --- // - - detections = tag_processor.getDetections(); - - // --- Artifacts --- // - -// // Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" -// // converts the image from the BGR color space to the YCrCB color space -// // the YCrCb color space has the channels [Luminance or Y, Red - Y, Blue - Y] -// Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); -// -// // Order is source, lowerBound, upperbound, dst. -// // filters the image to the lower and upper bounds -// Core.inRange(ycrcbmat, lower, upper, binaryMat); -// /* -// * Release the reusable Mat so that old data doesn't -// * affect the next step in the current processing -// */ -// maskedInputMat.release(); -// -// //Order: src1, src2, dst, mask. -// Core.bitwise_and(input, input, maskedInputMat, binaryMat); -// -// // filter maskedinputmat to grey. -// Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); -// -// // Order: input image, output edges(Array), lowerthreshold, upperthreshold -// Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); -// -// contours.clear(); -// -// //Order : input image, the contours from output, hierarchy, mode, and method -// Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); -// -// MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; -// -// Rect[] boundRect = new Rect[contours.size()]; -// //Might be useful later, idk so I am going to leave this here -// //Point[] centers = new Point[contours.size()]; -// -// -// for (int i = 0; i < contours.size(); i++) { -// contoursPoly[i] = new MatOfPoint2f(); -// Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); -// boundRect[i] = Imgproc.boundingRect(contours.get(i)); -// //centers[i] = new Point(); -// } -// -// -// List contoursPolyList = new ArrayList<>(contoursPoly.length); -// for (MatOfPoint2f poly : contoursPoly) { -// //Just to make sure that no problems really come up. -// if(poly == null) -// { -// break; -// } -// -// contoursPolyList.add(new MatOfPoint(poly.toArray())); -// } -// -// -// // now this can be removed during the meet. -// if (onlyContours == 1) -// { -// activeMat = maskedInputMat; -// } -// else if (onlyContours == 2) -// { -// activeMat = emptyMat; -// } -// else { -// activeMat = input; -// } -// -// largestArea = 0; -// -// for (int i = 0; i < contours.size(); i++) { -// -// MatOfPoint contour = contours.get(i); -// -// contourArea = Imgproc.contourArea(contour); -// // A simple filter I cam up with ;P -// if (contourArea < noiseSensitivity) -// { -// continue; -// } -// -// if (contourArea > largestArea) -// { -// largestArea = contourArea; -// index = i; -// } -// -// } -// -// currentLargest = largestArea; -// -// -// // pretty sure that we actually don't need this for the meet either, -// // but is good for demonstration purposes and debugging -// if (contours.size() != 0 && largestArea != 0) -// { -// // Adding Text3 -// Imgproc.putText ( -// activeMat, // Matrix obj of the image -// largestArea + "", // Text to be added -// new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point -// Imgproc.FONT_HERSHEY_SIMPLEX , // front face -// 1, // front scale -// contourColors, // Scalar object for color -// 2 // Thickness -// ); -// Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); -// Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); -// } -// -// return activeMat; - return input; + public Object processFrame(Mat input, long captureTimeNanoseconds) { + + // Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" + // converts the image from the BGR color space to the YCrCB color space + // the YCrCb color space has the channels [Luminance or Y, Red - Y, Blue - Y] + Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); + + // Order is source, lowerBound, upperbound, dst. + // filters the image to the lower and upper bounds + Core.inRange(ycrcbmat, lower, upper, binaryMat); + /* + * Release the reusable Mat so that old data doesn't + * affect the next step in the current processing + */ + maskedInputMat.release(); + + //Order: src1, src2, dst, mask. + Core.bitwise_and(input, input, maskedInputMat, binaryMat); + + // filter maskedinputmat to grey. + Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); + + // Order: input image, output edges(Array), lowerthreshold, upperthreshold + Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); + + contours.clear(); + + //Order : input image, the contours from output, hierarchy, mode, and method + Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); + + MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; + + Rect[] boundRect = new Rect[contours.size()]; + //Might be useful later, idk so I am going to leave this here + //Point[] centers = new Point[contours.size()]; + + + for (int i = 0; i < contours.size(); i++) { + contoursPoly[i] = new MatOfPoint2f(); + Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); + boundRect[i] = Imgproc.boundingRect(contours.get(i)); + //centers[i] = new Point(); + } + + + List contoursPolyList = new ArrayList<>(contoursPoly.length); + for (MatOfPoint2f poly : contoursPoly) { + //Just to make sure that no problems really come up. + if(poly == null) + { + break; + } + + contoursPolyList.add(new MatOfPoint(poly.toArray())); + } + + + // now this can be removed during the meet. + if (onlyContours == 1) + { + activeMat = maskedInputMat; + } + else if (onlyContours == 2) + { + activeMat = emptyMat; + } + else { + activeMat = input; + } + + largestArea = 0; + + for (int i = 0; i < contours.size(); i++) { + + MatOfPoint contour = contours.get(i); + + contourArea = Imgproc.contourArea(contour); + // A simple filter I cam up with ;P + if (contourArea < noiseSensitivity) + { + continue; + } + + if (contourArea > largestArea) + { + largestArea = contourArea; + index = i; + } + + } + + currentLargest = largestArea; + + + // pretty sure that we actually don't need this for the meet either, + // but is good for demonstration purposes and debugging + if (!contours.isEmpty() && largestArea != 0) + { + // Adding Text3 + Imgproc.putText ( + activeMat, // Matrix obj of the image + largestArea + "", // Text to be added + new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point + Imgproc.FONT_HERSHEY_SIMPLEX , // front face + 1, // front scale + contourColors, // Scalar object for color + 2 // Thickness + ); + Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); + Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); + } + + return activeMat; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + } // I think we could have just returned a Mat frame and have the FSM and Auton do the computing, but that is like // not smart -// public static double getLargestSize() -// { -// return currentLargest; -// } -// -// // boilerplate code if there is a better way to do this, please tell me -// @Override -// public void onViewportTapped() -// { -// if (onlyContours == 1 || onlyContours == 2) -// onlyContours++; -// else -// onlyContours = 1; -// } - - public List getDetections() { - return detections; + public static double getLargestSize() + { + return currentLargest; + } + + // boilerplate code if there is a better way to do this, please tell me + public void onViewportTapped() + { + if (onlyContours == 1 || onlyContours == 2) + onlyContours++; + else + onlyContours = 1; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index 038789e696b8..1130b7703f4f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -7,39 +7,29 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.Modules.AprilTagCustomDatabase; +import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import java.util.List; + @TeleOp public class TestVision extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - AprilTagProcessor tag_processor = new AprilTagProcessor.Builder() - .setTagLibrary(AprilTagCustomDatabase.getLibrary()) - .setDrawAxes(true) - .setDrawCubeProjection(true) - .setDrawTagID(true) - .setDrawTagOutline(true) - .build(); - - // can also be 640 and 488 - // there is also YUY2 - VisionPortal vision_portal = new VisionPortal.Builder() - .addProcessor(tag_processor) - .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) - .setCameraResolution(new Size(640, 480)) - .setStreamFormat(VisionPortal.StreamFormat.MJPEG) - .enableLiveView(true) - .setAutoStopLiveView(true) - .build(); + Camera camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); waitForStart(); while (!isStopRequested() && opModeIsActive()) { - if (!tag_processor.getDetections().isEmpty()) { - AprilTagDetection tag = tag_processor.getDetections().get(0); + List detections = camera.getDetections(); + if(!detections.isEmpty()) { + AprilTagDetection tag = detections.get(0); + telemetry.addData("ID: ", tag.metadata.id); + telemetry.addData("Name: ", tag.metadata.name); } + telemetry.update(); } } } From f974085e85def1222ce39a1bc461bdb385742ec5 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 13 Sep 2025 20:10:49 -0700 Subject: [PATCH 03/19] Apriltag detection is done. --- .../ftc/teamcode/Modules/Camera.java | 17 ++++++- .../ftc/teamcode/Tuning/TestVision.java | 20 ++++++-- .../ftc/teamcode/riptideUtil.java | 46 +++++++++++++++++++ 3 files changed, 78 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 352bf7913828..87c2dec8b9db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -9,6 +9,7 @@ // vision stuff import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.riptideUtil; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; @@ -50,7 +51,7 @@ public class Camera { public Camera(CameraName cameraname) { tag_processor = new AprilTagProcessor.Builder() - .setTagLibrary(AprilTagCustomDatabase.getLibrary()) + .setTagLibrary(riptideUtil.getLibrary()) .setDrawAxes(true) .setDrawCubeProjection(true) .setDrawTagID(true) @@ -70,6 +71,20 @@ public Camera(CameraName cameraname) { } public List getDetections() { + detections = tag_processor.getDetections(); + detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); return detections; } + + public void stop() { + vision_portal.close(); + } + + public void stop_streaming() { + vision_portal.stopStreaming(); + } + + public void resume_streaming() { + vision_portal.resumeStreaming(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index 1130b7703f4f..00302c22f236 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.Tuning; +import android.annotation.SuppressLint; import android.util.Size; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -16,6 +17,7 @@ @TeleOp public class TestVision extends LinearOpMode { + @SuppressLint("DefaultLocale") @Override public void runOpMode() throws InterruptedException { @@ -24,12 +26,22 @@ public void runOpMode() throws InterruptedException { waitForStart(); while (!isStopRequested() && opModeIsActive()) { List detections = camera.getDetections(); - if(!detections.isEmpty()) { - AprilTagDetection tag = detections.get(0); - telemetry.addData("ID: ", tag.metadata.id); - telemetry.addData("Name: ", tag.metadata.name); + + telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + + for (AprilTagDetection detection : detections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + } else { + telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); + } + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + telemetry.update(); } + + camera.stop(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 5708f0ba7af3..cf23d9d975bd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -2,6 +2,9 @@ import com.acmerobotics.dashboard.config.Config; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; + @Config public class riptideUtil { @@ -39,4 +42,47 @@ public class riptideUtil { // Speed relationships public static double MAX_A = 72; // UNDETERMINED public static double MAX_V = 96; // UNDETERMINED + + // ----- VISION ----- // + + public static double DETECTION_TIMEOUT = 25e+7; + + public static AprilTagLibrary getLibrary() { + return new AprilTagLibrary.Builder() + .addTag( + 20, + "Blue Goal", + 6.5, + //new VectorF(), + DistanceUnit.INCH + //Quaternion.identityQuaternion() + ) + .addTag( + 21, + "Green Purple Purple (GPP)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 22, + "Purple Green Purple (PGP)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 23, + "Purple Purple Green (PPG)", + 6.5, + DistanceUnit.INCH + ) + .addTag( + 24, + "Red Goal", + 6.5, + //new VectorF(), + DistanceUnit.INCH + //Quaternion.identityQuaternion() + ) + .build(); + } } From 83ad6745499ac6bf3b6fbb13e95deeb870ec1a34 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 13 Sep 2025 21:14:35 -0700 Subject: [PATCH 04/19] Got the artifact detections written out --- .../ftc/teamcode/Modules/Camera.java | 82 +++++++++++++++---- .../ftc/teamcode/Tuning/TestVision.java | 8 +- .../ftc/teamcode/riptideUtil.java | 4 + 3 files changed, 74 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 87c2dec8b9db..fc9fa7e17549 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,25 +1,36 @@ package org.firstinspires.ftc.teamcode.Modules; -import android.util.Size; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.HardwareMap; +// --- CONSTANTS & OTHER STUFF --- // +import org.firstinspires.ftc.teamcode.riptideUtil; -// vision stuff +// --- CAMERA --- // import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.riptideUtil; +import org.opencv.core.Mat; +import org.opencv.core.RotatedRect; +import org.openftc.easyopencv.OpenCvWebcam; + +// --- PORTALS & PROCESSORS --- // import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; -import org.openftc.easyopencv.OpenCvWebcam; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +// --- LISTS --- // +import java.util.ArrayList; +import java.util.Arrays; import java.util.List; +import android.util.Size; + +import com.qualcomm.robotcore.util.SortOrder; + +/* + Resources: + - https://github.com/FIRST-Tech-Challenge/FtcRobotController/blob/master/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java + - + */ + /* * We have to have two classes because java does not support multiple inheritance * @@ -39,8 +50,10 @@ public class Camera { OpenCvWebcam webcam = null; AprilTagProcessor tag_processor; + ColorBlobLocatorProcessor blob_processor; VisionPortal vision_portal; List detections; + List> blobs; /////////////////////////////////////////////// //// ///// @@ -57,10 +70,18 @@ public Camera(CameraName cameraname) { .setDrawTagID(true) .setDrawTagOutline(true) .build(); + + blob_processor = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setDrawContours(true) + .setBlurSize(5) + .build(); + // can also be 640 and 488 // there is also YUY2 vision_portal = new VisionPortal.Builder() - .addProcessor(tag_processor) + .addProcessors(tag_processor, blob_processor) //.addProcessor(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap.get(WebcamName.class, "Webcam 1"))) .setCamera(cameraname) .setCameraResolution(new Size(640, 480)) @@ -70,12 +91,45 @@ public Camera(CameraName cameraname) { .build(); } - public List getDetections() { + public List get_tag_detections() { detections = tag_processor.getDetections(); detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); return detections; } + public List> get_blob_detections() { + List blobs_detected = blob_processor.getBlobs(); + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 50, + 2000, + blobs_detected + ); + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, + 0.75, + 1, + blobs_detected + ); + + ColorBlobLocatorProcessor.Util.sortByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + SortOrder.DESCENDING, + blobs_detected + ); + + for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { + // bounding box + RotatedRect bbox = blob.getBoxFit(); + double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) + / (bbox.size.height * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); + blobs.add(Arrays.asList(bbox.center.x, bbox.center.y, distance)); + } + return blobs; + } + public void stop() { vision_portal.close(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index 00302c22f236..39388e942e54 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -1,21 +1,17 @@ package org.firstinspires.ftc.teamcode.Tuning; import android.annotation.SuppressLint; -import android.util.Size; 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.firstinspires.ftc.teamcode.Modules.AprilTagCustomDatabase; import org.firstinspires.ftc.teamcode.Modules.Camera; -import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import java.util.List; -@TeleOp +@TeleOp(name="Test Vision") public class TestVision extends LinearOpMode { @SuppressLint("DefaultLocale") @Override @@ -25,7 +21,7 @@ public void runOpMode() throws InterruptedException { waitForStart(); while (!isStopRequested() && opModeIsActive()) { - List detections = camera.getDetections(); + List detections = camera.get_tag_detections(); telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index cf23d9d975bd..e13d09ec49ec 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -47,6 +47,10 @@ public class riptideUtil { public static double DETECTION_TIMEOUT = 25e+7; + public static double ARTIFACT_SIZE_INCHES = 5; + public static double LENS_FOCAL_LEN_INCHES = 0.15748; + public static double LENS_HEIGHT_OFF_GROUND_INCHES = 2; + public static AprilTagLibrary getLibrary() { return new AprilTagLibrary.Builder() .addTag( From b233180d1c37411f9c0e6431aec38b172481177c Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 13 Sep 2025 21:23:34 -0700 Subject: [PATCH 05/19] minor changes --- .../ftc/teamcode/Modules/Camera.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index fc9fa7e17549..aa5da40af786 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -5,16 +5,15 @@ // --- CAMERA --- // import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; -import org.opencv.core.Mat; -import org.opencv.core.RotatedRect; import org.openftc.easyopencv.OpenCvWebcam; -// --- PORTALS & PROCESSORS --- // +// --- PORTALS & PROCESSORS & SIMILAR STUFF --- // import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.Circle; // --- LISTS --- // import java.util.ArrayList; @@ -76,6 +75,9 @@ public Camera(CameraName cameraname) { .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) .setDrawContours(true) .setBlurSize(5) + .setDilateSize(15) + .setErodeSize(15) + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) .build(); // can also be 640 and 488 @@ -121,11 +123,11 @@ public List> get_blob_detections() { ); for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { - // bounding box - RotatedRect bbox = blob.getBoxFit(); + // the circle which fits the artifacts + Circle circle_fit = blob.getCircle(); double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) - / (bbox.size.height * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); - blobs.add(Arrays.asList(bbox.center.x, bbox.center.y, distance)); + / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); + blobs.add(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), distance)); } return blobs; } From 0cece94a24709d3414519c12859a5e74861e39e6 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 20 Sep 2025 18:20:38 -0700 Subject: [PATCH 06/19] nothing really notable --- .../ftc/teamcode/Modules/Camera.java | 68 ++++++++++++++----- .../ftc/teamcode/Tuning/TestVision.java | 64 +++++++++++++---- 2 files changed, 104 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index aa5da40af786..fc565facc828 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -13,6 +13,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.opencv.core.Scalar; import org.firstinspires.ftc.vision.opencv.Circle; // --- LISTS --- // @@ -51,8 +52,14 @@ public class Camera { AprilTagProcessor tag_processor; ColorBlobLocatorProcessor blob_processor; VisionPortal vision_portal; - List detections; - List> blobs; + ArrayList detections; + ArrayList> blobs = new ArrayList<>(); + public enum processors_enabled { + NONE, + TAG, + COLOR, + ALL + } /////////////////////////////////////////////// //// ///// @@ -75,8 +82,8 @@ public Camera(CameraName cameraname) { .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) .setDrawContours(true) .setBlurSize(5) - .setDilateSize(15) - .setErodeSize(15) + .setDilateSize(6) + .setErodeSize(1) .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) .build(); @@ -99,19 +106,20 @@ public List get_tag_detections() { return detections; } - public List> get_blob_detections() { + public ArrayList> get_blob_detections() { + blobs.clear(); List blobs_detected = blob_processor.getBlobs(); ColorBlobLocatorProcessor.Util.filterByCriteria( ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, - 50, + 100, 2000, blobs_detected ); ColorBlobLocatorProcessor.Util.filterByCriteria( ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, - 0.75, + 0.6, 1, blobs_detected ); @@ -122,18 +130,42 @@ public List> get_blob_detections() { blobs_detected ); - for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { - // the circle which fits the artifacts - Circle circle_fit = blob.getCircle(); - double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) - / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); - blobs.add(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), distance)); - } +// for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { +// // the circle which fits the artifacts +// Circle circle_fit = blob.getCircle(); +//// double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) +//// / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); +// ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity()/*, distance*/)); +// blobs.add(temp); +// } + + if(blobs_detected.isEmpty()) {return new ArrayList<>();} + Circle circle_fit = blobs_detected.get(0).getCircle(); + blobs.add(new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY() /*blob.getCircularity(), distance*/))); + //return blobs; + return blobs; } - public void stop() { - vision_portal.close(); + // the default is ALL + public void set_pipeline(processors_enabled processors) { + switch (processors) { + case TAG: + vision_portal.setProcessorEnabled(tag_processor, true); + vision_portal.setProcessorEnabled(blob_processor, false); + break; + case COLOR: + vision_portal.setProcessorEnabled(tag_processor, false); + vision_portal.setProcessorEnabled(blob_processor, true); + break; + case NONE: + vision_portal.setProcessorEnabled(tag_processor, false); + vision_portal.setProcessorEnabled(blob_processor, false); + break; + default: + vision_portal.setProcessorEnabled(tag_processor, true); + vision_portal.setProcessorEnabled(blob_processor, true); + } } public void stop_streaming() { @@ -143,4 +175,8 @@ public void stop_streaming() { public void resume_streaming() { vision_portal.resumeStreaming(); } + + public void stop() { + vision_portal.close(); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index 39388e942e54..df6df313fd5f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -8,36 +8,76 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import java.util.ArrayList; import java.util.List; @TeleOp(name="Test Vision") public class TestVision extends LinearOpMode { - @SuppressLint("DefaultLocale") + @Override public void runOpMode() throws InterruptedException { Camera camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); + Camera.processors_enabled processor = Camera.processors_enabled.ALL; + boolean a_debounce = false; waitForStart(); while (!isStopRequested() && opModeIsActive()) { - List detections = camera.get_tag_detections(); + // this will switch the viewed pipeline on the driver hub + // in this order: ALL -> TAG -> COLOR + if (gamepad1.a) { + if(gamepad1.a ^ a_debounce) { + switch (processor) { + case ALL: + processor = Camera.processors_enabled.TAG; + camera.set_pipeline(processor); + break; + case TAG: + processor = Camera.processors_enabled.COLOR; + camera.set_pipeline(processor); + break; + case COLOR: + processor = Camera.processors_enabled.ALL; + camera.set_pipeline(processor); + break; + } + } + a_debounce = true; + } else { + a_debounce = false; + } + } + + camera.stop(); + } - telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + @SuppressLint("DefaultLocale") + public void telemetryStuff(Camera camera) { + List detections = camera.get_tag_detections(); + ArrayList> color_blobs = camera.get_blob_detections(); - for (AprilTagDetection detection : detections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); - } else { - telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); - } - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + for (AprilTagDetection detection : detections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + } else { + telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); } + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); - telemetry.update(); } - camera.stop(); + telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", color_blobs.size())); + + for (List blob : color_blobs) { + telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); + //telemetry.addLine(String.format("Circularity: %f", blob.get(2))); + //telemetry.addLine(String.format("Distance")); + } + + telemetry.update(); } } From c5348cfb71cffae0b5e6dc3f3de954cb81bbb343 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 26 Sep 2025 18:39:39 -0700 Subject: [PATCH 07/19] just so I don\'t lose all of my work --- .../ConceptVisionColorLocator_Circle.java | 3 +- .../ftc/teamcode/Modules/Camera.java | 56 ++++++++++++------- .../ftc/teamcode/Tuning/TestVision.java | 10 +++- 3 files changed, 47 insertions(+), 22 deletions(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java index 4ec17aaba6c8..7e7eb67292b6 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java @@ -68,8 +68,7 @@ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list */ -@Disabled -@TeleOp(name = "Concept: Vision Color-Locator (Circle)", group = "Concept") +@TeleOp(name = "Concept: Vision Color-Locator (Circle)") public class ConceptVisionColorLocator_Circle extends LinearOpMode { @Override public void runOpMode() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index fc565facc828..78fbe53c8777 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -5,6 +5,7 @@ // --- CAMERA --- // import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.vision.opencv.ImageRegion; import org.openftc.easyopencv.OpenCvWebcam; // --- PORTALS & PROCESSORS & SIMILAR STUFF --- // @@ -13,6 +14,7 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import org.firstinspires.ftc.vision.opencv.ColorRange; +import android.graphics.Color; import org.opencv.core.Scalar; import org.firstinspires.ftc.vision.opencv.Circle; @@ -21,6 +23,7 @@ import java.util.Arrays; import java.util.List; + import android.util.Size; import com.qualcomm.robotcore.util.SortOrder; @@ -77,13 +80,29 @@ public Camera(CameraName cameraname) { .setDrawTagOutline(true) .build(); - blob_processor = new ColorBlobLocatorProcessor.Builder() + blob_processor_purple = new ColorBlobLocatorProcessor.Builder() .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) .setDrawContours(true) + .setBoxFitColor(0) + .setCircleFitColor(Color.rgb(255, 0, 255)) .setBlurSize(5) - .setDilateSize(6) - .setErodeSize(1) + .setDilateSize(15) + .setErodeSize(15) + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) + .build(); + + blob_processor_green = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_GREEN) + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) + .setDrawContours(true) + .setBoxFitColor(0) + .setCircleFitColor(Color.rgb(255, 0, 255)) + .setBlurSize(5) + .setDilateSize(15) + .setErodeSize(15) .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) .build(); @@ -112,14 +131,14 @@ public ArrayList> get_blob_detections() { ColorBlobLocatorProcessor.Util.filterByCriteria( ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, - 100, - 2000, + 10, + 5000, blobs_detected ); ColorBlobLocatorProcessor.Util.filterByCriteria( ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, - 0.6, + 0.1, 1, blobs_detected ); @@ -130,19 +149,18 @@ public ArrayList> get_blob_detections() { blobs_detected ); -// for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { -// // the circle which fits the artifacts -// Circle circle_fit = blob.getCircle(); -//// double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) -//// / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); -// ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity()/*, distance*/)); -// blobs.add(temp); -// } - - if(blobs_detected.isEmpty()) {return new ArrayList<>();} - Circle circle_fit = blobs_detected.get(0).getCircle(); - blobs.add(new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY() /*blob.getCircularity(), distance*/))); - //return blobs; + for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { + // the circle which fits the artifacts + Circle circle_fit = blob.getCircle(); +// double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) +// / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); + ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity(), (double) blob.getContourArea()/*, distance*/)); + blobs.add(temp); + } + +// if(blobs_detected.isEmpty()) {return new ArrayList<>();} +// Circle circle_fit = blobs_detected.get(0).getCircle(); +// blobs.add(new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY() /*blob.getCircularity(), distance*/))); return blobs; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index df6df313fd5f..c324b521408e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @@ -21,8 +22,12 @@ public void runOpMode() throws InterruptedException { Camera camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); Camera.processors_enabled processor = Camera.processors_enabled.ALL; + camera.set_pipeline(processor); boolean a_debounce = false; + telemetry.setMsTransmissionInterval(100); // speed up telemetry updates for debugging + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + waitForStart(); while (!isStopRequested() && opModeIsActive()) { // this will switch the viewed pipeline on the driver hub @@ -48,6 +53,7 @@ public void runOpMode() throws InterruptedException { } else { a_debounce = false; } + telemetryStuff(camera); } camera.stop(); @@ -74,10 +80,12 @@ public void telemetryStuff(Camera camera) { for (List blob : color_blobs) { telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); - //telemetry.addLine(String.format("Circularity: %f", blob.get(2))); + telemetry.addLine(String.format("Circularity: %f", blob.get(2))); + telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); //telemetry.addLine(String.format("Distance")); } telemetry.update(); + sleep(100); } } From 39222569ddadb4b20685ad0ffa321c08f8e8a691 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 26 Sep 2025 20:50:03 -0700 Subject: [PATCH 08/19] got distance kinda working and localization kinda working --- .../ftc/teamcode/Modules/Camera.java | 38 ++++++++++++------- .../ftc/teamcode/Tuning/TestVision.java | 17 ++++++++- .../ftc/teamcode/riptideUtil.java | 12 +++--- 3 files changed, 46 insertions(+), 21 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 78fbe53c8777..fefdb7e1a7be 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.Modules; // --- CONSTANTS & OTHER STUFF --- // +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.riptideUtil; // --- CAMERA --- // @@ -15,7 +17,7 @@ import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import org.firstinspires.ftc.vision.opencv.ColorRange; import android.graphics.Color; -import org.opencv.core.Scalar; + import org.firstinspires.ftc.vision.opencv.Circle; // --- LISTS --- // @@ -53,7 +55,8 @@ public class Camera { OpenCvWebcam webcam = null; AprilTagProcessor tag_processor; - ColorBlobLocatorProcessor blob_processor; + ColorBlobLocatorProcessor blob_processor_purple; + ColorBlobLocatorProcessor blob_processor_green; VisionPortal vision_portal; ArrayList detections; ArrayList> blobs = new ArrayList<>(); @@ -78,6 +81,8 @@ public Camera(CameraName cameraname) { .setDrawCubeProjection(true) .setDrawTagID(true) .setDrawTagOutline(true) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + //.setCameraPose(cameraPosition, cameraOrientation) .build(); blob_processor_purple = new ColorBlobLocatorProcessor.Builder() @@ -99,7 +104,7 @@ public Camera(CameraName cameraname) { .setRoi(ImageRegion.asUnityCenterCoordinates(-1, 1, 1, -1)) .setDrawContours(true) .setBoxFitColor(0) - .setCircleFitColor(Color.rgb(255, 0, 255)) + .setCircleFitColor(Color.rgb(0, 255, 0)) .setBlurSize(5) .setDilateSize(15) .setErodeSize(15) @@ -109,7 +114,7 @@ public Camera(CameraName cameraname) { // can also be 640 and 488 // there is also YUY2 vision_portal = new VisionPortal.Builder() - .addProcessors(tag_processor, blob_processor) + .addProcessors(tag_processor, blob_processor_purple, blob_processor_green) //.addProcessor(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap.get(WebcamName.class, "Webcam 1"))) .setCamera(cameraname) .setCameraResolution(new Size(640, 480)) @@ -127,12 +132,13 @@ public List get_tag_detections() { public ArrayList> get_blob_detections() { blobs.clear(); - List blobs_detected = blob_processor.getBlobs(); + List blobs_detected = blob_processor_purple.getBlobs(); + blobs_detected.addAll(blob_processor_green.getBlobs()); ColorBlobLocatorProcessor.Util.filterByCriteria( ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, - 10, - 5000, + 250, + 9000, blobs_detected ); @@ -152,9 +158,9 @@ public ArrayList> get_blob_detections() { for (ColorBlobLocatorProcessor.Blob blob : blobs_detected) { // the circle which fits the artifacts Circle circle_fit = blob.getCircle(); -// double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) -// / (circle_fit.getRadius() * 2 * riptideUtil.LENS_HEIGHT_OFF_GROUND_INCHES); - ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity(), (double) blob.getContourArea()/*, distance*/)); + double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) + / (circle_fit.getRadius() * 2 * riptideUtil.SENSOR_HEIGHT); + ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity(), (double) blob.getContourArea(), distance)); blobs.add(temp); } @@ -170,19 +176,23 @@ public void set_pipeline(processors_enabled processors) { switch (processors) { case TAG: vision_portal.setProcessorEnabled(tag_processor, true); - vision_portal.setProcessorEnabled(blob_processor, false); + vision_portal.setProcessorEnabled(blob_processor_purple, false); + vision_portal.setProcessorEnabled(blob_processor_green, false); break; case COLOR: vision_portal.setProcessorEnabled(tag_processor, false); - vision_portal.setProcessorEnabled(blob_processor, true); + vision_portal.setProcessorEnabled(blob_processor_purple, true); + vision_portal.setProcessorEnabled(blob_processor_green, true); break; case NONE: vision_portal.setProcessorEnabled(tag_processor, false); - vision_portal.setProcessorEnabled(blob_processor, false); + vision_portal.setProcessorEnabled(blob_processor_purple, false); + vision_portal.setProcessorEnabled(blob_processor_green, false); break; default: vision_portal.setProcessorEnabled(tag_processor, true); - vision_portal.setProcessorEnabled(blob_processor, true); + vision_portal.setProcessorEnabled(blob_processor_purple, true); + vision_portal.setProcessorEnabled(blob_processor_green, true); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index c324b521408e..c862ba0c5d10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -7,6 +7,7 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; @@ -69,6 +70,20 @@ public void telemetryStuff(Camera camera) { for (AprilTagDetection detection : detections) { if (detection.metadata != null) { telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + if (!detection.metadata.name.contains("Obelisk")) { + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("Distance %f (inch)", + Math.sqrt(detection.robotPose.getPosition().x*detection.robotPose.getPosition().x + +detection.robotPose.getPosition().y*detection.robotPose.getPosition().y + +detection.robotPose.getPosition().z*detection.robotPose.getPosition().z))); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } } else { telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); } @@ -82,7 +97,7 @@ public void telemetryStuff(Camera camera) { telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); telemetry.addLine(String.format("Circularity: %f", blob.get(2))); telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); - //telemetry.addLine(String.format("Distance")); + telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(4))); } telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index e13d09ec49ec..2b706721e5e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -49,7 +49,7 @@ public class riptideUtil { public static double ARTIFACT_SIZE_INCHES = 5; public static double LENS_FOCAL_LEN_INCHES = 0.15748; - public static double LENS_HEIGHT_OFF_GROUND_INCHES = 2; + public static double SENSOR_HEIGHT = 0.086; public static AprilTagLibrary getLibrary() { return new AprilTagLibrary.Builder() @@ -57,25 +57,25 @@ public static AprilTagLibrary getLibrary() { 20, "Blue Goal", 6.5, - //new VectorF(), - DistanceUnit.INCH + //new VectorF(−1.482, −1.413, 0.749), + DistanceUnit.INCH//, //Quaternion.identityQuaternion() ) .addTag( 21, - "Green Purple Purple (GPP)", + "Obelisk Green Purple Purple (GPP)", 6.5, DistanceUnit.INCH ) .addTag( 22, - "Purple Green Purple (PGP)", + "Obelisk Purple Green Purple (PGP)", 6.5, DistanceUnit.INCH ) .addTag( 23, - "Purple Purple Green (PPG)", + "Obelisk Purple Purple Green (PPG)", 6.5, DistanceUnit.INCH ) From f1dacc6e13db3ccf5508239fbab0bc44b5a81028 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 26 Sep 2025 21:18:26 -0700 Subject: [PATCH 09/19] untested save --- .../firstinspires/ftc/teamcode/Modules/Camera.java | 13 +++++++++++++ .../firstinspires/ftc/teamcode/riptideUtil.java | 14 ++++++++------ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index fefdb7e1a7be..e899a2bfc9fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -3,6 +3,7 @@ // --- CONSTANTS & OTHER STUFF --- // import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.riptideUtil; // --- CAMERA --- // @@ -171,6 +172,18 @@ public ArrayList> get_blob_detections() { return blobs; } + public EditablePose2D localize() { + for(AprilTagDetection detection : detections) { + if (!detection.metadata.name.contains("Obelisk")) { + return new EditablePose2D( + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, 0, + DistanceUnit.INCH); + } + } + + } + // the default is ALL public void set_pipeline(processors_enabled processors) { switch (processors) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 2b706721e5e8..385aef2f2bbc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -2,7 +2,9 @@ import com.acmerobotics.dashboard.config.Config; +import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; @Config @@ -57,9 +59,9 @@ public static AprilTagLibrary getLibrary() { 20, "Blue Goal", 6.5, - //new VectorF(−1.482, −1.413, 0.749), - DistanceUnit.INCH//, - //Quaternion.identityQuaternion() + new VectorF(-0f, -0f, 0f), + DistanceUnit.INCH, + Quaternion.identityQuaternion() ) .addTag( 21, @@ -83,9 +85,9 @@ public static AprilTagLibrary getLibrary() { 24, "Red Goal", 6.5, - //new VectorF(), - DistanceUnit.INCH - //Quaternion.identityQuaternion() + new VectorF(-0f, -0f, -0f), + DistanceUnit.INCH, + Quaternion.identityQuaternion() ) .build(); } From 3f62c4777aade53ce5146afdc82c5bd08fbead43 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Fri, 3 Oct 2025 20:17:33 -0700 Subject: [PATCH 10/19] I think I'm done but I haven\'t tested my code yet --- .../ftc/teamcode/Modules/Camera.java | 84 +++++++++++++++++-- .../ftc/teamcode/Tuning/TestVision.java | 13 ++- .../ftc/teamcode/riptideUtil.java | 21 +++-- 3 files changed, 96 insertions(+), 22 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index e899a2bfc9fc..74a8596dc662 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -3,7 +3,6 @@ // --- CONSTANTS & OTHER STUFF --- // import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.teamcode.riptideUtil; // --- CAMERA --- // @@ -118,20 +117,29 @@ public Camera(CameraName cameraname) { .addProcessors(tag_processor, blob_processor_purple, blob_processor_green) //.addProcessor(new CameraPipeline(0.047, 578.272, 578.272, 402.145, 221.506, hardwareMap.get(WebcamName.class, "Webcam 1"))) .setCamera(cameraname) - .setCameraResolution(new Size(640, 480)) + .setCameraResolution(new Size(riptideUtil.CAMERA_WIDTH, riptideUtil.CAMERA_HEIGHT)) .setStreamFormat(VisionPortal.StreamFormat.MJPEG) //.enableCameraMonitoring(true) .setAutoStopLiveView(true) .build(); } - public List get_tag_detections() { + public List getTagDetections() { detections = tag_processor.getDetections(); detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); return detections; } - public ArrayList> get_blob_detections() { + public ArrayList> getBlobDetections() { + /* + * Returns an ArrayList of ArrayLists with 5 elements describing the artifact + * They are (in order): + * - x location (on the camera screen) + * - y location (on the camera screen) + * - distance away from the camera (needs tuning) + * - contour area + * - circularity + */ blobs.clear(); List blobs_detected = blob_processor_purple.getBlobs(); blobs_detected.addAll(blob_processor_green.getBlobs()); @@ -161,7 +169,7 @@ public ArrayList> get_blob_detections() { Circle circle_fit = blob.getCircle(); double distance = (riptideUtil.LENS_FOCAL_LEN_INCHES * riptideUtil.ARTIFACT_SIZE_INCHES * 480) / (circle_fit.getRadius() * 2 * riptideUtil.SENSOR_HEIGHT); - ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), blob.getCircularity(), (double) blob.getContourArea(), distance)); + ArrayList temp = new ArrayList<>(Arrays.asList((double) circle_fit.getX(), (double) circle_fit.getY(), distance, (double) blob.getContourArea(), blob.getCircularity())); blobs.add(temp); } @@ -172,7 +180,7 @@ public ArrayList> get_blob_detections() { return blobs; } - public EditablePose2D localize() { + public EditablePose2D getGoalApriltagLocation() { for(AprilTagDetection detection : detections) { if (!detection.metadata.name.contains("Obelisk")) { return new EditablePose2D( @@ -181,11 +189,69 @@ public EditablePose2D localize() { DistanceUnit.INCH); } } + return null; + } + + public EditablePose2D findNearestArtifact() { + return nearestArtifact(0); + } + public EditablePose2D findNearestArtifact(double turret_angle) { + return nearestArtifact(turret_angle); + } + + private EditablePose2D nearestArtifact(double turret_angle) { + /* + * This is just so the code looks nice and if there is no turret_angle, it is: + * findNearestArtifact(); + * instead of: + * findNearestArtifact(0); + * This is litterly a minor, insignificant, useless detail and Aaron will probably yell + * at me for this and I will probably change this in the future + * Future Aaron, I predicted your response, and I chose to do this useless thing. + * hahhahahahahhhahahahahahhahahahahahhahahahahahhashashahahahahahahahahahahahahah + * ahahahahahahahahahahahahahahahhahahahahahahahahahhahahahahahahahahahahahahahahah + * ahahhahahahahahhahahhhahahahahahahah + */ + if(blobs.isEmpty()) {return null;} + ArrayList largest_contour = new ArrayList<>(blobs.get(0)); + + double distance = largest_contour.get(2); + + // essentially setting the origin to the center of the screen + double delta_x = (double) riptideUtil.CAMERA_WIDTH / 2 - largest_contour.get(0); + double delta_y = (double) riptideUtil.CAMERA_HEIGHT / 2 - largest_contour.get(1); + + // relative to the camera + double horizontal_distance = delta_x * riptideUtil.HORIZ_FOV / riptideUtil.CAMERA_WIDTH; + double vertical_distance = delta_y * riptideUtil.VERT_FOV / riptideUtil.CAMERA_HEIGHT; + + double horizontal_angle_error = Math.atan(horizontal_distance/distance); + double vertical_angle_error = Math.atan(vertical_distance/distance); + + // difference between the errors and the camera angle, so this is relative to what ever + // the angle of the camera is relative to + double horizontal_angle = turret_angle - horizontal_angle_error; + double vertical_angle = riptideUtil.CAMERA_ANGLE - vertical_angle_error; + + // relative to whatever the turret_angle is + // 0 degrees means relative to the camera or robot (if they are looking in the same direction) + // anything else would imply relative to the robot + double horizontal_dist = Math.sin(horizontal_angle_error) * distance; + + // relative to the ground + double height = Math.sin(vertical_angle) * distance; + + return new EditablePose2D( + horizontal_dist, + height, + 0, + DistanceUnit.INCH + ); } // the default is ALL - public void set_pipeline(processors_enabled processors) { + public void setPipeline(processors_enabled processors) { switch (processors) { case TAG: vision_portal.setProcessorEnabled(tag_processor, true); @@ -209,11 +275,11 @@ public void set_pipeline(processors_enabled processors) { } } - public void stop_streaming() { + public void stopStreaming() { vision_portal.stopStreaming(); } - public void resume_streaming() { + public void resumeStreaming() { vision_portal.resumeStreaming(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index c862ba0c5d10..ea952d145d31 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -10,7 +10,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import java.util.ArrayList; import java.util.List; @@ -23,7 +22,7 @@ public void runOpMode() throws InterruptedException { Camera camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); Camera.processors_enabled processor = Camera.processors_enabled.ALL; - camera.set_pipeline(processor); + camera.setPipeline(processor); boolean a_debounce = false; telemetry.setMsTransmissionInterval(100); // speed up telemetry updates for debugging @@ -38,15 +37,15 @@ public void runOpMode() throws InterruptedException { switch (processor) { case ALL: processor = Camera.processors_enabled.TAG; - camera.set_pipeline(processor); + camera.setPipeline(processor); break; case TAG: processor = Camera.processors_enabled.COLOR; - camera.set_pipeline(processor); + camera.setPipeline(processor); break; case COLOR: processor = Camera.processors_enabled.ALL; - camera.set_pipeline(processor); + camera.setPipeline(processor); break; } } @@ -95,9 +94,9 @@ public void telemetryStuff(Camera camera) { for (List blob : color_blobs) { telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); - telemetry.addLine(String.format("Circularity: %f", blob.get(2))); + telemetry.addLine(String.format("Circularity: %f", blob.get(4))); telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); - telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(4))); + telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); } telemetry.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 385aef2f2bbc..dd7e609fbce6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -47,11 +47,20 @@ public class riptideUtil { // ----- VISION ----- // + public static int CAMERA_WIDTH = 640; + public static int CAMERA_HEIGHT = 480; + public static double CAMERA_ANGLE = 0; + + + public static double DETECTION_TIMEOUT = 25e+7; public static double ARTIFACT_SIZE_INCHES = 5; + public static double LENS_FOCAL_LEN_INCHES = 0.15748; public static double SENSOR_HEIGHT = 0.086; + public static double HORIZ_FOV = 55; + public static double VERT_FOV = 55; public static AprilTagLibrary getLibrary() { return new AprilTagLibrary.Builder() @@ -59,9 +68,9 @@ public static AprilTagLibrary getLibrary() { 20, "Blue Goal", 6.5, - new VectorF(-0f, -0f, 0f), - DistanceUnit.INCH, - Quaternion.identityQuaternion() + //new VectorF(-0f, -0f, 0f), + DistanceUnit.INCH//, + //Quaternion.identityQuaternion() ) .addTag( 21, @@ -85,9 +94,9 @@ public static AprilTagLibrary getLibrary() { 24, "Red Goal", 6.5, - new VectorF(-0f, -0f, -0f), - DistanceUnit.INCH, - Quaternion.identityQuaternion() + //new VectorF(-0f, -0f, -0f), + DistanceUnit.INCH//, + //Quaternion.identityQuaternion() ) .build(); } From eaf0f924fffe1168b5d49c13407b31b9a4f7887b Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 4 Oct 2025 16:21:50 -0700 Subject: [PATCH 11/19] created the turntable class --- .../Modules/AprilTagCustomDatabase.java | 29 --- .../ftc/teamcode/Modules/Camera.java | 24 +- .../ftc/teamcode/Modules/CameraPipeline.java | 229 ------------------ .../ftc/teamcode/Modules/TurnTable.java | 62 +++++ .../ftc/teamcode/Tuning/TestVision.java | 8 +- .../ftc/teamcode/riptideUtil.java | 5 +- 6 files changed, 90 insertions(+), 267 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java deleted file mode 100644 index 9058e43f83ad..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/AprilTagCustomDatabase.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.teamcode.Modules; - -import org.firstinspires.ftc.robotcore.external.matrices.VectorF; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; -import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; - -public class AprilTagCustomDatabase { - public static AprilTagLibrary getLibrary() { - return new AprilTagLibrary.Builder() - .addTag( - 20, - "Blue Goal", - 6.5, - //new VectorF(), - DistanceUnit.INCH - //Quaternion.identityQuaternion() - ) - .addTag( - 24, - "Red Goal", - 6.5, - //new VectorF(), - DistanceUnit.INCH - //Quaternion.identityQuaternion() - ) - .build(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 74a8596dc662..27240d4c1913 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -124,7 +124,7 @@ public Camera(CameraName cameraname) { .build(); } - public List getTagDetections() { + public ArrayList getTagDetections() { detections = tag_processor.getDetections(); detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); return detections; @@ -192,6 +192,28 @@ public EditablePose2D getGoalApriltagLocation() { return null; } + public double getAprilTagDistance(AprilTagDetection tag) { + return Math.pow(tag.robotPose.getPosition().x, 2) * + Math.pow(tag.robotPose.getPosition().y, 2) * + Math.pow(tag.robotPose.getPosition().z, 2); + } + + public double getTagHorizonalAngle(AprilTagDetection tag) { + /* + * Gets the angle of the april tag relative to the camera. + */ + double distance = this.getAprilTagDistance(tag); + + // essentially setting the origin to the center of the screen + double delta_x = (double) riptideUtil.CAMERA_WIDTH / 2 - tag.center.x; + + // relative to the camera + double horizontal_distance = delta_x * riptideUtil.HORIZ_FOV / riptideUtil.CAMERA_WIDTH; + + //the angle of the april tag relative to the camera + return Math.atan(horizontal_distance/distance); + } + public EditablePose2D findNearestArtifact() { return nearestArtifact(0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java deleted file mode 100644 index 362fb671abe2..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/CameraPipeline.java +++ /dev/null @@ -1,229 +0,0 @@ -package org.firstinspires.ftc.teamcode.Modules; - -import android.graphics.Canvas; -import android.util.Size; - -import com.qualcomm.robotcore.hardware.HardwareMap; - -//opencv for vision stuff -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; -import org.firstinspires.ftc.vision.VisionProcessor; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.opencv.core.*; -import org.opencv.imgproc.Imgproc; -import org.openftc.easyopencv.OpenCvPipeline; -import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.firstinspires.ftc.vision.VisionPortal; - -// idk why we need this -import java.util.ArrayList; -import java.util.List; - -/* - * This is the pipeline which will be processing the cameras feed - * It should be able to: - * - detect and recognize april tags - * - detect and locate (if I will we able to) artifacts - * - get the current state of the ramp - * - * welp lets see how well I make this - */ - -// omg this is going to need so much cleaning up -// https://www.youtube.com/watch?v=aWPIsqjMoDI&t=740s -// https://stackoverflow.com/questions/60867638/methods-for-detecting-a-known-shape-object-in-an-image-using-opencv -public class CameraPipeline implements VisionProcessor { - - /////////////////////////////////////////////// - //// ///// - //// VARIABLES ///// - //// ///// - ////////////////////////////////////////////// - - /* - Purple lower: (0, 136.0, 120.4) - Purple upper: (255, 255, 255) - */ - public Scalar lower = new Scalar(0, 136.0, 120.4); - public Scalar upper = new Scalar(255, 255, 255); - - - private final Mat ycrcbmat = new Mat(); - private final Mat binaryMat = new Mat(); - private final Mat maskedInputMat = new Mat(); - private final Mat grayscaleMat = new Mat(); - - // Contour Vars - - List contours = new ArrayList<>(); - public double lowerContourThreshold = 0; - public double upperContourThreshold = 300; - public Scalar contourColors = new Scalar(0,255,0); - public double noiseSensitivity = 153; - public int contourSize = 1; - - private final Mat edgeDetectorFrame = new Mat(); - private int onlyContours = 1; - - public static double currentLargest = 0; - - Mat activeMat; - Mat emptyMat = Mat.zeros(edgeDetectorFrame.size(), CvType.CV_8UC3); - int index = 0; - double contourArea; - double largestArea = 0; - - ////////////////////////////////////////////// - //// ///// - //// FUNCTIONS ///// - //// ///// - ////////////////////////////////////////////// - - @Override - public void init(int a, int b, CameraCalibration c) { - - } - - public CameraPipeline(double tagsize, double fx, double fy, double cx, double cy, WebcamName camera) { - - } - - @Override - public Object processFrame(Mat input, long captureTimeNanoseconds) { - - // Takes our "input" mat as an input, and outputs to a separate Mat buffer "ycrcbMat" - // converts the image from the BGR color space to the YCrCB color space - // the YCrCb color space has the channels [Luminance or Y, Red - Y, Blue - Y] - Imgproc.cvtColor(input, ycrcbmat, Imgproc.COLOR_RGB2YCrCb); - - // Order is source, lowerBound, upperbound, dst. - // filters the image to the lower and upper bounds - Core.inRange(ycrcbmat, lower, upper, binaryMat); - /* - * Release the reusable Mat so that old data doesn't - * affect the next step in the current processing - */ - maskedInputMat.release(); - - //Order: src1, src2, dst, mask. - Core.bitwise_and(input, input, maskedInputMat, binaryMat); - - // filter maskedinputmat to grey. - Imgproc.cvtColor(maskedInputMat, grayscaleMat, Imgproc.COLOR_RGB2GRAY); - - // Order: input image, output edges(Array), lowerthreshold, upperthreshold - Imgproc.Canny(grayscaleMat, edgeDetectorFrame, lowerContourThreshold, upperContourThreshold); - - contours.clear(); - - //Order : input image, the contours from output, hierarchy, mode, and method - Imgproc.findContours(edgeDetectorFrame, contours, new Mat(), Imgproc.RETR_TREE, Imgproc.CHAIN_APPROX_SIMPLE); - - MatOfPoint2f[] contoursPoly = new MatOfPoint2f[contours.size()]; - - Rect[] boundRect = new Rect[contours.size()]; - //Might be useful later, idk so I am going to leave this here - //Point[] centers = new Point[contours.size()]; - - - for (int i = 0; i < contours.size(); i++) { - contoursPoly[i] = new MatOfPoint2f(); - Imgproc.approxPolyDP(new MatOfPoint2f(contours.get(i).toArray()), contoursPoly[i], 3, true); - boundRect[i] = Imgproc.boundingRect(contours.get(i)); - //centers[i] = new Point(); - } - - - List contoursPolyList = new ArrayList<>(contoursPoly.length); - for (MatOfPoint2f poly : contoursPoly) { - //Just to make sure that no problems really come up. - if(poly == null) - { - break; - } - - contoursPolyList.add(new MatOfPoint(poly.toArray())); - } - - - // now this can be removed during the meet. - if (onlyContours == 1) - { - activeMat = maskedInputMat; - } - else if (onlyContours == 2) - { - activeMat = emptyMat; - } - else { - activeMat = input; - } - - largestArea = 0; - - for (int i = 0; i < contours.size(); i++) { - - MatOfPoint contour = contours.get(i); - - contourArea = Imgproc.contourArea(contour); - // A simple filter I cam up with ;P - if (contourArea < noiseSensitivity) - { - continue; - } - - if (contourArea > largestArea) - { - largestArea = contourArea; - index = i; - } - - } - - currentLargest = largestArea; - - - // pretty sure that we actually don't need this for the meet either, - // but is good for demonstration purposes and debugging - if (!contours.isEmpty() && largestArea != 0) - { - // Adding Text3 - Imgproc.putText ( - activeMat, // Matrix obj of the image - largestArea + "", // Text to be added - new Point(boundRect[index].tl().x, boundRect[index].tl().y), // point - Imgproc.FONT_HERSHEY_SIMPLEX , // front face - 1, // front scale - contourColors, // Scalar object for color - 2 // Thickness - ); - Imgproc.drawContours(activeMat, contoursPolyList, index, contourColors, contourSize); - Imgproc.rectangle(activeMat, boundRect[index].tl(), boundRect[index].br(), contourColors, 2); - } - - return activeMat; - } - - @Override - public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - - } - - // I think we could have just returned a Mat frame and have the FSM and Auton do the computing, but that is like - // not smart - public static double getLargestSize() - { - return currentLargest; - } - - // boilerplate code if there is a better way to do this, please tell me - public void onViewportTapped() - { - if (onlyContours == 1 || onlyContours == 2) - onlyContours++; - else - onlyContours = 1; - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java new file mode 100644 index 000000000000..1ea082be77c1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import org.firstinspires.ftc.teamcode.riptideUtil; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; + +public class TurnTable { + Camera camera; + Servo turntable_servo; + DcMotor turntable_motor; + PIDController tablePID; + + double tableKP = 0, tableKI = 0, tableKD = 0; // needs tuning + + // This is the name of the tag which the turn table will be following + // the goal_tag can be either "Red Goal" or "Blue Goal" + String goal_tag; + + public TurnTable(HardwareMap hardwareMap, String goal_tag) { + this.camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); + this.turntable_servo = hardwareMap.servo.get("turntables"); // turn table servo + this.turntable_motor = hardwareMap.dcMotor.get("turntablem"); // turn table motor + this.tablePID = new PIDController(tableKP, tableKI, tableKD); // gotta tune + this.goal_tag = goal_tag; + } + + public void setPosition(double pos) { + /* + * This would be relative to the robot, + * I don't know if we are using a servo or a motor so the + * code might be a bit different + */ + // for the servo + turntable_servo.setPosition(tablePID.calculate(turntable_servo.getPosition(), pos)); + + // for the motor + turntable_motor.setPower(tablePID.calculate(0, pos)); + } + + public void followTag() { + /* + * This is going to be constantly called to update the servo position or + * motor power. It looks for the april tag and finds the position needed + * so that the turn table faces the tag. + */ + ArrayList detections = camera.getTagDetections(); + + for (AprilTagDetection detection : detections) { + if (detection.metadata.name.equals(goal_tag)) { + double horiz_angle = camera.getTagHorizonalAngle(detection); + setPosition(horiz_angle * riptideUtil.MOTOR_POS_CONST * 2 / riptideUtil.CAMERA_WIDTH); + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index ea952d145d31..889a9bd50ed4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -61,8 +61,8 @@ public void runOpMode() throws InterruptedException { @SuppressLint("DefaultLocale") public void telemetryStuff(Camera camera) { - List detections = camera.get_tag_detections(); - ArrayList> color_blobs = camera.get_blob_detections(); + List detections = camera.getTagDetections(); + ArrayList> color_blobs = camera.getBlobDetections(); telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); @@ -75,9 +75,7 @@ public void telemetryStuff(Camera camera) { detection.robotPose.getPosition().y, detection.robotPose.getPosition().z)); telemetry.addLine(String.format("Distance %f (inch)", - Math.sqrt(detection.robotPose.getPosition().x*detection.robotPose.getPosition().x - +detection.robotPose.getPosition().y*detection.robotPose.getPosition().y - +detection.robotPose.getPosition().z*detection.robotPose.getPosition().z))); + camera.getAprilTagDistance(detection))); telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index dd7e609fbce6..c342d8bcdbc9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -51,10 +51,7 @@ public class riptideUtil { public static int CAMERA_HEIGHT = 480; public static double CAMERA_ANGLE = 0; - - public static double DETECTION_TIMEOUT = 25e+7; - public static double ARTIFACT_SIZE_INCHES = 5; public static double LENS_FOCAL_LEN_INCHES = 0.15748; @@ -62,6 +59,8 @@ public class riptideUtil { public static double HORIZ_FOV = 55; public static double VERT_FOV = 55; + public static double MOTOR_POS_CONST = 1; // needs tuning + public static AprilTagLibrary getLibrary() { return new AprilTagLibrary.Builder() .addTag( From a73c6cb1337b62c3a081154198c6af5d986f2841 Mon Sep 17 00:00:00 2001 From: pikachu7911 Date: Sat, 25 Oct 2025 20:43:37 -0700 Subject: [PATCH 12/19] Camera lock on target function in Meet0FSM? --- .../ftc/teamcode/Modules/Camera.java | 2 +- .../ftc/teamcode/Modules/TurnTable.java | 2 +- .../org/firstinspires/ftc/teamcode/Robot.java | 5 +++++ .../ftc/teamcode/Teleop/Meet0FSM.java | 17 +++++++++++++++++ 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 27240d4c1913..43007462209e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -198,7 +198,7 @@ public double getAprilTagDistance(AprilTagDetection tag) { Math.pow(tag.robotPose.getPosition().z, 2); } - public double getTagHorizonalAngle(AprilTagDetection tag) { + public double getTagHorizontalAngle(AprilTagDetection tag) { /* * Gets the angle of the april tag relative to the camera. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java index 1ea082be77c1..5e36babb735f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java @@ -54,7 +54,7 @@ public void followTag() { for (AprilTagDetection detection : detections) { if (detection.metadata.name.equals(goal_tag)) { - double horiz_angle = camera.getTagHorizonalAngle(detection); + double horiz_angle = camera.getTagHorizontalAngle(detection); setPosition(horiz_angle * riptideUtil.MOTOR_POS_CONST * 2 / riptideUtil.CAMERA_WIDTH); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index e92de83b1e36..d28b85164f3e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -11,15 +11,20 @@ public class Robot { HardwareMap hardwareMap; Drivetrain drivetrain; + Camera camera; public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); + camera = hardwareMap.get(Camera.class, "camera"); } public Drivetrain getDrivetrain(){ return drivetrain; } + public Camera getCamera() { + return camera; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 0f2b893aca2f..99bfa49c3dc6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.Robot; @@ -98,6 +99,10 @@ private void FSM(){ hasrun = false; } + if (gamepad1.right_bumper) { + lockOnTarget(); + } + if (gamepad2.left_trigger > 0.1){ currentState = states.INTAKE; hasrun = false; @@ -132,4 +137,16 @@ private void fieldCentricDrive() { robot.getDrivetrain().resetImu(); } } + + private void lockOnTarget() { + double goalAngle = robot.getCamera().getTagHorizontalAngle(robot.getCamera().getTagDetections().get(0)) * Math.PI / 180; // I literally don't know T-T + PIDController turnPID = new PIDController(0.05, 0.1, 0.1); // these are random numbers + turnPID.setDeadZone(0.1); // Also random, idk if this is low + double calcPow = turnPID.calculate(0, goalAngle); // This feels wrong, someone tell me if it's wrong plz + + double rWheelPower = Math.min(calcPow, 1); + double lWheelPower = Math.max(-1, -calcPow); + + robot.getDrivetrain().setWheelPowers(lWheelPower, rWheelPower, rWheelPower, lWheelPower); + } } From 94989f8caa7de436059f09df8e44f4ba2da16003 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sun, 26 Oct 2025 12:49:59 -0700 Subject: [PATCH 13/19] need to test align now --- .../ftc/teamcode/Modules/TurnTable.java | 86 +++++++++++-------- .../org/firstinspires/ftc/teamcode/Robot.java | 5 ++ .../ftc/teamcode/Teleop/Meet0FSM.java | 53 +++++++----- 3 files changed, 89 insertions(+), 55 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java index 5e36babb735f..bfddc1327371 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java @@ -11,52 +11,70 @@ import java.util.ArrayList; + public class TurnTable { - Camera camera; - Servo turntable_servo; - DcMotor turntable_motor; + //Servo turntable_servo; + //DcMotor turntable_motor; PIDController tablePID; double tableKP = 0, tableKI = 0, tableKD = 0; // needs tuning // This is the name of the tag which the turn table will be following // the goal_tag can be either "Red Goal" or "Blue Goal" - String goal_tag; + //String goal_tag; - public TurnTable(HardwareMap hardwareMap, String goal_tag) { - this.camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); - this.turntable_servo = hardwareMap.servo.get("turntables"); // turn table servo - this.turntable_motor = hardwareMap.dcMotor.get("turntablem"); // turn table motor + public TurnTable(double kp, double ki, double kd/*HardwareMap hardwareMap, String goal_tag*/) { + //this.camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); + //this.turntable_servo = hardwareMap.servo.get("turntables"); // turn table servo + //this.turntable_motor = hardwareMap.dcMotor.get("turntablem"); // turn table motor + tableKP = kp; + tableKI = ki; + tableKD = kd; this.tablePID = new PIDController(tableKP, tableKI, tableKD); // gotta tune - this.goal_tag = goal_tag; + //this.goal_tag = goal_tag; + + // this is to try to stop oscillation, if the difference in angle < this value + // the pid will return 0 + tablePID.setDeadZone(0); } - public void setPosition(double pos) { - /* - * This would be relative to the robot, - * I don't know if we are using a servo or a motor so the - * code might be a bit different - */ - // for the servo - turntable_servo.setPosition(tablePID.calculate(turntable_servo.getPosition(), pos)); - - // for the motor - turntable_motor.setPower(tablePID.calculate(0, pos)); + public void setPID(double kp, double ki, double kd) { + tableKP = kp; + tableKI = ki; + tableKD = kd; + this.tablePID = new PIDController(tableKP, tableKI, tableKD); } - public void followTag() { - /* - * This is going to be constantly called to update the servo position or - * motor power. It looks for the april tag and finds the position needed - * so that the turn table faces the tag. - */ - ArrayList detections = camera.getTagDetections(); - - for (AprilTagDetection detection : detections) { - if (detection.metadata.name.equals(goal_tag)) { - double horiz_angle = camera.getTagHorizontalAngle(detection); - setPosition(horiz_angle * riptideUtil.MOTOR_POS_CONST * 2 / riptideUtil.CAMERA_WIDTH); - } - } +// public void setPosition(double pos) { +// /* +// * This would be relative to the robot, +// * I don't know if we are using a servo or a motor so the +// * code might be a bit different +// */ +// // for the servo +// turntable_servo.setPosition(tablePID.calculate(turntable_servo.getPosition(), pos)); +// +// // for the motor +// turntable_motor.setPower(tablePID.calculate(0, pos)); +// } + +// public void followTag() { +// /* +// * This is going to be constantly called to update the servo position or +// * motor power. It looks for the april tag and finds the position needed +// * so that the turn table faces the tag. +// */ +// ArrayList detections = camera.getTagDetections(); +// +// for (AprilTagDetection detection : detections) { +// if (detection.metadata.name.equals(goal_tag)) { +// double horiz_angle = camera.getTagHorizontalAngle(detection); +// setPosition(horiz_angle * riptideUtil.MOTOR_POS_CONST * 2 / riptideUtil.CAMERA_WIDTH); +// } +// } +// } + + public double alignTag(double goalAngle) { + return tablePID.calculate(0, goalAngle); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index d28b85164f3e..1142f3162d6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -5,6 +5,7 @@ import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; import org.firstinspires.ftc.teamcode.Modules.Slides; +import org.firstinspires.ftc.teamcode.Modules.TurnTable; public class Robot { @@ -13,11 +14,14 @@ public class Robot { Drivetrain drivetrain; Camera camera; + TurnTable table; + public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); camera = hardwareMap.get(Camera.class, "camera"); + table = new TurnTable(2, 0, 0); } public Drivetrain getDrivetrain(){ @@ -26,5 +30,6 @@ public Drivetrain getDrivetrain(){ public Camera getCamera() { return camera; } + public TurnTable getTable() {return table;} } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 99bfa49c3dc6..898fb0ea8d8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -7,6 +7,9 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; @Config @@ -16,10 +19,9 @@ public class Meet0FSM extends LinearOpMode { Robot robot; boolean hasrun = false; - - - - + boolean align = false; + double kp = 0, ki = 0, kd = 0; + String goalTag = "Red Goal"; public enum states{ IDLE, @@ -33,6 +35,7 @@ public enum states{ public void runOpMode() throws InterruptedException { hasrun = false; + align = false; telemetry.addData("Robot status:", "succesfully initiated"); telemetry.update(); @@ -44,6 +47,10 @@ public void runOpMode() throws InterruptedException { telemetry.update(); while(opModeIsActive()){ + robot.getTable().setPID(kp, ki, kd); + if (gamepad1.a) { + align = true; + } telemetry.addData("Current State:", currentState); telemetry.update(); fieldCentricDrive(); @@ -99,10 +106,6 @@ private void FSM(){ hasrun = false; } - if (gamepad1.right_bumper) { - lockOnTarget(); - } - if (gamepad2.left_trigger > 0.1){ currentState = states.INTAKE; hasrun = false; @@ -113,11 +116,31 @@ private void FSM(){ } } + private double getTagAlignVal() { + ArrayList detections = robot.getCamera().getTagDetections(); + AprilTagDetection goalTag = null; + for (AprilTagDetection detection : detections) { + if(detection.metadata.name.equals(goalTag) || !detection.metadata.name.contains("Obelisk")) { + goalTag = detection; + } + } + + if (goalTag == null) { align = false; return 0; } + + double rx = robot.getTable().alignTag(robot.getCamera().getTagHorizontalAngle(goalTag)); + + if(rx == 0) { + align = false; + } + return rx; + } + private void fieldCentricDrive() { double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; double y = -gamepad1.left_stick_y * slowdown; double x = gamepad1.left_stick_x * 1.1 * slowdown; - double rx = gamepad1.right_stick_x * slowdown; + double alignVal = getTagAlignVal(); + double rx = (align) ? alignVal : gamepad1.right_stick_x * slowdown; double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); @@ -137,16 +160,4 @@ private void fieldCentricDrive() { robot.getDrivetrain().resetImu(); } } - - private void lockOnTarget() { - double goalAngle = robot.getCamera().getTagHorizontalAngle(robot.getCamera().getTagDetections().get(0)) * Math.PI / 180; // I literally don't know T-T - PIDController turnPID = new PIDController(0.05, 0.1, 0.1); // these are random numbers - turnPID.setDeadZone(0.1); // Also random, idk if this is low - double calcPow = turnPID.calculate(0, goalAngle); // This feels wrong, someone tell me if it's wrong plz - - double rWheelPower = Math.min(calcPow, 1); - double lWheelPower = Math.max(-1, -calcPow); - - robot.getDrivetrain().setWheelPowers(lWheelPower, rWheelPower, rWheelPower, lWheelPower); - } } From ab424fc3e047ea3c128106cf3b335c17f032cce4 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sun, 26 Oct 2025 15:23:27 -0700 Subject: [PATCH 14/19] made an angle tuner --- .../ftc/teamcode/Modules/Camera.java | 29 ++++- .../ftc/teamcode/Modules/Drivetrain.java | 4 +- .../ftc/teamcode/Modules/TurnTable.java | 2 +- .../org/firstinspires/ftc/teamcode/Robot.java | 4 +- .../ftc/teamcode/Teleop/Meet0FSM.java | 37 ++----- .../ftc/teamcode/Tuning/AnglePIDTuner.java | 100 ++++++++++++++++++ .../ftc/teamcode/riptideUtil.java | 3 +- 7 files changed, 138 insertions(+), 41 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 43007462209e..3786e7d9a2f0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -60,6 +60,7 @@ public class Camera { VisionPortal vision_portal; ArrayList detections; ArrayList> blobs = new ArrayList<>(); + String goalTag; public enum processors_enabled { NONE, TAG, @@ -124,6 +125,14 @@ public Camera(CameraName cameraname) { .build(); } + public void setGoalTag(String goalTag) { + this.goalTag = goalTag; + } + + public String getGoalTag() { + return goalTag; + } + public ArrayList getTagDetections() { detections = tag_processor.getDetections(); detections.removeIf(detection -> System.nanoTime() - detection.frameAcquisitionNanoTime > riptideUtil.DETECTION_TIMEOUT); @@ -180,7 +189,18 @@ public ArrayList> getBlobDetections() { return blobs; } + public AprilTagDetection getGoalApriltag() { + detections = getTagDetections(); + for(AprilTagDetection detection : detections) { + if(detection.metadata.name.equals(goalTag)) { + return detection; + } + } + return null; + } + public EditablePose2D getGoalApriltagLocation() { + detections = getTagDetections(); for(AprilTagDetection detection : detections) { if (!detection.metadata.name.contains("Obelisk")) { return new EditablePose2D( @@ -202,16 +222,15 @@ public double getTagHorizontalAngle(AprilTagDetection tag) { /* * Gets the angle of the april tag relative to the camera. */ - double distance = this.getAprilTagDistance(tag); // essentially setting the origin to the center of the screen double delta_x = (double) riptideUtil.CAMERA_WIDTH / 2 - tag.center.x; // relative to the camera - double horizontal_distance = delta_x * riptideUtil.HORIZ_FOV / riptideUtil.CAMERA_WIDTH; + double horizontal_angle = delta_x * riptideUtil.CAM_FOV / riptideUtil.CAMERA_WIDTH; //the angle of the april tag relative to the camera - return Math.atan(horizontal_distance/distance); + return horizontal_angle; } public EditablePose2D findNearestArtifact() { @@ -245,8 +264,8 @@ private EditablePose2D nearestArtifact(double turret_angle) { double delta_y = (double) riptideUtil.CAMERA_HEIGHT / 2 - largest_contour.get(1); // relative to the camera - double horizontal_distance = delta_x * riptideUtil.HORIZ_FOV / riptideUtil.CAMERA_WIDTH; - double vertical_distance = delta_y * riptideUtil.VERT_FOV / riptideUtil.CAMERA_HEIGHT; + double horizontal_distance = delta_x * riptideUtil.CAM_FOV / riptideUtil.CAMERA_WIDTH; + double vertical_distance = delta_y * riptideUtil.CAM_FOV / riptideUtil.CAMERA_HEIGHT; double horizontal_angle_error = Math.atan(horizontal_distance/distance); double vertical_angle_error = Math.atan(vertical_distance/distance); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index d69cb433e7ea..a601a8e1b9f3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -50,8 +50,8 @@ public Drivetrain(HardwareMap hardwareMap) { blWheel.setDirection(DcMotorSimple.Direction.REVERSE); flWheel.setDirection(DcMotorSimple.Direction.REVERSE); - frWheel.setDirection(DcMotorSimple.Direction.FORWARD); - brWheel.setDirection(DcMotorSimple.Direction.FORWARD); + frWheel.setDirection(DcMotorSimple.Direction.REVERSE); + brWheel.setDirection(DcMotorSimple.Direction.REVERSE); brWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); blWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java index bfddc1327371..ad41034db306 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/TurnTable.java @@ -35,7 +35,7 @@ public TurnTable(double kp, double ki, double kd/*HardwareMap hardwareMap, Strin // this is to try to stop oscillation, if the difference in angle < this value // the pid will return 0 - tablePID.setDeadZone(0); + tablePID.setDeadZone(2); } public void setPID(double kp, double ki, double kd) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 1142f3162d6c..ecb866c5db2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -2,10 +2,12 @@ import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; import org.firstinspires.ftc.teamcode.Modules.Slides; import org.firstinspires.ftc.teamcode.Modules.TurnTable; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; public class Robot { @@ -20,7 +22,7 @@ public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); - camera = hardwareMap.get(Camera.class, "camera"); + camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); table = new TurnTable(2, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java index 898fb0ea8d8b..3bc2c31d7ff8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Teleop/Meet0FSM.java @@ -12,15 +12,14 @@ import java.util.ArrayList; -@Config @TeleOp(name = "Meet 0 FSM") public class Meet0FSM extends LinearOpMode { Robot robot; boolean hasrun = false; - boolean align = false; - double kp = 0, ki = 0, kd = 0; + public static boolean align = false; + public static double kp = 0, ki = 0, kd = 0; String goalTag = "Red Goal"; public enum states{ @@ -33,9 +32,9 @@ public enum states{ @Override public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); hasrun = false; - align = false; telemetry.addData("Robot status:", "succesfully initiated"); telemetry.update(); @@ -47,13 +46,11 @@ public void runOpMode() throws InterruptedException { telemetry.update(); while(opModeIsActive()){ - robot.getTable().setPID(kp, ki, kd); - if (gamepad1.a) { - align = true; - } + FSM(); + fieldCentricDrive(); telemetry.addData("Current State:", currentState); + telemetry.addData("Robot angle", robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); telemetry.update(); - fieldCentricDrive(); } } @@ -116,31 +113,11 @@ private void FSM(){ } } - private double getTagAlignVal() { - ArrayList detections = robot.getCamera().getTagDetections(); - AprilTagDetection goalTag = null; - for (AprilTagDetection detection : detections) { - if(detection.metadata.name.equals(goalTag) || !detection.metadata.name.contains("Obelisk")) { - goalTag = detection; - } - } - - if (goalTag == null) { align = false; return 0; } - - double rx = robot.getTable().alignTag(robot.getCamera().getTagHorizontalAngle(goalTag)); - - if(rx == 0) { - align = false; - } - return rx; - } - private void fieldCentricDrive() { double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; double y = -gamepad1.left_stick_y * slowdown; double x = gamepad1.left_stick_x * 1.1 * slowdown; - double alignVal = getTagAlignVal(); - double rx = (align) ? alignVal : gamepad1.right_stick_x * slowdown; + double rx = gamepad1.right_stick_x * slowdown; double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java new file mode 100644 index 000000000000..c3ad54f2df60 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.teamcode.Teleop; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.teamcode.Modules.PIDController; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +import java.util.ArrayList; + + +@Config +@TeleOp(name = "Angle Tuner", group = "Tuning") +public class AnglePIDTuner extends LinearOpMode { + + Robot robot; + PIDController anglePID; + + boolean hasrun = false; + public static boolean align = false; + public static double kp = 0, ki = 0, kd = 0; + public static double error = 0; + private Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + String goalTag = "Red Goal"; + + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + + hasrun = false; + align = false; + boolean prevAlign = false; + + double kp = 0, ki = 0, kd = 0; + anglePID = new PIDController(kp, ki, kd); + + double prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + + waitForStart(); + if (isStopRequested()) return; + + while(opModeIsActive()){ + if (gamepad1.a || align) { + align = true; + } + if (gamepad1.b) { + align = false; + } + + if(prevAlign != align) { + anglePID.setPID(kp, ki, kd); + } + + fieldCentricDrive(); + prevAlign = align; + } + } + + public double turnPowerAngle(double angle) { + //AprilTagDetection goalTag = robot.getCamera().getGoalApriltag(); + + //if (goalTag == null) {return 0; } + + return anglePID.calculate(0, angle); + } + + private void fieldCentricDrive() { + double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; + double y = -gamepad1.left_stick_y * slowdown; + double x = gamepad1.left_stick_x * 1.1 * slowdown; + double alignVal = turnPowerAngle(error+prevHeading-robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); + if(alignVal == 0) { align = false; } + t.addData("Turn Power Lock on Tag: ", alignVal); + double rx = (alignVal != 0 && align) ? alignVal : gamepad1.right_stick_x * slowdown; + + double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); + + + double rotX = x * Math.cos(-heading) - y * Math.sin(-heading); + double rotY = x * Math.sin(-heading) + y * Math.cos(-heading); + + double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); + double frWheelPower = (rotY - rotX - rx) / denominator; + double flWheelPower = (rotY + rotX + rx) / denominator; + double brWheelPower = (rotY + rotX - rx) / denominator; + double blWheelPower = (rotY - rotX + rx) / denominator; + + robot.getDrivetrain().setWheelPowers(flWheelPower, frWheelPower, brWheelPower, blWheelPower); + + if (gamepad1.y) { + robot.getDrivetrain().resetImu(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index c342d8bcdbc9..20f74d73ae93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -56,8 +56,7 @@ public class riptideUtil { public static double LENS_FOCAL_LEN_INCHES = 0.15748; public static double SENSOR_HEIGHT = 0.086; - public static double HORIZ_FOV = 55; - public static double VERT_FOV = 55; + public static double CAM_FOV = 55; public static double MOTOR_POS_CONST = 1; // needs tuning From 8143f05df873f1054836ddf7d14388bc2f46e9e5 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Thu, 30 Oct 2025 19:19:24 -0700 Subject: [PATCH 15/19] angle pid tuner maybe done? --- .../ftc/teamcode/Tuning/AnglePIDTuner.java | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java index c3ad54f2df60..3595f7011a49 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java @@ -16,7 +16,7 @@ @Config -@TeleOp(name = "Angle Tuner", group = "Tuning") +@TeleOp(name = "Angle PID Tuner", group = "Tuning") public class AnglePIDTuner extends LinearOpMode { Robot robot; @@ -24,6 +24,7 @@ public class AnglePIDTuner extends LinearOpMode { boolean hasrun = false; public static boolean align = false; + boolean alignDebounce = false; public static double kp = 0, ki = 0, kd = 0; public static double error = 0; private Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); @@ -33,31 +34,31 @@ public class AnglePIDTuner extends LinearOpMode { public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - hasrun = false; - align = false; boolean prevAlign = false; - double kp = 0, ki = 0, kd = 0; anglePID = new PIDController(kp, ki, kd); - double prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + double prevHeading = 0; waitForStart(); if (isStopRequested()) return; - while(opModeIsActive()){ - if (gamepad1.a || align) { + while(opModeIsActive()) { + if (gamepad1.a && !alignDebounce) { align = true; + alignDebounce = true; + prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); } if (gamepad1.b) { align = false; + alignDebounce = false; } if(prevAlign != align) { anglePID.setPID(kp, ki, kd); } - fieldCentricDrive(); + fieldCentricDrive(prevHeading); prevAlign = align; } } @@ -70,18 +71,18 @@ public double turnPowerAngle(double angle) { return anglePID.calculate(0, angle); } - private void fieldCentricDrive() { + private void fieldCentricDrive(double prevHeading) { double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; double y = -gamepad1.left_stick_y * slowdown; double x = gamepad1.left_stick_x * 1.1 * slowdown; - double alignVal = turnPowerAngle(error+prevHeading-robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); - if(alignVal == 0) { align = false; } - t.addData("Turn Power Lock on Tag: ", alignVal); - double rx = (alignVal != 0 && align) ? alignVal : gamepad1.right_stick_x * slowdown; + error -= prevHeading - robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + double alignVal = turnPowerAngle(error); + if(alignVal == 0) { align = false; alignDebounce = false;} + t.addData("Turn Power: ", alignVal); + double rx = align ? alignVal : gamepad1.right_stick_x * slowdown; double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); - double rotX = x * Math.cos(-heading) - y * Math.sin(-heading); double rotY = x * Math.sin(-heading) + y * Math.cos(-heading); From 3a3cd7f7a113c7f62cff8dd65f424bd43b8b8443 Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 1 Nov 2025 07:16:04 -0700 Subject: [PATCH 16/19] i don\'t remember what I did, I also am very good at making commit messages --- .../ftc/teamcode/Modules/Drivetrain.java | 8 ++--- .../ftc/teamcode/Tuning/AnglePIDTuner.java | 29 ++++++++++++++----- 2 files changed, 26 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java index a601a8e1b9f3..910d17643a36 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Drivetrain.java @@ -48,9 +48,9 @@ public Drivetrain(HardwareMap hardwareMap) { brWheel = hardwareMap.dcMotor.get("brWheel"); blWheel = hardwareMap.dcMotor.get("blWheel"); - blWheel.setDirection(DcMotorSimple.Direction.REVERSE); + blWheel.setDirection(DcMotorSimple.Direction.FORWARD); flWheel.setDirection(DcMotorSimple.Direction.REVERSE); - frWheel.setDirection(DcMotorSimple.Direction.REVERSE); + frWheel.setDirection(DcMotorSimple.Direction.FORWARD); brWheel.setDirection(DcMotorSimple.Direction.REVERSE); brWheel.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -71,8 +71,8 @@ public Drivetrain(HardwareMap hardwareMap) { imu = hardwareMap.get(IMU.class, "imu"); IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot( - RevHubOrientationOnRobot.LogoFacingDirection.UP, - RevHubOrientationOnRobot.UsbFacingDirection.BACKWARD)); + RevHubOrientationOnRobot.LogoFacingDirection.RIGHT, + RevHubOrientationOnRobot.UsbFacingDirection.UP)); imu.resetYaw(); imu.initialize(parameters); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java index 3595f7011a49..a5a170482312 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java @@ -22,7 +22,6 @@ public class AnglePIDTuner extends LinearOpMode { Robot robot; PIDController anglePID; - boolean hasrun = false; public static boolean align = false; boolean alignDebounce = false; public static double kp = 0, ki = 0, kd = 0; @@ -40,22 +39,32 @@ public void runOpMode() throws InterruptedException { double prevHeading = 0; + waitForStart(); if (isStopRequested()) return; while(opModeIsActive()) { - if (gamepad1.a && !alignDebounce) { + if (gamepad1.x && !alignDebounce) { align = true; alignDebounce = true; - prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); } - if (gamepad1.b) { + + if(align && !alignDebounce) { + prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES) % 360; + } + + if (gamepad1.triangle) { align = false; alignDebounce = false; } + if(gamepad1.circle) { + prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES) % 360; + } + if(prevAlign != align) { anglePID.setPID(kp, ki, kd); + robot.getDrivetrain().resetImu(); } fieldCentricDrive(prevHeading); @@ -75,10 +84,16 @@ private void fieldCentricDrive(double prevHeading) { double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; double y = -gamepad1.left_stick_y * slowdown; double x = gamepad1.left_stick_x * 1.1 * slowdown; - error -= prevHeading - robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + error = prevHeading - robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); + error %= 360; double alignVal = turnPowerAngle(error); - if(alignVal == 0) { align = false; alignDebounce = false;} - t.addData("Turn Power: ", alignVal); + if(alignVal == 0 || error <= 2) { align = false; alignDebounce = false;} + t.addData("Current Heading", robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); + t.addData("Previous Heading", prevHeading); + t.addData("Turn Power", alignVal); + t.addData("Error", error); + t.update(); + double rx = align ? alignVal : gamepad1.right_stick_x * slowdown; double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); From d52af6e34a9563daf05cae087fcade95255c26bc Mon Sep 17 00:00:00 2001 From: MrWalium Date: Sat, 1 Nov 2025 07:49:42 -0700 Subject: [PATCH 17/19] fixed logic for the error --- .../ftc/teamcode/Tuning/AnglePIDTuner.java | 55 +++++++++---------- 1 file changed, 27 insertions(+), 28 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java index a5a170482312..b757b1e5ac43 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/AnglePIDTuner.java @@ -10,9 +10,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.PIDController; import org.firstinspires.ftc.teamcode.Robot; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; - -import java.util.ArrayList; @Config @@ -21,25 +18,24 @@ public class AnglePIDTuner extends LinearOpMode { Robot robot; PIDController anglePID; + private double initHeading = 0; public static boolean align = false; boolean alignDebounce = false; public static double kp = 0, ki = 0, kd = 0; + + public static double goal = 0; + private double prevGoal = 0; public static double error = 0; private Telemetry t = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - String goalTag = "Red Goal"; @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - boolean prevAlign = false; anglePID = new PIDController(kp, ki, kd); - double prevHeading = 0; - - waitForStart(); if (isStopRequested()) return; @@ -50,7 +46,7 @@ public void runOpMode() throws InterruptedException { } if(align && !alignDebounce) { - prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES) % 360; + initHeading = currHeading() % 360; } if (gamepad1.triangle) { @@ -59,7 +55,7 @@ public void runOpMode() throws InterruptedException { } if(gamepad1.circle) { - prevHeading = robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES) % 360; + initHeading = currHeading() % 360; } if(prevAlign != align) { @@ -67,39 +63,42 @@ public void runOpMode() throws InterruptedException { robot.getDrivetrain().resetImu(); } - fieldCentricDrive(prevHeading); + if(prevGoal != goal) { + initHeading = currHeading(); + anglePID.setPID(kp, ki, kd); + } + + fieldCentricDrive(); prevAlign = align; + prevGoal = goal; } } - public double turnPowerAngle(double angle) { - //AprilTagDetection goalTag = robot.getCamera().getGoalApriltag(); - - //if (goalTag == null) {return 0; } - - return anglePID.calculate(0, angle); + public double currHeading() { + return robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); } - private void fieldCentricDrive(double prevHeading) { + private void fieldCentricDrive() { double slowdown = gamepad1.right_trigger > 0 ? 0.25 : 1; double y = -gamepad1.left_stick_y * slowdown; double x = gamepad1.left_stick_x * 1.1 * slowdown; - error = prevHeading - robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES); - error %= 360; - double alignVal = turnPowerAngle(error); - if(alignVal == 0 || error <= 2) { align = false; alignDebounce = false;} - t.addData("Current Heading", robot.getDrivetrain().getRobotHeading(AngleUnit.DEGREES)); - t.addData("Previous Heading", prevHeading); + + double currHeading = currHeading(); + double goalHeading = initHeading*2 - currHeading(); + error = goalHeading - currHeading; + if(Math.abs(error) > 180) { error = (error + -Math.signum(error) * 180); } + double alignVal = anglePID.calculate(0, error); + if(alignVal == 0 || Math.abs(error) <= 2) { align = false; alignDebounce = false;} + t.addData("Current Heading", currHeading); + t.addData("Initial Heading", initHeading); t.addData("Turn Power", alignVal); t.addData("Error", error); t.update(); double rx = align ? alignVal : gamepad1.right_stick_x * slowdown; - double heading = robot.getDrivetrain().getRobotHeading(AngleUnit.RADIANS); - - double rotX = x * Math.cos(-heading) - y * Math.sin(-heading); - double rotY = x * Math.sin(-heading) + y * Math.cos(-heading); + double rotX = x * Math.cos(-currHeading) - y * Math.sin(-currHeading); + double rotY = x * Math.sin(-currHeading) + y * Math.cos(-currHeading); double denominator = Math.max(Math.abs(rotY) + Math.abs(rotX) + Math.abs(rx), 1); double frWheelPower = (rotY - rotX - rx) / denominator; From caa2faef5f390e3fd447fa07d6744e455b750d6a Mon Sep 17 00:00:00 2001 From: J A Date: Thu, 6 Nov 2025 04:50:58 -0800 Subject: [PATCH 18/19] changes --- .../ftc/teamcode/Modules/Camera.java | 78 ++++++++++++++++++- .../org/firstinspires/ftc/teamcode/Robot.java | 2 +- .../ftc/teamcode/Tuning/TestVision.java | 19 +++-- 3 files changed, 88 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 3786e7d9a2f0..3c60e12d1dce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,6 +1,10 @@ package org.firstinspires.ftc.teamcode.Modules; // --- CONSTANTS & OTHER STUFF --- // +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.riptideUtil; @@ -16,6 +20,8 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; import org.firstinspires.ftc.vision.opencv.ColorRange; + +import android.annotation.SuppressLint; import android.graphics.Color; import org.firstinspires.ftc.vision.opencv.Circle; @@ -28,6 +34,7 @@ import android.util.Size; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.SortOrder; /* @@ -61,6 +68,7 @@ public class Camera { ArrayList detections; ArrayList> blobs = new ArrayList<>(); String goalTag; + CameraName cameraname; public enum processors_enabled { NONE, TAG, @@ -74,8 +82,8 @@ public enum processors_enabled { //// ///// ////////////////////////////////////////////// - public Camera(CameraName cameraname) { - + public Camera(HardwareMap hardwareMap) { + cameraname = hardwareMap.get(WebcamName.class, "Webcam 1"); tag_processor = new AprilTagProcessor.Builder() .setTagLibrary(riptideUtil.getLibrary()) .setDrawAxes(true) @@ -327,4 +335,70 @@ public void resumeStreaming() { public void stop() { vision_portal.close(); } + + @SuppressLint("DefaultLocale") + public void distanceFromGoal(double x, double y, double z){ + + //telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + for (AprilTagDetection detection : detections) { + if (detection.metadata != null) { + //telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + if (!detection.metadata.name.contains("Obelisk")) { + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("Distance %f (inch)", + getAprilTagDistance(detection))); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } + } else { + telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); + } + + + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + x = detection.robotPose.getPosition().x; + y = detection.robotPose.getPosition().y; + z = detection.robotPose.getPosition().z; + } + + telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", blobs.size())); + + for (List blob : blobs) { + telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); + telemetry.addLine(String.format("Circularity: %f", blob.get(4))); + telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); + telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); + } + } + + public void tagAngle(double angle){ + + } + + public Double getGoalDistance() { + AprilTagDetection goalDetection = getGoalApriltag(); + if (goalDetection == null) { + return null; + } + + double x = goalDetection.robotPose.getPosition().x; + double y = goalDetection.robotPose.getPosition().y; + double z = goalDetection.robotPose.getPosition().z; + + return Math.sqrt(x * x + y * y + z * z); + } + + public Double getGoalAngleError() { + AprilTagDetection goalDetection = getGoalApriltag(); + if (goalDetection == null) { + return null; + } + + return getTagHorizontalAngle(goalDetection); + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index ecb866c5db2b..9edeb3cc2f77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -22,7 +22,7 @@ public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); - camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); + camera = new Camera(hardwareMap); table = new TurnTable(2, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index 889a9bd50ed4..f757d6ddbfea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.teamcode.Modules.Camera; +import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import java.util.ArrayList; @@ -16,13 +17,13 @@ @TeleOp(name="Test Vision") public class TestVision extends LinearOpMode { - + Robot robot; @Override public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); - Camera camera = new Camera(hardwareMap.get(WebcamName.class, "Webcam 1")); Camera.processors_enabled processor = Camera.processors_enabled.ALL; - camera.setPipeline(processor); + robot.getCamera().setPipeline(processor); boolean a_debounce = false; telemetry.setMsTransmissionInterval(100); // speed up telemetry updates for debugging @@ -37,15 +38,15 @@ public void runOpMode() throws InterruptedException { switch (processor) { case ALL: processor = Camera.processors_enabled.TAG; - camera.setPipeline(processor); + robot.getCamera().setPipeline(processor); break; case TAG: processor = Camera.processors_enabled.COLOR; - camera.setPipeline(processor); + robot.getCamera().setPipeline(processor); break; case COLOR: processor = Camera.processors_enabled.ALL; - camera.setPipeline(processor); + robot.getCamera().setPipeline(processor); break; } } @@ -53,10 +54,10 @@ public void runOpMode() throws InterruptedException { } else { a_debounce = false; } - telemetryStuff(camera); + telemetryStuff(robot.getCamera()); } - camera.stop(); + robot.getCamera().stop(); } @SuppressLint("DefaultLocale") @@ -101,3 +102,5 @@ public void telemetryStuff(Camera camera) { sleep(100); } } + + From c3792ad18552cfd5c82a290c52b7dcff635e60cd Mon Sep 17 00:00:00 2001 From: J A Date: Sun, 9 Nov 2025 21:02:27 -0800 Subject: [PATCH 19/19] added outtake class (without hardwarmap bc of errors) --- .../ftc/teamcode/Modules/Camera.java | 21 +- .../ftc/teamcode/Modules/Outtake.java | 221 ++++++++++++++++++ .../org/firstinspires/ftc/teamcode/Robot.java | 31 ++- .../ftc/teamcode/Tuning/TestVision.java | 46 ++-- .../teamcode/UnitTests/PowerOnDistance.java | 41 ++++ .../ftc/teamcode/riptideUtil.java | 45 +++- 6 files changed, 360 insertions(+), 45 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java index 3c60e12d1dce..342eb2534845 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Camera.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.Modules; // --- CONSTANTS & OTHER STUFF --- // -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -197,10 +196,15 @@ public ArrayList> getBlobDetections() { return blobs; } + public boolean isGoalTag(AprilTagDetection detection) { + // For red alliance goal + return detection.id == 24; + } + public AprilTagDetection getGoalApriltag() { detections = getTagDetections(); for(AprilTagDetection detection : detections) { - if(detection.metadata.name.equals(goalTag)) { + if(isGoalTag(detection)/* detection.metadata.name.equals(goalTag) */) { return detection; } } @@ -221,9 +225,10 @@ public EditablePose2D getGoalApriltagLocation() { } public double getAprilTagDistance(AprilTagDetection tag) { - return Math.pow(tag.robotPose.getPosition().x, 2) * - Math.pow(tag.robotPose.getPosition().y, 2) * - Math.pow(tag.robotPose.getPosition().z, 2); + double x = tag.robotPose.getPosition().x; + double y = tag.robotPose.getPosition().y; + double z = tag.robotPose.getPosition().z; + return Math.sqrt(x * x + y * y + z * z); } public double getTagHorizontalAngle(AprilTagDetection tag) { @@ -350,10 +355,6 @@ public void distanceFromGoal(double x, double y, double z){ detection.robotPose.getPosition().z)); telemetry.addLine(String.format("Distance %f (inch)", getAprilTagDistance(detection))); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", - detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); } } else { telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); @@ -390,6 +391,8 @@ public Double getGoalDistance() { double y = goalDetection.robotPose.getPosition().y; double z = goalDetection.robotPose.getPosition().z; + + return Math.sqrt(x * x + y * y + z * z); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java new file mode 100644 index 000000000000..2eb55de3d4fb --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Modules/Outtake.java @@ -0,0 +1,221 @@ +package org.firstinspires.ftc.teamcode.Modules; + +import static org.firstinspires.ftc.teamcode.riptideUtil.KPBottom; +import static org.firstinspires.ftc.teamcode.riptideUtil.KPTop; +import static org.firstinspires.ftc.teamcode.riptideUtil.TOP_FLYWHEEL_KP; +import static org.firstinspires.ftc.teamcode.riptideUtil.BOTTOM_FLYWHEEL_KP; +import static org.firstinspires.ftc.teamcode.riptideUtil.angularVelocity; +import static org.firstinspires.ftc.teamcode.riptideUtil.econserved; +import static org.firstinspires.ftc.teamcode.riptideUtil.tolerance; + +import android.annotation.SuppressLint; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.util.LinkedList; + +public class Outtake { +// private final DcMotor topFlywheel; +// private final DcMotor bottomFlywheel; + + private final PIDController flywheelVelocityControllerBottom = new PIDController(TOP_FLYWHEEL_KP, 0, 0); + private final PIDController flywheelVelocityControllerTop = new PIDController(BOTTOM_FLYWHEEL_KP, 0, 0); + + private boolean updatePID = false; + + + + // OUTTAKE TUNER THINGS + LinkedList topRecords = new LinkedList<>(); + LinkedList bottomRecords = new LinkedList<>(); + public static int queueSize = 3; + private static double rpmTopPrev = 360; + private static double rpmBottomPrev = 360; + + private double currPosTop; + private double currPosBottom; + private double startTime = System.nanoTime() / 1e9; + private PIDController RPMControllerTop = new PIDController(KPTop, 0, 0); + private PIDController RPMControllerBottom = new PIDController(KPBottom, 0, 0); + private double prevPosTop = 0; + private double prevPosBottom = 0; + public void runOuttakePID(double rpmTop, double rpmBottom, Telemetry tele){ + + pidtunedmotor(rpmTop, rpmBottom, tele); + + tele.addData("goalRPMTop", rpmTop); + tele.addData("goalRPMBottom", rpmBottom); + tele.update(); + if (rpmTopPrev != rpmTop) { + rpmTopPrev = rpmTop; + RPMControllerTop = new PIDController(KPTop, 0, 0); + + } + if (rpmBottomPrev != rpmBottom) { + rpmBottomPrev = rpmBottom; + RPMControllerBottom = new PIDController(KPBottom, 0, 0); + } + tele.update(); + } + + private boolean atGoalSpeed = false; + + + public void pidtunedmotor(double rpmTop, double rpmBottom, Telemetry telemetry) { + + prevPosTop = currPosTop; + prevPosBottom = currPosBottom; + +// currPosTop = currPosL(); +// currPosBottom = currPosR(); + + double dThetaTop = (currPosTop - prevPosTop)/28; + double dThetaBottom = (currPosBottom - prevPosBottom)/28; + + double dt = System.nanoTime() / 1e9 - startTime; + startTime = System.nanoTime() / 1e9; + + double currRPMTop = dThetaTop / (dt / 60); + double currRPMBottom = dThetaBottom / (dt / 60); + + topRecords.add(currRPMTop); + while (topRecords.size() > queueSize) + topRecords.remove(0); + + bottomRecords.add(currRPMBottom); + while (bottomRecords.size() > queueSize) + bottomRecords.remove(0); + + double undividedAverageBottom = 0; + double undividedAverageTop = 0; + + for (int i = 0; i < topRecords.size(); i++) { + undividedAverageTop += topRecords.get(i); + undividedAverageBottom += bottomRecords.get(i); + } + double averageTop; + double averageBottom; + if (topRecords.size() == queueSize) { + averageTop = undividedAverageTop / queueSize; + averageBottom = undividedAverageBottom / queueSize; + } else { + averageTop = currRPMTop; + averageBottom = currRPMBottom; + } + + //double averageTop = topRecords.size() >= queueSize ? (topRecords.get(0)+topRecords.get(1)+topRecords.get(2)+topRecords.get(3)+topRecords.get(4))/5 : currRPMTop; + //double averageBottom = bottomRecords.size() >= queueSize ? (bottomRecords.get(0)+bottomRecords.get(1)+bottomRecords.get(2)+bottomRecords.get(3)+bottomRecords.get(4))/5 : currRPMBottom; + + double wantedWheelPowerTopAverage = RPMControllerTop.calculate(averageTop - 200, rpmTop); + double wantedWheelPowerBottomAverage = RPMControllerBottom.calculate(averageBottom - 200, rpmBottom); + + + //setFlyWheelPower(rpmTop != 0 ? wantedWheelPowerTopAverage:0,rpmBottom != 0 ? wantedWheelPowerBottomAverage:0); + + telemetry.addData("ready", bottomRecords.size() >= queueSize); + telemetry.addData("top", averageTop); + telemetry.addData("bottom", averageBottom); + + + boolean atTopRPM = Math.abs(averageTop - rpmTop) <= tolerance; + boolean atBotRPM = Math.abs(averageBottom - rpmBottom) <= tolerance; + atGoalSpeed = atTopRPM && atBotRPM; + } + + public Outtake(HardwareMap hardwareMap){ + +// topFlywheel = hardwareMap.dcMotor.get("topFlywheel"); +// bottomFlywheel = hardwareMap.dcMotor.get("bottomFlywheel"); + +// topFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); +// bottomFlywheel.setDirection(DcMotorSimple.Direction.REVERSE); + } + +// public void start(double speed){ +//// bottomFlywheel.setPower(speed); +//// topFlywheel.setPower(speed); +// } +//// +// //This one method is only to set motors individually +// public void setFlyWheelPower(double speedL, double speedR) { +// bottomFlywheel.setPower(speedR); +// topFlywheel.setPower(speedL); +// +// } +// +// public void stop(){ +// bottomFlywheel.setPower(0); +// topFlywheel.setPower(0); +// } + + + public boolean isAtGoalSpeed(){ + return atGoalSpeed; + } + + + public void startFlywheel(){ + this.startTime = System.nanoTime() / 1e9; // Current Time in Seconds + } + +// public double currPos(){ +// double currPos = (double) (bottomFlywheel.getCurrentPosition() + topFlywheel.getCurrentPosition()) / 2; +// return currPos; +// } +// +// //Created to allow OuttakePIDTuner to access individual positions +// public double currPosL(){ +// return topFlywheel.getCurrentPosition(); +// } +// public double currPosR(){ +// return bottomFlywheel.getCurrentPosition(); +// } + + + @SuppressLint("DefaultLocale") + public void setPowerOnDist(Double dist /* INCHES */, boolean run, Telemetry tele){ + + + double rBottom = 1.41732; // INCHES + double rTop = 1.41732; // INCHES + double angle = Math.toRadians(75); + double y = 25 - rBottom; + + double term = dist * Math.tan(angle) - y; + if (term <= 0) { + tele.addLine("Invalid term: target too close or below launch point"); + tele.addData("term", term); + pidtunedmotor(0, 0, tele); + return; + } + + double numerator = 9.8 * 39.3701 * dist * dist; + double denominator = 2 * Math.pow(Math.cos(angle), 2) * term; + + double ballVelocity = Math.sqrt(numerator / denominator); + + double rpmTop; + double rpmBottom; + + if (run) { + rpmTop = ((ballVelocity * 60) / (2 * Math.PI * rTop)) * econserved - (angularVelocity * 30 / Math.PI) * econserved; + rpmBottom = ((ballVelocity * 60) / (2 * Math.PI * rBottom)) * econserved - (angularVelocity * 30 / Math.PI) * econserved; + } + + else {rpmTop = 0; rpmBottom = 0;} + pidtunedmotor(rpmTop, rpmBottom, tele); +// tele.addLine(String.format("Calculated rpm top: %f", rpmTop)); +// tele.addLine(String.format("Calculated rpm bottom: %f", rpmBottom)); + tele.update(); + } + + private double rpmTopGoal; + private double rpmBottomGoal; + + public void setOuttakeRPM (double rpmTop, double rpmBottom){rpmTopGoal = rpmTop; rpmBottomGoal = rpmBottom;} + public void runOuttakePID(Telemetry tele){pidtunedmotor(rpmTopGoal, rpmBottomGoal, tele);} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java index 9edeb3cc2f77..0ab70c074cd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -2,36 +2,43 @@ import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Modules.Camera; import org.firstinspires.ftc.teamcode.Modules.Drivetrain; -import org.firstinspires.ftc.teamcode.Modules.Slides; -import org.firstinspires.ftc.teamcode.Modules.TurnTable; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.teamcode.Modules.Outtake; public class Robot { HardwareMap hardwareMap; Drivetrain drivetrain; - Camera camera; - TurnTable table; + Outtake outtake; + + Camera camera; public Robot (HardwareMap hardwareMap){ this.hardwareMap = hardwareMap; drivetrain = new Drivetrain(hardwareMap); + + outtake = new Outtake(hardwareMap); + camera = new Camera(hardwareMap); - table = new TurnTable(2, 0, 0); } public Drivetrain getDrivetrain(){ return drivetrain; } - public Camera getCamera() { - return camera; - } - public TurnTable getTable() {return table;} -} + public Outtake getOuttake() {return outtake;} + + public Camera getCamera() {return camera;} + + public void setFlyWheelPowerOnDistance(boolean run, Telemetry tele){ + Double distance = camera.getGoalDistance(); + if (distance != null) { + outtake.setPowerOnDist(distance, run, tele); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java index f757d6ddbfea..454532754ce1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tuning/TestVision.java @@ -50,11 +50,15 @@ public void runOpMode() throws InterruptedException { break; } } + + + a_debounce = true; } else { a_debounce = false; } telemetryStuff(robot.getCamera()); + robot.setFlyWheelPowerOnDistance(true, telemetry); } robot.getCamera().stop(); @@ -65,38 +69,40 @@ public void telemetryStuff(Camera camera) { List detections = camera.getTagDetections(); ArrayList> color_blobs = camera.getBlobDetections(); - telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); + //telemetry.addLine(String.format(" --- %d AprilTags Detected --- ", detections.size())); for (AprilTagDetection detection : detections) { if (detection.metadata != null) { - telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); + // telemetry.addLine(String.format("%s (ID %d)", detection.metadata.name, detection.id)); if (!detection.metadata.name.contains("Obelisk")) { - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", - detection.robotPose.getPosition().x, - detection.robotPose.getPosition().y, - detection.robotPose.getPosition().z)); +// telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", +// detection.robotPose.getPosition().x, +// detection.robotPose.getPosition().y, +// detection.robotPose.getPosition().z)); telemetry.addLine(String.format("Distance %f (inch)", camera.getAprilTagDistance(detection))); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", - detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), - detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + telemetry.addLine(String.format("Goal Angle Error %f (Degrees)", + camera.getGoalAngleError())); +// telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", +// detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), +// detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), +// detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); } } else { - telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); + // telemetry.addLine(String.format("Unknown Name (ID %d)", detection.id)); } - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + // telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); } - telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", color_blobs.size())); - - for (List blob : color_blobs) { - telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); - telemetry.addLine(String.format("Circularity: %f", blob.get(4))); - telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); - telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); - } +// telemetry.addLine(String.format(" --- %d Artifacts Detected --- ", color_blobs.size())); +// +// for (List blob : color_blobs) { +// telemetry.addLine(String.format("Position: (%f, %f)", blob.get(0), blob.get(1))); +// telemetry.addLine(String.format("Circularity: %f", blob.get(4))); +// telemetry.addLine(String.format("Contour Area: %f", blob.get(3))); +// telemetry.addLine(String.format("Distance: %f Inches Away", blob.get(2))); +// } telemetry.update(); sleep(100); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java new file mode 100644 index 000000000000..c4abb085ec49 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/UnitTests/PowerOnDistance.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.UnitTests; + +import static org.firstinspires.ftc.teamcode.riptideUtil.angularVelocity; +import static org.firstinspires.ftc.teamcode.riptideUtil.econserved; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.Modules.Camera; +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +@Config +@TeleOp(name = "Power on Distance") +public class PowerOnDistance extends LinearOpMode { + Robot robot; + Telemetry tele = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + @Override + public void runOpMode(){ + robot = new Robot(hardwareMap); + + Camera.processors_enabled processor = Camera.processors_enabled.ALL; + robot.getCamera().setPipeline(processor); + + + waitForStart(); + while(opModeIsActive()){ + robot.setFlyWheelPowerOnDistance(true, tele); + tele.update(); + + for (AprilTagDetection d : robot.getCamera().getTagDetections()) { + telemetry.addLine("Detected tag name: " + d.metadata.name + " id: " + d.id); + } + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java index 20f74d73ae93..7a19ad542f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/riptideUtil.java @@ -2,9 +2,7 @@ import com.acmerobotics.dashboard.config.Config; -import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; import org.firstinspires.ftc.vision.apriltag.AprilTagLibrary; @Config @@ -17,7 +15,26 @@ public class riptideUtil { */ /** General constants */ + // COLOR SENSING + // Green + public static final float GREEN_R = 0.136f; + public static final float GREEN_R_STDEV = 0.05f; + public static final float GREEN_G = 0.578f; + public static final float GREEN_G_STDEV = 0.15f; + public static final float GREEN_B = 0.404f; + public static final float GREEN_B_STDEV = 0.13f; + // Purple + public static final float PURPLE_R = 0.346f; + public static final float PURPLE_R_STDEV = 0.02f; + public static final float PURPLE_G = 0.376f; + public static final float PURPLE_G_STDEV = 0.03f; + public static final float PURPLE_B = 0.686f; + public static final float PURPLE_B_STDEV = 0.04f; + //flywheel + + public static double TOP_FLYWHEEL_KP = 0.001; + public static double BOTTOM_FLYWHEEL_KP = 0.001; /** Autonomous Constants */ public static final double POINT_TOLERANCE = 2; // UNDETERMINED @@ -45,8 +62,25 @@ public class riptideUtil { public static double MAX_A = 72; // UNDETERMINED public static double MAX_V = 96; // UNDETERMINED - // ----- VISION ----- // + public static double LONG_DIST_TOP = 3250; // UNDETERMINED + public static double LONG_DIST_BOT = 3730; // UNDETERMINED + + public static double MID_DIST_TOP = 2650; // UNDETERMINED + public static double MID_DIST_BOT = 3000;// UNDETERMINED + + public static double SHORT_DIST_TOP = 2950; // UNDETERMINED + public static double SHORT_DIST_BOT = 3480;// UNDETERMINED + + public static double transferOpen = 0.8; // UNDETERMINED + public static double transferClosed = -0.8; // UNDETERMINED + public static double KPTop = 0.0032; //0.004 + public static double KPBottom = 0.004; // 0.0045 + + public static double tolerance = 50; + + + // Vision public static int CAMERA_WIDTH = 640; public static int CAMERA_HEIGHT = 480; public static double CAMERA_ANGLE = 0; @@ -98,4 +132,7 @@ public static AprilTagLibrary getLibrary() { ) .build(); } -} + + public static double angularVelocity = 37.5; + public static double econserved = 1; +} \ No newline at end of file