From 1aa49e979d00122a0b2ec8ec694ca52fe293df06 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 15:17:17 -0600 Subject: [PATCH] Working auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 19 +++---------------- .../ftc/teamcode/hardware/Camera.java | 17 ++++------------- .../vision/PropDetectionPipeline.java | 19 ++++++++++++------- 3 files changed, 19 insertions(+), 36 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index ffebca3..8ae0c69 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTAN import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Center; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Left; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right; +import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -167,7 +168,7 @@ public abstract class AutoBase extends LinearOpMode { private void dislodgePropAndPlacePixelCenter() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(rendezvous); + builder.lineToConstantHeading(rendezvous.vec()); builder.addDisplacementMarker(10, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); @@ -178,7 +179,7 @@ public abstract class AutoBase extends LinearOpMode { } private void determinePropLocation() { - setPropLocationIfVisible(Center, null); + setPropLocationIfVisible(Center, Unknown); if (this.propLocation != Center) { peekRight(); } @@ -193,20 +194,6 @@ public abstract class AutoBase extends LinearOpMode { setPropLocationIfVisible(Right, Left); } - protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) { - switch (propLocation) { - case Left: - return 1; - case Unknown: - case Center: - return 2; - case Right: - return 3; - } - - return 2; - } - protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) { float seenCount = 0; float samples = 10; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 38e7a13..1c5c7bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -18,7 +18,8 @@ import org.opencv.core.Point; @Config public class Camera { - public static float PROP_REJECTION_VERTICAL = 300; + public static float PROP_REJECTION_VERTICAL_UPPER = 175; + public static float PROP_REJECTION_VERTICAL_LOWER = 300; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -49,23 +50,13 @@ public class Camera { return INVALID_DETECTION; } - Detection detection = null; switch (getAlliance()) { case Blue: Detection blue = this.prop.getBlue(); - detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION; - break; + return blue != null && blue.isValid() ? blue : INVALID_DETECTION; case Red: Detection red = this.prop.getRed(); - detection = red != null && red.isValid() ? red : INVALID_DETECTION; - break; - } - - if (detection != null && detection.isValid()) { - Point center = detection.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { - return detection; - } + return red != null && red.isValid() ? red : INVALID_DETECTION; } return INVALID_DETECTION; 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 65d323b..36502d6 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 @@ -1,17 +1,16 @@ package org.firstinspires.ftc.teamcode.vision; -import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_LOWER; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MIN; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_CENTER_X; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_LEFT_X; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGHT_X; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1; import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2; import static org.firstinspires.ftc.teamcode.util.Colors.RED; import static org.firstinspires.ftc.teamcode.util.Colors.WHITE; import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR; +import static org.firstinspires.ftc.teamcode.util.Constants.BLACK; import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE; import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS; import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT; @@ -91,6 +90,9 @@ public class PropDetectionPipeline implements VisionProcessor { Core.add(mask, tmpMask, mask); } + Imgproc.rectangle(mask, new Point(0,0), new Point(mask.cols() - 1, (int)PROP_REJECTION_VERTICAL_UPPER), BLACK, -1); + Imgproc.rectangle(mask, new Point(0,(int)PROP_REJECTION_VERTICAL_LOWER), new Point(mask.cols() - 1, mask.rows() -1), BLACK, -1); + Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); @@ -102,11 +104,13 @@ public class PropDetectionPipeline implements VisionProcessor { @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - canvas.drawLine(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE); if (red != null && red.isValid()) { Point center = red.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER + && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED); @@ -115,7 +119,8 @@ public class PropDetectionPipeline implements VisionProcessor { if (blue != null && blue.isValid()) { Point center = blue.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER + && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED);