From 87399f0670dde687a1581fa516f8e78cc2ac155a Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Wed, 20 Mar 2024 17:37:42 -0500 Subject: [PATCH] Try to fix camera. --- .../ftc/teamcode/opmodes/AutoBlue.java | 4 +- .../ftc/teamcode/opmodes/AutoBlueFarStem.java | 4 +- .../ftc/teamcode/vision/Camera.java | 72 ++++--------------- .../vision/PropDetectionPipeline.java | 9 +-- 4 files changed, 18 insertions(+), 71 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java index 3874e81..8a32ad3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java @@ -75,8 +75,8 @@ public class AutoBlue extends LinearOpMode { // Do super fancy chinese shit while (!this.isStarted()) { - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); - randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); this.telemetry.update(); } boardScore(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java index 881dd63..05ce233 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFarStem.java @@ -132,8 +132,8 @@ public class AutoBlueFarStem extends LinearOpMode { // Do super fancy chinese shit while (!this.isStarted()) { - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); - randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); if (gamepad2.dpad_left) { parkLocation="LEFT"; } else if (gamepad2.dpad_right) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java index 966da71..2fa6dcc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java @@ -67,37 +67,27 @@ public class Camera { this.initialized = true; } - // Close the Targeting Camera - public void stopTargetingCamera() { - if (targetingCameraInitialized) { - targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false); - } - } - - // Get the Red Goal Detection - public Detection getRed() { - return targetingCameraInitialized ? prop.getRed() : INVALID_DETECTION; - } - - public void setAlliance(Alliance alliance) { - this.prop.setAlliance(alliance); - } - public StarterPosition getStartingPosition() { - if (!targetingCameraInitialized) { + Detection red = this.prop.getRed(); + Detection blue = this.prop.getBlue(); + + double redArea = red.getArea(); + double blueArea = blue.getArea(); + + return getStartingPosition(redArea > blueArea ? red : blue); + } + + private static StarterPosition getStartingPosition(Detection detection) { + if (detection == null) { return StarterPosition.UNKNOWN; } - Detection detection = this.getRed(); - if (detection == null || !detection.isValid()) { - return StarterPosition.UNKNOWN; - } Point center = detection.getCenter(); if (center == null) { return StarterPosition.UNKNOWN; } - double centerX = this.getRed().getCenter().x + 50; + double centerX = detection.getCenter().x + 50; if (centerX < 33) { return StarterPosition.LEFT; } else if (centerX < 66) { @@ -106,51 +96,14 @@ public class Camera { return StarterPosition.RIGHT; } - - return StarterPosition.UNKNOWN; } - public Detection getBlue() { - return targetingCameraInitialized ? prop.getBlue() : INVALID_DETECTION; - } - - public StarterPositionBlue getStartingPositionBlue() { - if (!targetingCameraInitialized) { - return StarterPositionBlue.UNKNOWN; - } - - Detection detection = this.getBlue(); - if (detection == null || !detection.isValid()) { - return StarterPositionBlue.UNKNOWN; - } - Point center = detection.getCenter(); - if (center == null) { - return StarterPositionBlue.UNKNOWN; - } - - double centerX = this.getBlue().getCenter().x + 50; - if (centerX < 33) { - return StarterPositionBlue.LEFT; - } else if (centerX < 66) { - return StarterPositionBlue.CENTER; - } else if (centerX < 100) { - return StarterPositionBlue.RIGHT; - } - - - return StarterPositionBlue.UNKNOWN; - } public enum StarterPosition { UNKNOWN, LEFT, CENTER, RIGHT } - public enum StarterPositionBlue { - UNKNOWN, LEFT, CENTER, RIGHT - } - - public AprilTagDetection getBestAprilTagDetection() { return this.aprilTag.getDetections() .stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin)) @@ -164,5 +117,4 @@ public class Camera { } return this.aprilTagPoseEstimator.estimatePose(detection); } - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java index dbbc07f..5728b7f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java @@ -55,13 +55,8 @@ public class PropDetectionPipeline implements VisionProcessor { Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); - if (alliance == Alliance.Red) { - red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2)); - } - - if (alliance == Alliance.Blue) { - blue.setContour(detect(FTC_BLUE_RANGE)); - } + red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2)); + blue.setContour(detect(FTC_BLUE_RANGE)); return input; }