From 9ca692e35cbeef5e01571b75f1ee1016f88ee350 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Sun, 16 Jun 2024 16:18:50 +0300 Subject: [PATCH 01/23] Gil - added builders --- .../org/firstinspires/ftc/teamcode/Robot.java | 3 + .../ftc/teamcode/vision/Vision.java | 64 +++++++++++++++++++ .../ftc/teamcode/vision/VisionConstant.java | 19 ++++++ 3 files changed, 86 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java 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 0046dab..308d037 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.subsystems.launcher.Launcher; import org.firstinspires.ftc.teamcode.subsystems.elevator.Elevator; import org.firstinspires.ftc.teamcode.subsystems.arm.Arm; +import org.firstinspires.ftc.teamcode.vision.Vision; public class Robot { @@ -25,11 +26,13 @@ public static Robot getInstance() { private Arm arm; private Elevator elevator; private Launcher launcher; + private Vision vision; public void initSubsystems(HardwareMap hardwareMap) { this.arm = new Arm(hardwareMap); this.elevator = new Elevator(hardwareMap); this.launcher = new Launcher(hardwareMap); + this.vision = new Vision(hardwareMap); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java new file mode 100644 index 0000000..04c9ac3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.vision; + +import android.graphics.Canvas; +import android.util.Size; + +import com.arcrobotics.ftclib.command.SubsystemBase; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.firstinspires.ftc.vision.tfod.TfodProcessor; +import org.opencv.core.Mat; + +public class Vision extends SubsystemBase implements VisionProcessor { + + private VisionPortal visionPortal; + + public Vision(HardwareMap hardwareMap) { + + AprilTagProcessor.Builder aprilTagProcessorBuilder = new AprilTagProcessor.Builder() + .setDrawTagID(true) + .setDrawTagOutline(true) + .setDrawAxes(true) + .setDrawCubeProjection(true); + + AprilTagProcessor aprilTagProcessor = aprilTagProcessorBuilder.build(); + + TfodProcessor.Builder tfodProcessorBuilder = new TfodProcessor.Builder() + .setMaxNumRecognitions(VisionConstant.MAX_NUM_RECOGNITIONS) + .setUseObjectTracker(true) + .setTrackerMaxOverlap(VisionConstant.TRACKER_MAX_OVERLAP) + .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE); + + TfodProcessor tfodProcessor = tfodProcessorBuilder.build(); + + VisionPortal.Builder visionPortalBuilder = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) + .addProcessors(aprilTagProcessor,tfodProcessor) + .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH,VisionConstant.CAMERA_RESOLUTION_HEIGHT)) + .setStreamFormat(VisionConstant.STREAM_FORMAT) + .enableLiveView(true) + .setAutoStopLiveView(true); + + visionPortal = visionPortalBuilder.build(); + } + + @Override + public void init(int width, int height, CameraCalibration calibration) { + + } + + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + return null; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java new file mode 100644 index 0000000..edaf4f3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.vision; + +import org.firstinspires.ftc.vision.VisionPortal; +import org.openftc.easyopencv.OpenCvWebcam; + +public class VisionConstant { + + protected static final String CAMERA_ID = "Camera - 1"; + + protected static final int CAMERA_RESOLUTION_WIDTH = 640; + protected static final int CAMERA_RESOLUTION_HEIGHT = 480; + protected static final int TRACKER_MIN_SIZE = 16; + protected static final int MAX_NUM_RECOGNITIONS = 10; + + protected static final float TRACKER_MAX_OVERLAP = 0.2f; + + protected static final VisionPortal.StreamFormat STREAM_FORMAT = VisionPortal.StreamFormat.YUY2; + +} From dad2f840dc50c5007f805c11823d3d146af2884e Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Sun, 16 Jun 2024 17:02:31 +0300 Subject: [PATCH 02/23] Gil - updated the webcam's id --- .../org/firstinspires/ftc/teamcode/vision/VisionConstant.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java index edaf4f3..fcb1341 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java @@ -5,7 +5,7 @@ public class VisionConstant { - protected static final String CAMERA_ID = "Camera - 1"; + protected static final String CAMERA_ID = "Webcam1"; protected static final int CAMERA_RESOLUTION_WIDTH = 640; protected static final int CAMERA_RESOLUTION_HEIGHT = 480; From 2d69ce8a1e01a00383fd3bd4296c19f8d4b4429a Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Sun, 16 Jun 2024 18:53:51 +0300 Subject: [PATCH 03/23] Gil - changed something --- .../ftc/teamcode/vision/Vision.java | 66 +++++++++++-------- 1 file changed, 39 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java index 04c9ac3..83039e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java @@ -1,64 +1,76 @@ package org.firstinspires.ftc.teamcode.vision; -import android.graphics.Canvas; +import android.util.Pair; import android.util.Size; - import com.arcrobotics.ftclib.command.SubsystemBase; +import com.arcrobotics.ftclib.geometry.Vector2d; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.VisionProcessor; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; -import org.opencv.core.Mat; +import java.util.ArrayList; +import java.util.List; -public class Vision extends SubsystemBase implements VisionProcessor { +public class Vision extends SubsystemBase { private VisionPortal visionPortal; + private AprilTagProcessor aprilTagProcessor; + private TfodProcessor tfodProcessor; + private List aprilTagDetections; public Vision(HardwareMap hardwareMap) { - AprilTagProcessor.Builder aprilTagProcessorBuilder = new AprilTagProcessor.Builder() + aprilTagDetections = new ArrayList<>(); + + this.aprilTagProcessor = new AprilTagProcessor.Builder() .setDrawTagID(true) .setDrawTagOutline(true) .setDrawAxes(true) - .setDrawCubeProjection(true); - - AprilTagProcessor aprilTagProcessor = aprilTagProcessorBuilder.build(); + .setDrawCubeProjection(true) + .build(); - TfodProcessor.Builder tfodProcessorBuilder = new TfodProcessor.Builder() + this.tfodProcessor = new TfodProcessor.Builder() .setMaxNumRecognitions(VisionConstant.MAX_NUM_RECOGNITIONS) .setUseObjectTracker(true) .setTrackerMaxOverlap(VisionConstant.TRACKER_MAX_OVERLAP) - .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE); + .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) + .build(); - TfodProcessor tfodProcessor = tfodProcessorBuilder.build(); - - VisionPortal.Builder visionPortalBuilder = new VisionPortal.Builder() + this.visionPortal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) - .addProcessors(aprilTagProcessor,tfodProcessor) - .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH,VisionConstant.CAMERA_RESOLUTION_HEIGHT)) + .addProcessors(aprilTagProcessor, tfodProcessor) + .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH, VisionConstant.CAMERA_RESOLUTION_HEIGHT)) .setStreamFormat(VisionConstant.STREAM_FORMAT) .enableLiveView(true) - .setAutoStopLiveView(true); - - visionPortal = visionPortalBuilder.build(); + .setAutoStopLiveView(true) + .build(); } @Override - public void init(int width, int height, CameraCalibration calibration) { + public void periodic() { - } + aprilTagDetections = aprilTagProcessor.getDetections(); - @Override - public Object processFrame(Mat frame, long captureTimeNanos) { - return null; } - @Override - public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + public List getTagsDetections() { + return aprilTagDetections; + } + public List> getTagsPoses() { + List> poses = new ArrayList<>(); + for(AprilTagDetection detection : getTagsDetections()) + if(detection.metadata != null) + poses.add(new Pair<>(detection.ftcPose,detection.id)); + return poses; + } + public void telemetry(Telemetry telemetry) { + telemetry.addData("first tag x: ", getTagsPoses().get(0).first.x); } + } From 7196b1b1b87dea9295630dfeb3a17b370f162e55 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 15:03:02 +0300 Subject: [PATCH 04/23] Gil - added a test and a telemetry --- .../org/firstinspires/ftc/teamcode/Robot.java | 3 +++ .../opmodes/testers/RazVisionTest.java | 25 +++++++++++++++++++ .../ftc/teamcode/vision/Vision.java | 10 +++++--- 3 files changed, 34 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/testers/RazVisionTest.java 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 308d037..0bb3804 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -47,5 +47,8 @@ public Elevator getElevator() { public Launcher getLauncher() { return launcher; } + public Vision getVision() { + return vision; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/testers/RazVisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/testers/RazVisionTest.java new file mode 100644 index 0000000..767e3ad --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/testers/RazVisionTest.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.opmodes.testers; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; + +@TeleOp(name = "Raz Vision Test") +public class RazVisionTest extends DefaultRaz { + + @Override + public void initialize() { + + } + + @Override + public void execute() { + Robot.getInstance().getVision().telemetry(telemetry); + } + + @Override + public void configureBindings() { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java index 83039e7..95c4884 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java @@ -3,9 +3,7 @@ import android.util.Pair; import android.util.Size; import com.arcrobotics.ftclib.command.SubsystemBase; -import com.arcrobotics.ftclib.geometry.Vector2d; import com.qualcomm.robotcore.hardware.HardwareMap; - import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; @@ -62,15 +60,19 @@ public List getTagsDetections() { return aprilTagDetections; } public List> getTagsPoses() { + List temp = getTagsDetections(); List> poses = new ArrayList<>(); - for(AprilTagDetection detection : getTagsDetections()) + for(AprilTagDetection detection : temp) if(detection.metadata != null) poses.add(new Pair<>(detection.ftcPose,detection.id)); return poses; } public void telemetry(Telemetry telemetry) { - telemetry.addData("first tag x: ", getTagsPoses().get(0).first.x); + List> poses = getTagsPoses(); + telemetry.addData("first tag x: ", poses.isEmpty()? "null" : poses.get(0).first.x); + telemetry.addData("first tag y: ", poses.isEmpty()? "null" : poses.get(0).first.y); + telemetry.addData("first tag z: ", poses.isEmpty()? "null" : poses.get(0).first.z); } } From fca5551451360ab910ea684127e249da8bdd03fc Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 15:12:05 +0300 Subject: [PATCH 05/23] Gil - changes to the telemetry --- .../java/org/firstinspires/ftc/teamcode/Robot.java | 2 +- .../teamcode/{ => subsystems}/vision/Vision.java | 14 +++++++++----- .../{ => subsystems}/vision/VisionConstant.java | 2 +- 3 files changed, 11 insertions(+), 7 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystems}/vision/Vision.java (85%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{ => subsystems}/vision/VisionConstant.java (91%) 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 0bb3804..88e516d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -5,7 +5,7 @@ import org.firstinspires.ftc.teamcode.subsystems.launcher.Launcher; import org.firstinspires.ftc.teamcode.subsystems.elevator.Elevator; import org.firstinspires.ftc.teamcode.subsystems.arm.Arm; -import org.firstinspires.ftc.teamcode.vision.Vision; +import org.firstinspires.ftc.teamcode.subsystems.vision.Vision; public class Robot { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java similarity index 85% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 95c4884..faf2830 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.vision; +package org.firstinspires.ftc.teamcode.subsystems.vision; import android.util.Pair; import android.util.Size; @@ -70,9 +70,13 @@ public List> getTagsPoses() { public void telemetry(Telemetry telemetry) { List> poses = getTagsPoses(); - telemetry.addData("first tag x: ", poses.isEmpty()? "null" : poses.get(0).first.x); - telemetry.addData("first tag y: ", poses.isEmpty()? "null" : poses.get(0).first.y); - telemetry.addData("first tag z: ", poses.isEmpty()? "null" : poses.get(0).first.z); + if(!poses.isEmpty()) { + telemetry.addData("detected: ", poses.size() + " AprilTags"); + telemetry.addData("the first tag is: ", poses.get(0).second); + telemetry.addData("first tag x: ", poses.get(0).first.x); + telemetry.addData("first tag y: ", poses.get(0).first.y); + telemetry.addData("first tag z: ", poses.get(0).first.z); + } } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java similarity index 91% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index fcb1341..379c0b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.vision; +package org.firstinspires.ftc.teamcode.subsystems.vision; import org.firstinspires.ftc.vision.VisionPortal; import org.openftc.easyopencv.OpenCvWebcam; From c5ffcd63683d167e7bc951b4350122a51b7d4d33 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 16:34:46 +0300 Subject: [PATCH 06/23] Gil - saving changes --- .../subsystems/vision/ObjectProcessor.java | 78 +++++++++++++++++++ .../vision/TeamObjectDetection.java | 16 ++++ .../teamcode/subsystems/vision/Vision.java | 18 ++++- .../subsystems/vision/VisionConstant.java | 1 + 4 files changed, 109 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java new file mode 100644 index 0000000..48f1918 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java @@ -0,0 +1,78 @@ +package org.firstinspires.ftc.teamcode.subsystems.vision; + +import android.graphics.Canvas; + +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.core.Mat; + +public class ObjectProcessor implements VisionProcessor { + + private boolean isBlueAlliance; + private TeamObjectDetection objectDetection; + + public ObjectProcessor(boolean isBlueAlliance) { + this.isBlueAlliance = isBlueAlliance; + } + + private boolean isPixelOfObject(double[] pixel) { + if(isBlueAlliance) { + return pixel[0]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2] && + pixel[1]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2]; + } + else { + return pixel[2]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[0] && + pixel[1]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[0]; + } + } + + private int[] getObjectCoords(Mat frame) { + int avgX = 0; + int avgY = 0; + int pixelCount = 0; + + for(int i = 0; i < frame.width(); i++) + for(int j = 0; j < frame.height(); j++) + if(isPixelOfObject(frame.get(i,j))) { + avgX += i; + avgY += j; + pixelCount++; + } + + if(pixelCount != 0) { + avgX /= pixelCount; + avgY /= pixelCount; + return new int[]{avgX,avgY}; + } + else { + return null; + } + + } + + public TeamObjectDetection getLastDetection() { + return objectDetection; + } + + @Override + public void init(int width, int height, CameraCalibration calibration) { + + } + + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + + int[] coordinates = getObjectCoords(frame); + + if(coordinates != null) { + this.objectDetection = new TeamObjectDetection(coordinates[0],coordinates[1], frame.width(), frame.height()); + return objectDetection; + } + return null; + } + + @Override + public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java new file mode 100644 index 0000000..c1890f6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.teamcode.subsystems.vision; + +public class TeamObjectDetection { + + public int x; + public int y; + public int frameWidth; + public int frameHeight; + + public TeamObjectDetection(int x, int y, int frameWidth, int frameHeight) { + this.x = x; + this.y = y; + this.frameWidth = frameWidth; + this.frameHeight = frameHeight; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index faf2830..bab3e8e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -1,25 +1,33 @@ package org.firstinspires.ftc.teamcode.subsystems.vision; +import android.graphics.Canvas; import android.util.Pair; import android.util.Size; import com.arcrobotics.ftclib.command.SubsystemBase; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; +import org.opencv.core.Mat; + import java.util.ArrayList; import java.util.List; public class Vision extends SubsystemBase { - private VisionPortal visionPortal; private AprilTagProcessor aprilTagProcessor; private TfodProcessor tfodProcessor; + private ObjectProcessor objectProcessor; + private VisionPortal visionPortal; private List aprilTagDetections; + private TeamObjectDetection teamObjectDetection; public Vision(HardwareMap hardwareMap) { @@ -39,9 +47,11 @@ public Vision(HardwareMap hardwareMap) { .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) .build(); + objectProcessor = new ObjectProcessor(true); + this.visionPortal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) - .addProcessors(aprilTagProcessor, tfodProcessor) + .addProcessors(aprilTagProcessor, tfodProcessor, objectProcessor) .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH, VisionConstant.CAMERA_RESOLUTION_HEIGHT)) .setStreamFormat(VisionConstant.STREAM_FORMAT) .enableLiveView(true) @@ -52,7 +62,8 @@ public Vision(HardwareMap hardwareMap) { @Override public void periodic() { - aprilTagDetections = aprilTagProcessor.getDetections(); + this.aprilTagDetections = aprilTagProcessor.getDetections(); + this.teamObjectDetection = objectProcessor.getLastDetection(); } @@ -78,5 +89,4 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("first tag z: ", poses.get(0).first.z); } } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index 379c0b1..dcdfb4f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -13,6 +13,7 @@ public class VisionConstant { protected static final int MAX_NUM_RECOGNITIONS = 10; protected static final float TRACKER_MAX_OVERLAP = 0.2f; + protected static final float PERCENTAGE_OF_ALLIANCE_COLOR = 2f; protected static final VisionPortal.StreamFormat STREAM_FORMAT = VisionPortal.StreamFormat.YUY2; From 85333aec0eade5fe84ee17fbdf38b57be0055f20 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 21:38:39 +0300 Subject: [PATCH 07/23] Gil - little changes --- .../ftc/teamcode/subsystems/vision/ObjectProcessor.java | 7 +++++-- .../ftc/teamcode/subsystems/vision/Vision.java | 6 +++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java index 48f1918..1833205 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java @@ -16,6 +16,9 @@ public ObjectProcessor(boolean isBlueAlliance) { } private boolean isPixelOfObject(double[] pixel) { + if(pixel == null || pixel.length < 3) { + return false; + } if(isBlueAlliance) { return pixel[0]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2] && pixel[1]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2]; @@ -31,8 +34,8 @@ private int[] getObjectCoords(Mat frame) { int avgY = 0; int pixelCount = 0; - for(int i = 0; i < frame.width(); i++) - for(int j = 0; j < frame.height(); j++) + for(int i = 0; i < frame.width(); i+=2) + for(int j = 0; j < frame.height(); j+=2) if(isPixelOfObject(frame.get(i,j))) { avgX += i; avgY += j; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index bab3e8e..ffcae2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -47,7 +47,7 @@ public Vision(HardwareMap hardwareMap) { .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) .build(); - objectProcessor = new ObjectProcessor(true); + objectProcessor = new ObjectProcessor(false); this.visionPortal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) @@ -88,5 +88,9 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("first tag y: ", poses.get(0).first.y); telemetry.addData("first tag z: ", poses.get(0).first.z); } + if(teamObjectDetection != null) { + telemetry.addData("object X on screen: ", teamObjectDetection.x); + telemetry.addData("object Y on screen: ", teamObjectDetection.y); + } } } \ No newline at end of file From 7c860e1624aa458f6dd5af27f0711bc1977bdee3 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 22:54:29 +0300 Subject: [PATCH 08/23] Gil - changed the whole prop detection pipeline --- .../firstinspires/ftc/teamcode/Alliance.java | 6 ++ .../ftc/teamcode/RobotConstants.java | 10 ++ .../teamcode/subsystems/vision/Location.java | 25 +++++ .../subsystems/vision/ObjectProcessor.java | 99 +++++++++---------- .../teamcode/subsystems/vision/Vision.java | 20 ++-- .../subsystems/vision/VisionConstant.java | 10 +- 6 files changed, 103 insertions(+), 67 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java new file mode 100644 index 0000000..7cf9fe6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java @@ -0,0 +1,6 @@ +package org.firstinspires.ftc.teamcode; + +public enum Alliance { + BLUE, + RED +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java new file mode 100644 index 0000000..fc640a7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.teamcode; + +import org.firstinspires.ftc.teamcode.subsystems.vision.Location; + +public class RobotConstants { + + public static final Alliance ALLIANCE = Alliance.BLUE; + public static final Location SIDE = Location.FAR; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java new file mode 100644 index 0000000..50351b0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.subsystems.vision; + +public enum Location { + LEFT, CENTER, RIGHT, + FAR, CLOSE; + + @Override + public String toString() { + if(this == Location.LEFT) { + return "LEFT"; + } + else if(this == Location.CENTER) { + return "CENTER"; + } + else if(this == Location.RIGHT) { + return "RIGHT"; + } + else if(this == Location.FAR) { + return "FAR"; + } + else { + return "CLOSE"; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java index 1833205..f449488 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java @@ -1,76 +1,67 @@ package org.firstinspires.ftc.teamcode.subsystems.vision; import android.graphics.Canvas; - import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.teamcode.Alliance; +import org.firstinspires.ftc.teamcode.RobotConstants; import org.firstinspires.ftc.vision.VisionProcessor; +import org.opencv.core.Core; import org.opencv.core.Mat; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; public class ObjectProcessor implements VisionProcessor { - private boolean isBlueAlliance; - private TeamObjectDetection objectDetection; + private Location location; - public ObjectProcessor(boolean isBlueAlliance) { - this.isBlueAlliance = isBlueAlliance; - } + @Override + public void init(int width, int height, CameraCalibration calibration) { - private boolean isPixelOfObject(double[] pixel) { - if(pixel == null || pixel.length < 3) { - return false; - } - if(isBlueAlliance) { - return pixel[0]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2] && - pixel[1]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[2]; - } - else { - return pixel[2]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[0] && - pixel[1]*VisionConstant.PERCENTAGE_OF_ALLIANCE_COLOR < pixel[0]; - } } - private int[] getObjectCoords(Mat frame) { - int avgX = 0; - int avgY = 0; - int pixelCount = 0; - - for(int i = 0; i < frame.width(); i+=2) - for(int j = 0; j < frame.height(); j+=2) - if(isPixelOfObject(frame.get(i,j))) { - avgX += i; - avgY += j; - pixelCount++; - } - - if(pixelCount != 0) { - avgX /= pixelCount; - avgY /= pixelCount; - return new int[]{avgX,avgY}; - } - else { - return null; + @Override + public Object processFrame(Mat frame, long captureTimeNanos) { + Rect leftZoneArea; + Rect centerZoneArea; + + if (RobotConstants.ALLIANCE == Alliance.RED && RobotConstants.SIDE == Location.FAR || RobotConstants.ALLIANCE == Alliance.BLUE && RobotConstants.SIDE == Location.CLOSE) { + leftZoneArea = VisionConstant.RED_LEFT_ZONE_AREA; + centerZoneArea = VisionConstant.RED_CENTER_ZONE_AREA; + } else { + leftZoneArea = VisionConstant.BLUE_LEFT_ZONE_AREA; + centerZoneArea = VisionConstant.BLUE_CENTER_ZONE_AREA; } - } - - public TeamObjectDetection getLastDetection() { - return objectDetection; - } + Mat leftZone = frame.submat(leftZoneArea); + Mat centerZone = frame.submat(centerZoneArea); - @Override - public void init(int width, int height, CameraCalibration calibration) { + Imgproc.blur(leftZone, leftZone, new Size(5, 5)); + Imgproc.blur(centerZone, centerZone, new Size(5, 5)); - } + Scalar left = Core.mean(leftZone); + Scalar center = Core.mean(centerZone); - @Override - public Object processFrame(Mat frame, long captureTimeNanos) { + double threshold = RobotConstants.ALLIANCE == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; + int idx = RobotConstants.ALLIANCE == Alliance.RED ? 0 : 2; - int[] coordinates = getObjectCoords(frame); + double leftColor = left.val[idx]; + double centerColor = center.val[idx]; - if(coordinates != null) { - this.objectDetection = new TeamObjectDetection(coordinates[0],coordinates[1], frame.width(), frame.height()); - return objectDetection; + if (leftColor > threshold && (left.val[0] + left.val[1] + left.val[2] - left.val[idx] < left.val[idx])) { + this.location = Location.LEFT; + Imgproc.rectangle(frame, leftZoneArea, new Scalar(255, 255, 255), 10); + } else if (centerColor > threshold && (center.val[0] + center.val[1] + center.val[2] - center.val[idx] < center.val[idx])) { + this.location = Location.CENTER; + Imgproc.rectangle(frame, centerZoneArea, new Scalar(255, 255, 255), 10); + } else { + this.location = Location.RIGHT; } + + leftZone.release(); + centerZone.release(); + return null; } @@ -78,4 +69,8 @@ public Object processFrame(Mat frame, long captureTimeNanos) { public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { } + + public Location getLocation() { + return location; + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index ffcae2e..3ac1f05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -1,21 +1,16 @@ package org.firstinspires.ftc.teamcode.subsystems.vision; -import android.graphics.Canvas; import android.util.Pair; import android.util.Size; import com.arcrobotics.ftclib.command.SubsystemBase; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.tfod.Recognition; -import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.firstinspires.ftc.vision.VisionPortal; -import org.firstinspires.ftc.vision.VisionProcessor; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; -import org.opencv.core.Mat; import java.util.ArrayList; import java.util.List; @@ -24,10 +19,10 @@ public class Vision extends SubsystemBase { private AprilTagProcessor aprilTagProcessor; private TfodProcessor tfodProcessor; - private ObjectProcessor objectProcessor; + private ObjectProcessor propProcessor; private VisionPortal visionPortal; private List aprilTagDetections; - private TeamObjectDetection teamObjectDetection; + private Location propLocation; public Vision(HardwareMap hardwareMap) { @@ -47,11 +42,11 @@ public Vision(HardwareMap hardwareMap) { .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) .build(); - objectProcessor = new ObjectProcessor(false); + propProcessor = new ObjectProcessor(); this.visionPortal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) - .addProcessors(aprilTagProcessor, tfodProcessor, objectProcessor) + .addProcessors(aprilTagProcessor, tfodProcessor, propProcessor) .setCameraResolution(new Size(VisionConstant.CAMERA_RESOLUTION_WIDTH, VisionConstant.CAMERA_RESOLUTION_HEIGHT)) .setStreamFormat(VisionConstant.STREAM_FORMAT) .enableLiveView(true) @@ -63,7 +58,7 @@ public Vision(HardwareMap hardwareMap) { public void periodic() { this.aprilTagDetections = aprilTagProcessor.getDetections(); - this.teamObjectDetection = objectProcessor.getLastDetection(); + this.propLocation = propProcessor.getLocation(); } @@ -88,9 +83,8 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("first tag y: ", poses.get(0).first.y); telemetry.addData("first tag z: ", poses.get(0).first.z); } - if(teamObjectDetection != null) { - telemetry.addData("object X on screen: ", teamObjectDetection.x); - telemetry.addData("object Y on screen: ", teamObjectDetection.y); + if(propLocation != null) { + telemetry.addData("prop's location is: ", propLocation.toString()); } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index dcdfb4f..62069b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.subsystems.vision; import org.firstinspires.ftc.vision.VisionPortal; +import org.opencv.core.Rect; import org.openftc.easyopencv.OpenCvWebcam; -public class VisionConstant { +class VisionConstant { protected static final String CAMERA_ID = "Webcam1"; @@ -13,8 +14,13 @@ public class VisionConstant { protected static final int MAX_NUM_RECOGNITIONS = 10; protected static final float TRACKER_MAX_OVERLAP = 0.2f; - protected static final float PERCENTAGE_OF_ALLIANCE_COLOR = 2f; protected static final VisionPortal.StreamFormat STREAM_FORMAT = VisionPortal.StreamFormat.YUY2; + protected static final double BLUE_THRESHOLD = 70; + protected static final double RED_THRESHOLD = 100; + protected static final Rect RED_LEFT_ZONE_AREA = new Rect(815, 550, 175, 100); + protected static final Rect RED_CENTER_ZONE_AREA = new Rect(1365, 475, 125, 125); + protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(240, 525, 175, 100); + protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(925, 485, 125, 125); } From 2059166e221cda002dd20c74ce8ef54aa5db701b Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 23:08:41 +0300 Subject: [PATCH 09/23] Gil - changed magic numbers to constants --- .../subsystems/vision/ObjectProcessor.java | 15 +++++++-------- .../subsystems/vision/VisionConstant.java | 8 ++++++++ 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java index f449488..75755e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java @@ -9,7 +9,6 @@ import org.opencv.core.Mat; import org.opencv.core.Rect; import org.opencv.core.Scalar; -import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; public class ObjectProcessor implements VisionProcessor { @@ -37,24 +36,24 @@ public Object processFrame(Mat frame, long captureTimeNanos) { Mat leftZone = frame.submat(leftZoneArea); Mat centerZone = frame.submat(centerZoneArea); - Imgproc.blur(leftZone, leftZone, new Size(5, 5)); - Imgproc.blur(centerZone, centerZone, new Size(5, 5)); + Imgproc.blur(leftZone, leftZone, VisionConstant.BLUR_SIZE); + Imgproc.blur(centerZone, centerZone, VisionConstant.BLUR_SIZE); Scalar left = Core.mean(leftZone); Scalar center = Core.mean(centerZone); double threshold = RobotConstants.ALLIANCE == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; - int idx = RobotConstants.ALLIANCE == Alliance.RED ? 0 : 2; + int idx = RobotConstants.ALLIANCE == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; double leftColor = left.val[idx]; double centerColor = center.val[idx]; - if (leftColor > threshold && (left.val[0] + left.val[1] + left.val[2] - left.val[idx] < left.val[idx])) { + if (leftColor > threshold && (left.val[VisionConstant.RED_INDEX] + left.val[VisionConstant.GREEN_INDEX] + left.val[VisionConstant.BLUE_INDEX] - left.val[idx] < left.val[idx])) { this.location = Location.LEFT; - Imgproc.rectangle(frame, leftZoneArea, new Scalar(255, 255, 255), 10); - } else if (centerColor > threshold && (center.val[0] + center.val[1] + center.val[2] - center.val[idx] < center.val[idx])) { + Imgproc.rectangle(frame, leftZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); + } else if (centerColor > threshold && (center.val[VisionConstant.RED_INDEX] + center.val[VisionConstant.GREEN_INDEX] + center.val[VisionConstant.BLUE_INDEX] - center.val[idx] < center.val[idx])) { this.location = Location.CENTER; - Imgproc.rectangle(frame, centerZoneArea, new Scalar(255, 255, 255), 10); + Imgproc.rectangle(frame, centerZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); } else { this.location = Location.RIGHT; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index 62069b4..4e6f37f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -2,6 +2,8 @@ import org.firstinspires.ftc.vision.VisionPortal; import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; import org.openftc.easyopencv.OpenCvWebcam; class VisionConstant { @@ -23,4 +25,10 @@ class VisionConstant { protected static final Rect RED_CENTER_ZONE_AREA = new Rect(1365, 475, 125, 125); protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(240, 525, 175, 100); protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(925, 485, 125, 125); + protected static final Size BLUR_SIZE = new Size(5, 5); + protected static final Scalar WHITE_COLOR_RGB = new Scalar(255,255,255); + protected static final int AREA_RECTANGLE_THICKNESS = 10; + protected static final int RED_INDEX = 0; + protected static final int GREEN_INDEX = 1; + protected static final int BLUE_INDEX = 2; } From 28a60c60783ab01b3ec82140f4014ae82e9db182 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Mon, 17 Jun 2024 23:20:13 +0300 Subject: [PATCH 10/23] Gil - change the objectProcessor to PropProcessor --- .../vision/{ObjectProcessor.java => PropProcessor.java} | 2 +- .../firstinspires/ftc/teamcode/subsystems/vision/Vision.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/{ObjectProcessor.java => PropProcessor.java} (98%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java similarity index 98% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 75755e8..6b118e0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/ObjectProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -11,7 +11,7 @@ import org.opencv.core.Scalar; import org.opencv.imgproc.Imgproc; -public class ObjectProcessor implements VisionProcessor { +public class PropProcessor implements VisionProcessor { private Location location; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 3ac1f05..cfcd9f7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -19,7 +19,7 @@ public class Vision extends SubsystemBase { private AprilTagProcessor aprilTagProcessor; private TfodProcessor tfodProcessor; - private ObjectProcessor propProcessor; + private PropProcessor propProcessor; private VisionPortal visionPortal; private List aprilTagDetections; private Location propLocation; @@ -42,7 +42,7 @@ public Vision(HardwareMap hardwareMap) { .setTrackerMinSize(VisionConstant.TRACKER_MIN_SIZE) .build(); - propProcessor = new ObjectProcessor(); + propProcessor = new PropProcessor(); this.visionPortal = new VisionPortal.Builder() .setCamera(hardwareMap.get(WebcamName.class, VisionConstant.CAMERA_ID)) From 42c5959a321548487d05bff90cfa06869f10c64d Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Tue, 18 Jun 2024 12:38:46 +0300 Subject: [PATCH 11/23] Gil - scaled the frame in prop detection --- .../subsystems/vision/TeamObjectDetection.java | 16 ---------------- .../subsystems/vision/VisionConstant.java | 8 ++++---- 2 files changed, 4 insertions(+), 20 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java deleted file mode 100644 index c1890f6..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/TeamObjectDetection.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems.vision; - -public class TeamObjectDetection { - - public int x; - public int y; - public int frameWidth; - public int frameHeight; - - public TeamObjectDetection(int x, int y, int frameWidth, int frameHeight) { - this.x = x; - this.y = y; - this.frameWidth = frameWidth; - this.frameHeight = frameHeight; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index 4e6f37f..11c337e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -21,10 +21,10 @@ class VisionConstant { protected static final double BLUE_THRESHOLD = 70; protected static final double RED_THRESHOLD = 100; - protected static final Rect RED_LEFT_ZONE_AREA = new Rect(815, 550, 175, 100); - protected static final Rect RED_CENTER_ZONE_AREA = new Rect(1365, 475, 125, 125); - protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(240, 525, 175, 100); - protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(925, 485, 125, 125); + protected static final Rect RED_LEFT_ZONE_AREA = new Rect(271, 244, 58, 44); + protected static final Rect RED_CENTER_ZONE_AREA = new Rect(455, 211, 41, 55); + protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(80, 233, 58, 44); + protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(308, 215, 41, 55); protected static final Size BLUR_SIZE = new Size(5, 5); protected static final Scalar WHITE_COLOR_RGB = new Scalar(255,255,255); protected static final int AREA_RECTANGLE_THICKNESS = 10; From 413686783ba0298727cf5577137c0a2aa1e0513d Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 12:05:30 +0300 Subject: [PATCH 12/23] Gil - changed the rectangles of the areas --- .../ftc/teamcode/subsystems/vision/PropProcessor.java | 6 +++--- .../ftc/teamcode/subsystems/vision/VisionConstant.java | 10 +++++----- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 6b118e0..3e69877 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -25,7 +25,7 @@ public Object processFrame(Mat frame, long captureTimeNanos) { Rect leftZoneArea; Rect centerZoneArea; - if (RobotConstants.ALLIANCE == Alliance.RED && RobotConstants.SIDE == Location.FAR || RobotConstants.ALLIANCE == Alliance.BLUE && RobotConstants.SIDE == Location.CLOSE) { + if (RobotConstants.ALLIANCE == Alliance.RED) { leftZoneArea = VisionConstant.RED_LEFT_ZONE_AREA; centerZoneArea = VisionConstant.RED_CENTER_ZONE_AREA; } else { @@ -48,10 +48,10 @@ public Object processFrame(Mat frame, long captureTimeNanos) { double leftColor = left.val[idx]; double centerColor = center.val[idx]; - if (leftColor > threshold && (left.val[VisionConstant.RED_INDEX] + left.val[VisionConstant.GREEN_INDEX] + left.val[VisionConstant.BLUE_INDEX] - left.val[idx] < left.val[idx])) { + if (leftColor > threshold && (left.val[VisionConstant.RED_INDEX] + left.val[VisionConstant.GREEN_INDEX] + left.val[VisionConstant.BLUE_INDEX] < VisionConstant.AMOUNT_OF_COLOR * left.val[idx])) { this.location = Location.LEFT; Imgproc.rectangle(frame, leftZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); - } else if (centerColor > threshold && (center.val[VisionConstant.RED_INDEX] + center.val[VisionConstant.GREEN_INDEX] + center.val[VisionConstant.BLUE_INDEX] - center.val[idx] < center.val[idx])) { + } else if (centerColor > threshold && (center.val[VisionConstant.RED_INDEX] + center.val[VisionConstant.GREEN_INDEX] + center.val[VisionConstant.BLUE_INDEX] < VisionConstant.AMOUNT_OF_COLOR * center.val[idx])) { this.location = Location.CENTER; Imgproc.rectangle(frame, centerZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); } else { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index 11c337e..ea22137 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -4,7 +4,6 @@ import org.opencv.core.Rect; import org.opencv.core.Scalar; import org.opencv.core.Size; -import org.openftc.easyopencv.OpenCvWebcam; class VisionConstant { @@ -21,10 +20,11 @@ class VisionConstant { protected static final double BLUE_THRESHOLD = 70; protected static final double RED_THRESHOLD = 100; - protected static final Rect RED_LEFT_ZONE_AREA = new Rect(271, 244, 58, 44); - protected static final Rect RED_CENTER_ZONE_AREA = new Rect(455, 211, 41, 55); - protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(80, 233, 58, 44); - protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(308, 215, 41, 55); + protected static final double AMOUNT_OF_COLOR = 2.8; + protected static final Rect RED_LEFT_ZONE_AREA = new Rect(0, 244, 320, 236); + protected static final Rect RED_CENTER_ZONE_AREA = new Rect(320, 211, 320, 269); + protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(0, 233, 320, 247); + protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(320, 215, 320, 265); protected static final Size BLUR_SIZE = new Size(5, 5); protected static final Scalar WHITE_COLOR_RGB = new Scalar(255,255,255); protected static final int AREA_RECTANGLE_THICKNESS = 10; From 13a4ff3bc0871eeb61460e9f7356c405119eb560 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 13:29:18 +0300 Subject: [PATCH 13/23] Gil - optimizations --- .../subsystems/vision/PropProcessor.java | 1 + .../teamcode/subsystems/vision/Vision.java | 28 ++++++------------- 2 files changed, 10 insertions(+), 19 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 3e69877..7196ded 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -48,6 +48,7 @@ public Object processFrame(Mat frame, long captureTimeNanos) { double leftColor = left.val[idx]; double centerColor = center.val[idx]; + if (leftColor > threshold && (left.val[VisionConstant.RED_INDEX] + left.val[VisionConstant.GREEN_INDEX] + left.val[VisionConstant.BLUE_INDEX] < VisionConstant.AMOUNT_OF_COLOR * left.val[idx])) { this.location = Location.LEFT; Imgproc.rectangle(frame, leftZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index cfcd9f7..8029fb3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -17,17 +17,13 @@ public class Vision extends SubsystemBase { - private AprilTagProcessor aprilTagProcessor; - private TfodProcessor tfodProcessor; - private PropProcessor propProcessor; - private VisionPortal visionPortal; - private List aprilTagDetections; - private Location propLocation; + private final AprilTagProcessor aprilTagProcessor; + private final TfodProcessor tfodProcessor; + private final PropProcessor propProcessor; + private final VisionPortal visionPortal; public Vision(HardwareMap hardwareMap) { - aprilTagDetections = new ArrayList<>(); - this.aprilTagProcessor = new AprilTagProcessor.Builder() .setDrawTagID(true) .setDrawTagOutline(true) @@ -54,18 +50,10 @@ public Vision(HardwareMap hardwareMap) { .build(); } - @Override - public void periodic() { - - this.aprilTagDetections = aprilTagProcessor.getDetections(); - this.propLocation = propProcessor.getLocation(); - - } - public List getTagsDetections() { - return aprilTagDetections; + return aprilTagProcessor.getDetections(); } - public List> getTagsPoses() { + public List> getRelativeTagsPoses() { List temp = getTagsDetections(); List> poses = new ArrayList<>(); for(AprilTagDetection detection : temp) @@ -75,7 +63,7 @@ public List> getTagsPoses() { } public void telemetry(Telemetry telemetry) { - List> poses = getTagsPoses(); + List> poses = getRelativeTagsPoses(); if(!poses.isEmpty()) { telemetry.addData("detected: ", poses.size() + " AprilTags"); telemetry.addData("the first tag is: ", poses.get(0).second); @@ -83,6 +71,8 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("first tag y: ", poses.get(0).first.y); telemetry.addData("first tag z: ", poses.get(0).first.z); } + Location propLocation = propProcessor.getLocation(); + if(propLocation != null) { telemetry.addData("prop's location is: ", propLocation.toString()); } From 8cb28b2d39e0dbd591c548fe5027dd9c8494e9c6 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 13:43:51 +0300 Subject: [PATCH 14/23] Gil - spaces --- .../ftc/teamcode/subsystems/vision/PropProcessor.java | 1 + .../firstinspires/ftc/teamcode/subsystems/vision/Vision.java | 2 +- .../ftc/teamcode/subsystems/vision/VisionConstant.java | 5 +++++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 7196ded..1d7efe8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -73,4 +73,5 @@ public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, fl public Location getLocation() { return location; } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 8029fb3..9ba3745 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -11,7 +11,6 @@ import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; import org.firstinspires.ftc.vision.tfod.TfodProcessor; - import java.util.ArrayList; import java.util.List; @@ -77,4 +76,5 @@ public void telemetry(Telemetry telemetry) { telemetry.addData("prop's location is: ", propLocation.toString()); } } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index ea22137..9238194 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -21,14 +21,19 @@ class VisionConstant { protected static final double BLUE_THRESHOLD = 70; protected static final double RED_THRESHOLD = 100; protected static final double AMOUNT_OF_COLOR = 2.8; + protected static final Rect RED_LEFT_ZONE_AREA = new Rect(0, 244, 320, 236); protected static final Rect RED_CENTER_ZONE_AREA = new Rect(320, 211, 320, 269); protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(0, 233, 320, 247); protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(320, 215, 320, 265); + protected static final Size BLUR_SIZE = new Size(5, 5); + protected static final Scalar WHITE_COLOR_RGB = new Scalar(255,255,255); + protected static final int AREA_RECTANGLE_THICKNESS = 10; protected static final int RED_INDEX = 0; protected static final int GREEN_INDEX = 1; protected static final int BLUE_INDEX = 2; + } From 58fd53c9aa41fefde240b79e7d55b9581843b465 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 13:47:45 +0300 Subject: [PATCH 15/23] Gil - more spaces --- .../src/main/java/org/firstinspires/ftc/teamcode/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 88e516d..3505280 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,6 @@ public static Robot getInstance() { return instance; } - private Arm arm; private Elevator elevator; private Launcher launcher; @@ -47,6 +46,7 @@ public Elevator getElevator() { public Launcher getLauncher() { return launcher; } + public Vision getVision() { return vision; } From af9a943bd34b9f1809bed9b708ec624ce3ae8c02 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 13:51:19 +0300 Subject: [PATCH 16/23] Gil - more spaces --- .../org/firstinspires/ftc/teamcode/Alliance.java | 14 +++++++++++++- .../teamcode/{subsystems/vision => }/Location.java | 4 +++- .../firstinspires/ftc/teamcode/RobotConstants.java | 2 -- .../teamcode/subsystems/vision/PropProcessor.java | 1 + .../ftc/teamcode/subsystems/vision/Vision.java | 1 + 5 files changed, 18 insertions(+), 4 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{subsystems/vision => }/Location.java (89%) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java index 7cf9fe6..7c63333 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java @@ -1,6 +1,18 @@ package org.firstinspires.ftc.teamcode; public enum Alliance { + BLUE, - RED + RED; + + @Override + public String toString() { + if(this == Alliance.BLUE) { + return "BLUE"; + } + else { + return "RED"; + } + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java similarity index 89% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java index 50351b0..ac0fe33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Location.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java @@ -1,6 +1,7 @@ -package org.firstinspires.ftc.teamcode.subsystems.vision; +package org.firstinspires.ftc.teamcode; public enum Location { + LEFT, CENTER, RIGHT, FAR, CLOSE; @@ -22,4 +23,5 @@ else if(this == Location.FAR) { return "CLOSE"; } } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java index fc640a7..0e65d85 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode; -import org.firstinspires.ftc.teamcode.subsystems.vision.Location; - public class RobotConstants { public static final Alliance ALLIANCE = Alliance.BLUE; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 1d7efe8..61b8af8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -3,6 +3,7 @@ import android.graphics.Canvas; import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.firstinspires.ftc.teamcode.Alliance; +import org.firstinspires.ftc.teamcode.Location; import org.firstinspires.ftc.teamcode.RobotConstants; import org.firstinspires.ftc.vision.VisionProcessor; import org.opencv.core.Core; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 9ba3745..68630f4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.teamcode.Location; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; From 23c6aae900dc4ee38c6aa1755e522af173d2b085 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 14:05:54 +0300 Subject: [PATCH 17/23] Gil - no more far or close --- .../src/main/java/org/firstinspires/ftc/teamcode/Location.java | 3 +-- .../java/org/firstinspires/ftc/teamcode/RobotConstants.java | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java index 5f44d34..9924aba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java @@ -2,7 +2,6 @@ public enum Location { - LEFT, CENTER, RIGHT, - FAR, CLOSE; + LEFT, CENTER, RIGHT } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java index 0e65d85..855d8a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java @@ -3,6 +3,5 @@ public class RobotConstants { public static final Alliance ALLIANCE = Alliance.BLUE; - public static final Location SIDE = Location.FAR; } From c2132c8baeedd5a81468c75af967c0e7de6d4aea Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 14:28:37 +0300 Subject: [PATCH 18/23] Gil - added alliance initialization --- .../java/org/firstinspires/ftc/teamcode/Robot.java | 13 +++++++++++-- .../firstinspires/ftc/teamcode/RobotConstants.java | 7 ------- .../ftc/teamcode/opmodes/DefaultRaz.java | 3 +++ .../robotstatetesters/RazRobotStateTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazArmTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazChassisTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazClawTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazElevatorTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazLauncherTest.java | 6 ++++++ .../opmodes/subsystemstesters/RazVisionTest.java | 7 +++++++ .../opmodes/subsystemstesters/RazWristTest.java | 6 ++++++ .../teamcode/subsystems/vision/PropProcessor.java | 8 ++++---- 12 files changed, 67 insertions(+), 13 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java 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 18d6eff..ea757a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -24,8 +24,8 @@ public static Robot getInstance() { } return instance; } - - + + private Alliance alliance; private RobotState currentState; private Arm arm; private MecanumChassis chassis; @@ -35,8 +35,13 @@ public static Robot getInstance() { private Vision vision; private Wrist wrist; + public void setAlliance(Alliance alliance) { + this.alliance = alliance; + } + public void initSubsystems(HardwareMap hardwareMap) { this.currentState = RobotState.DRIVE; + this.arm = new Arm(hardwareMap); this.chassis = new MecanumChassis(hardwareMap); this.claw = new Claw(hardwareMap); @@ -65,6 +70,10 @@ public RobotState getCurrentState() { return currentState; } + public Alliance getAlliance() { + return alliance; + } + public Arm getArm() { return arm; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java deleted file mode 100644 index 855d8a1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotConstants.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -public class RobotConstants { - - public static final Alliance ALLIANCE = Alliance.BLUE; - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java index 5f3dc95..1e47150 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java @@ -3,12 +3,14 @@ import com.arcrobotics.ftclib.command.CommandScheduler; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Robot; public abstract class DefaultRaz extends OpMode { @Override public final void init() { + Robot.getInstance().setAlliance(setAlliance()); Robot.init(hardwareMap); configureBindings(); initialize(); @@ -21,6 +23,7 @@ public final void loop() { } public abstract void initialize(); + public abstract Alliance setAlliance(); public abstract void execute(); public abstract void configureBindings(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java index 4e60f01..9e3afcc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java @@ -3,6 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -15,6 +16,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { telemetry.addData("Current Robot State: ", Robot.getInstance().getCurrentState()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java index 69b6acd..715a3f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -14,6 +15,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getArm().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java index 134413c..4090f70 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -13,6 +14,11 @@ public class RazChassisTest extends DefaultRaz { public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getChassis().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java index 555059f..d661734 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -14,6 +15,11 @@ public void initialize() { Robot.getInstance().getClaw().openBothFingers(); } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getClaw().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java index 88204db..ffc53f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -14,6 +15,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getElevator().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java index b2433f5..11ea867 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmodes.subsystemstesters; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -14,6 +15,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getLauncher().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java index 0ff118f..21afcc1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java @@ -1,6 +1,8 @@ package org.firstinspires.ftc.teamcode.opmodes.subsystemstesters; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -12,6 +14,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getVision().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java index c26cf8f..75b0dac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java @@ -2,6 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Bindings; import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.teamcode.opmodes.DefaultRaz; @@ -14,6 +15,11 @@ public void initialize() { } + @Override + public Alliance setAlliance() { + return Alliance.RED; + } + @Override public void execute() { Robot.getInstance().getWrist().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 61b8af8..1f6ec51 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -4,7 +4,7 @@ import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; import org.firstinspires.ftc.teamcode.Alliance; import org.firstinspires.ftc.teamcode.Location; -import org.firstinspires.ftc.teamcode.RobotConstants; +import org.firstinspires.ftc.teamcode.Robot; import org.firstinspires.ftc.vision.VisionProcessor; import org.opencv.core.Core; import org.opencv.core.Mat; @@ -26,7 +26,7 @@ public Object processFrame(Mat frame, long captureTimeNanos) { Rect leftZoneArea; Rect centerZoneArea; - if (RobotConstants.ALLIANCE == Alliance.RED) { + if (Robot.getInstance().getAlliance() == Alliance.RED) { leftZoneArea = VisionConstant.RED_LEFT_ZONE_AREA; centerZoneArea = VisionConstant.RED_CENTER_ZONE_AREA; } else { @@ -43,8 +43,8 @@ public Object processFrame(Mat frame, long captureTimeNanos) { Scalar left = Core.mean(leftZone); Scalar center = Core.mean(centerZone); - double threshold = RobotConstants.ALLIANCE == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; - int idx = RobotConstants.ALLIANCE == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; + double threshold = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; + int idx = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; double leftColor = left.val[idx]; double centerColor = center.val[idx]; From 280233bca82ba9b490c6ed3214ce087584ee0deb Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 14:49:41 +0300 Subject: [PATCH 19/23] Gil - added DefaultGil for autonomous --- .../ftc/teamcode/autos/DefaultGil.java | 19 +++++++++++++++++++ .../ftc/teamcode/opmodes/DefaultRaz.java | 5 ++++- .../opmodes/subsystemstesters/RazArmTest.java | 5 ----- .../subsystemstesters/RazChassisTest.java | 5 ----- .../subsystemstesters/RazClawTest.java | 5 ----- .../subsystemstesters/RazElevatorTest.java | 5 ----- .../subsystemstesters/RazLauncherTest.java | 5 ----- .../subsystemstesters/RazVisionTest.java | 5 ----- .../subsystemstesters/RazWristTest.java | 5 ----- 9 files changed, 23 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java new file mode 100644 index 0000000..b642b40 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/DefaultGil.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.autos; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.Alliance; +import org.firstinspires.ftc.teamcode.Robot; + +public abstract class DefaultGil extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + Robot.getInstance().setAlliance(setAlliance()); + run(); + } + + public abstract Alliance setAlliance(); + public abstract void run() throws InterruptedException; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java index 1e47150..9921b01 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java @@ -22,8 +22,11 @@ public final void loop() { execute(); } + public Alliance setAlliance() { + return Alliance.RED; + } + public abstract void initialize(); - public abstract Alliance setAlliance(); public abstract void execute(); public abstract void configureBindings(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java index 715a3f9..c8f2247 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazArmTest.java @@ -15,11 +15,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getArm().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java index 4090f70..3730b19 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazChassisTest.java @@ -14,11 +14,6 @@ public class RazChassisTest extends DefaultRaz { public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getChassis().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java index d661734..cb4d3da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazClawTest.java @@ -15,11 +15,6 @@ public void initialize() { Robot.getInstance().getClaw().openBothFingers(); } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getClaw().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java index ffc53f9..4df719b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazElevatorTest.java @@ -15,11 +15,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getElevator().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java index 11ea867..99d85d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazLauncherTest.java @@ -15,11 +15,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getLauncher().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java index 21afcc1..ab9c59c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java @@ -14,11 +14,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getVision().telemetry(telemetry); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java index 75b0dac..63287c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazWristTest.java @@ -15,11 +15,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { Robot.getInstance().getWrist().telemetry(telemetry); From 4de73a9ed0a0730488714f58b7a1c66c8a3deb79 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 15:37:43 +0300 Subject: [PATCH 20/23] Gil - code is now clean --- .../firstinspires/ftc/teamcode/Alliance.java | 2 +- .../ftc/teamcode/opmodes/DefaultRaz.java | 5 -- .../robotstatetesters/RazRobotStateTest.java | 5 -- .../subsystemstesters/RazVisionTest.java | 1 + .../subsystems/vision/PropProcessor.java | 73 +++++++++++-------- .../teamcode/subsystems/vision/Vision.java | 2 +- 6 files changed, 47 insertions(+), 41 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java index 0b1dd41..cdf45a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Alliance.java @@ -3,6 +3,6 @@ public enum Alliance { BLUE, - RED; + RED } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java index 9921b01..5ddd411 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/DefaultRaz.java @@ -10,7 +10,6 @@ public abstract class DefaultRaz extends OpMode { @Override public final void init() { - Robot.getInstance().setAlliance(setAlliance()); Robot.init(hardwareMap); configureBindings(); initialize(); @@ -22,10 +21,6 @@ public final void loop() { execute(); } - public Alliance setAlliance() { - return Alliance.RED; - } - public abstract void initialize(); public abstract void execute(); public abstract void configureBindings(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java index 9e3afcc..e87370b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/robotstatetesters/RazRobotStateTest.java @@ -16,11 +16,6 @@ public void initialize() { } - @Override - public Alliance setAlliance() { - return Alliance.RED; - } - @Override public void execute() { telemetry.addData("Current Robot State: ", Robot.getInstance().getCurrentState()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java index ab9c59c..40c8691 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/subsystemstesters/RazVisionTest.java @@ -23,4 +23,5 @@ public void execute() { public void configureBindings() { } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index 1f6ec51..e0b7c86 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -23,40 +23,20 @@ public void init(int width, int height, CameraCalibration calibration) { @Override public Object processFrame(Mat frame, long captureTimeNanos) { - Rect leftZoneArea; - Rect centerZoneArea; - if (Robot.getInstance().getAlliance() == Alliance.RED) { - leftZoneArea = VisionConstant.RED_LEFT_ZONE_AREA; - centerZoneArea = VisionConstant.RED_CENTER_ZONE_AREA; - } else { - leftZoneArea = VisionConstant.BLUE_LEFT_ZONE_AREA; - centerZoneArea = VisionConstant.BLUE_CENTER_ZONE_AREA; - } - - Mat leftZone = frame.submat(leftZoneArea); - Mat centerZone = frame.submat(centerZoneArea); - - Imgproc.blur(leftZone, leftZone, VisionConstant.BLUR_SIZE); - Imgproc.blur(centerZone, centerZone, VisionConstant.BLUR_SIZE); - - Scalar left = Core.mean(leftZone); - Scalar center = Core.mean(centerZone); - - double threshold = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; - int idx = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; - - double leftColor = left.val[idx]; - double centerColor = center.val[idx]; + Mat leftZone = getLeftZoneMatrix(frame); + Mat centerZone = getCenterZoneMatrix(frame); + Scalar left = getAvgColor(leftZone); + Scalar center = getAvgColor(centerZone); - if (leftColor > threshold && (left.val[VisionConstant.RED_INDEX] + left.val[VisionConstant.GREEN_INDEX] + left.val[VisionConstant.BLUE_INDEX] < VisionConstant.AMOUNT_OF_COLOR * left.val[idx])) { + if (isPropPartOfAvgColor(left)) { this.location = Location.LEFT; - Imgproc.rectangle(frame, leftZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); - } else if (centerColor > threshold && (center.val[VisionConstant.RED_INDEX] + center.val[VisionConstant.GREEN_INDEX] + center.val[VisionConstant.BLUE_INDEX] < VisionConstant.AMOUNT_OF_COLOR * center.val[idx])) { + } + else if (isPropPartOfAvgColor(center)) { this.location = Location.CENTER; - Imgproc.rectangle(frame, centerZoneArea, VisionConstant.WHITE_COLOR_RGB, VisionConstant.AREA_RECTANGLE_THICKNESS); - } else { + } + else { this.location = Location.RIGHT; } @@ -66,6 +46,41 @@ public Object processFrame(Mat frame, long captureTimeNanos) { return null; } + public Mat getLeftZoneMatrix(Mat frame) { + if (Robot.getInstance().getAlliance() == Alliance.RED) { + return frame.submat(VisionConstant.RED_LEFT_ZONE_AREA); + } + else { + return frame.submat(VisionConstant.BLUE_LEFT_ZONE_AREA); + } + } + + public Mat getCenterZoneMatrix(Mat frame) { + if (Robot.getInstance().getAlliance() == Alliance.RED) { + return frame.submat(VisionConstant.RED_CENTER_ZONE_AREA); + } + else { + return frame.submat(VisionConstant.BLUE_CENTER_ZONE_AREA); + } + } + + public Scalar getAvgColor(Mat matrix) { + Imgproc.blur(matrix, matrix, VisionConstant.BLUR_SIZE); + return Core.mean(matrix); + } + + public double getAllianceColorThreshold() { + return Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_THRESHOLD : VisionConstant.BLUE_THRESHOLD; + } + + public boolean isPropPartOfAvgColor(Scalar color) { + int allianceColorIndex = Robot.getInstance().getAlliance() == Alliance.RED ? VisionConstant.RED_INDEX : VisionConstant.BLUE_INDEX; + double allianceColor = color.val[allianceColorIndex]; + boolean allianceColorPassesThreshold = allianceColor > getAllianceColorThreshold(); + double sumOfColor = color.val[VisionConstant.RED_INDEX] + color.val[VisionConstant.GREEN_INDEX] + color.val[VisionConstant.BLUE_INDEX]; + return allianceColorPassesThreshold && sumOfColor < VisionConstant.AMOUNT_OF_COLOR * allianceColor; + } + @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 68630f4..969bbb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -23,7 +23,6 @@ public class Vision extends SubsystemBase { private final VisionPortal visionPortal; public Vision(HardwareMap hardwareMap) { - this.aprilTagProcessor = new AprilTagProcessor.Builder() .setDrawTagID(true) .setDrawTagOutline(true) @@ -53,6 +52,7 @@ public Vision(HardwareMap hardwareMap) { public List getTagsDetections() { return aprilTagProcessor.getDetections(); } + public List> getRelativeTagsPoses() { List temp = getTagsDetections(); List> poses = new ArrayList<>(); From d792606a3ce16532a27c6eaec3541dd2f29c0edc Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 16:05:30 +0300 Subject: [PATCH 21/23] Gil - adjusted the frames crop --- .../java/org/firstinspires/ftc/teamcode/Robot.java | 4 ++++ .../teamcode/subsystems/vision/VisionConstant.java | 11 ++++------- 2 files changed, 8 insertions(+), 7 deletions(-) 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 ea757a5..41162a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -18,6 +18,10 @@ public static void init(HardwareMap hardwareMap) { getInstance().initSubsystems(hardwareMap); } + private Robot() { + this.alliance = Alliance.RED; + } + public static Robot getInstance() { if (instance == null) { instance = new Robot(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java index 9238194..89b942b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/VisionConstant.java @@ -22,16 +22,13 @@ class VisionConstant { protected static final double RED_THRESHOLD = 100; protected static final double AMOUNT_OF_COLOR = 2.8; - protected static final Rect RED_LEFT_ZONE_AREA = new Rect(0, 244, 320, 236); - protected static final Rect RED_CENTER_ZONE_AREA = new Rect(320, 211, 320, 269); - protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(0, 233, 320, 247); - protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(320, 215, 320, 265); + protected static final Rect RED_LEFT_ZONE_AREA = new Rect(0, 330, 100, 150); + protected static final Rect RED_CENTER_ZONE_AREA = new Rect(320, 330, 320, 150); + protected static final Rect BLUE_LEFT_ZONE_AREA = new Rect(0, 330, 320, 150); + protected static final Rect BLUE_CENTER_ZONE_AREA = new Rect(380, 330, 100, 150); protected static final Size BLUR_SIZE = new Size(5, 5); - protected static final Scalar WHITE_COLOR_RGB = new Scalar(255,255,255); - - protected static final int AREA_RECTANGLE_THICKNESS = 10; protected static final int RED_INDEX = 0; protected static final int GREEN_INDEX = 1; protected static final int BLUE_INDEX = 2; From a3e7952d2b38e403d2ac171d1be7a959c5693a07 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 16:42:31 +0300 Subject: [PATCH 22/23] Gil - spaces --- .../main/java/org/firstinspires/ftc/teamcode/Location.java | 4 +++- .../ftc/teamcode/subsystems/vision/PropProcessor.java | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java index 9924aba..e2288c1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Location.java @@ -2,6 +2,8 @@ public enum Location { - LEFT, CENTER, RIGHT + LEFT, + CENTER, + RIGHT } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java index e0b7c86..2f199d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/PropProcessor.java @@ -23,7 +23,6 @@ public void init(int width, int height, CameraCalibration calibration) { @Override public Object processFrame(Mat frame, long captureTimeNanos) { - Mat leftZone = getLeftZoneMatrix(frame); Mat centerZone = getCenterZoneMatrix(frame); From 434c42292d393977a9b8335e9f96bf47ca280e71 Mon Sep 17 00:00:00 2001 From: GilStein1 Date: Wed, 19 Jun 2024 17:23:43 +0300 Subject: [PATCH 23/23] Gil - idk --- .../main/java/org/firstinspires/ftc/teamcode/Robot.java | 8 ++++---- .../ftc/teamcode/subsystems/vision/Vision.java | 8 +++++--- 2 files changed, 9 insertions(+), 7 deletions(-) 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 41162a8..fc49d1e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java @@ -18,10 +18,6 @@ public static void init(HardwareMap hardwareMap) { getInstance().initSubsystems(hardwareMap); } - private Robot() { - this.alliance = Alliance.RED; - } - public static Robot getInstance() { if (instance == null) { instance = new Robot(); @@ -39,6 +35,10 @@ public static Robot getInstance() { private Vision vision; private Wrist wrist; + private Robot() { + this.alliance = Alliance.RED; + } + public void setAlliance(Alliance alliance) { this.alliance = alliance; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java index 969bbb0..7f2135b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/vision/Vision.java @@ -56,9 +56,11 @@ public List getTagsDetections() { public List> getRelativeTagsPoses() { List temp = getTagsDetections(); List> poses = new ArrayList<>(); - for(AprilTagDetection detection : temp) - if(detection.metadata != null) - poses.add(new Pair<>(detection.ftcPose,detection.id)); + for(AprilTagDetection detection : temp) { + if (detection.metadata != null) { + poses.add(new Pair<>(detection.ftcPose, detection.id)); + } + } return poses; }