Working auto

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;

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;
+                return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
-                break;
             case Red:
                 Detection red = this.prop.getRed();
-                detection =  red != null && red.isValid() ? red : INVALID_DETECTION;
+                return 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 INVALID_DETECTION;

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