diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java new file mode 100644 index 0000000..81775e6 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -0,0 +1,33 @@ +package opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; +import org.firstinspires.ftc.teamcode.vision.Detection; + +public abstract class AutoBase extends LinearOpMode { + protected Robot robot; + protected FtcDashboard dashboard; + protected Telemetry dashboardTelemetry; + protected CenterStageCommon.PropLocation propLocation; + + @Override + public void runOpMode() { + this.robot = new Robot(hardwareMap, telemetry); + this.dashboard = FtcDashboard.getInstance(); + this.dashboardTelemetry = dashboard.getTelemetry(); + + this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Red); + + while(!isStarted() && !isStopRequested()) { + this.propLocation = this.robot.getCamera().getPropLocation(); + Detection detection = this.robot.getCamera().getProp(); + this.dashboardTelemetry.addData("Prop", detection.getCenterPx()); + this.dashboardTelemetry.addData("Prop Location", this.propLocation); + this.dashboardTelemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java new file mode 100644 index 0000000..de2d102 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/LeftAuto.java @@ -0,0 +1,11 @@ +package opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp") +public class LeftAuto extends AutoBase { + @Override + public void runOpMode() { + super.runOpMode(); + } +} 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 f5a05a2..3339346 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 @@ -21,9 +21,6 @@ import lombok.Getter; import lombok.Setter; public class Camera { - @Getter - @Setter - private CenterStageCommon.Alliance alliance; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -50,16 +47,17 @@ public class Camera { } public Detection getProp() { - if (!initialized || alliance == null) { + if (!initialized || getAlliance() == null) { return INVALID_DETECTION; } - switch (alliance) { - + switch (getAlliance()) { case Blue: - return this.prop.getBlue(); + Detection blue = this.prop.getBlue(); + return blue != null && blue.isValid() ? blue : INVALID_DETECTION; case Red: - return this.prop.getRed(); + Detection red = this.prop.getRed(); + return red != null && red.isValid() ? red : INVALID_DETECTION; } return INVALID_DETECTION; @@ -67,7 +65,7 @@ public class Camera { public CenterStageCommon.PropLocation getPropLocation() { Detection prop = this.getProp(); - if (!prop.isValid()) { + if (prop == null || !prop.isValid()) { return CenterStageCommon.PropLocation.Unknown; } @@ -93,4 +91,12 @@ public class Camera { .findFirst() .orElse(null); } + + public void setAlliance(CenterStageCommon.Alliance alliance) { + this.prop.setAlliance(alliance); + } + + public CenterStageCommon.Alliance getAlliance() { + return this.prop != null ? this.prop.getAlliance() : null; + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index e224a3a..cb1d8bb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java @@ -60,8 +60,8 @@ public class RobotConfig { // Vision public static double CAMERA_OFFSET_X = 0f; - public static double DETECTION_AREA_MIN = 0f; - public static double DETECTION_AREA_MAX = 1f; + public static double DETECTION_AREA_MIN = 0.02f; + public static double DETECTION_AREA_MAX = 0.3f; public static double DETECTION_LEFT_X = 0; public static double DETECTION_CENTER_X = .5; public static double DETECTION_RIGHT_X = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java index a40a8ce..beeadc5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java @@ -6,11 +6,11 @@ public class Colors { // CV Color Threshold Constants public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80); public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255); - public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER); - public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER); + public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER); + public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2])); public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80); public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255); - public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_RED_LOWER); + public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER); public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40); public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java index b7962d7..89fbb7e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java @@ -67,7 +67,7 @@ public class Detection { // Check if the current Detection is valid public boolean isValid() { - return (this.contour != null) && (this.centerPx != INVALID_POINT) && (this.areaPx != INVALID_AREA); + return (this.contour != null) && (this.areaPx != INVALID_AREA); } // Get the current contour 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 cb0628d..35f4a99 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 @@ -8,6 +8,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGH 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.BLUR_SIZE; @@ -79,11 +80,12 @@ public class PropDetectionPipeline implements VisionProcessor { } private MatOfPoint detect(ScalarRange... colorRanges) { - if (!mask.empty()) { - mask.setTo(this.zeros(mask.size(), mask.type())); - } + mask.release(); for (ScalarRange colorRange : colorRanges) { Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask); + if (mask.empty() || mask.rows() <= 0) { + Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask); + } Core.add(mask, tmpMask, mask); } @@ -105,5 +107,13 @@ public class PropDetectionPipeline implements VisionProcessor { canvas.drawLine(detectionLeftXPx, 0, detectionLeftXPx, canvas.getHeight(), WHITE); canvas.drawLine(detectionCenterXPx, 0, detectionCenterXPx, canvas.getHeight(), WHITE); canvas.drawLine(detectionRightXPx, 0, detectionRightXPx, canvas.getHeight(), WHITE); + + if (red.isValid()) { + canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); + } + + if (blue.isValid()) { + canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); + } } } \ No newline at end of file