Fixed prop detector

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();
+        }
+    }
+}

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();
+    }
+}

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;
+    }
 }

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_MIN = 0.02f;
-    public static double DETECTION_AREA_MAX = 1f;
+    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;

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_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, 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);
 

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

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.release();
-            mask.setTo(this.zeros(mask.size(), mask.type()));
-        }
         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);
+        }
     }
 }