diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 676102c..ffebca3 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -24,7 +24,7 @@ public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; public static double SCORING_DURATION_S = 5f; public static double APRIL_TAG_RIGHT_DELTA = -8.5; - public static double APRIL_TAG_LEFT_DELTA = 4.0; + public static double APRIL_TAG_LEFT_DELTA = 8.0; protected Robot robot; protected FtcDashboard dashboard; @@ -59,8 +59,12 @@ public abstract class AutoBase extends LinearOpMode { TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: - // TODO Tommy: Place the pixel on the left tape and move to rendezvous position - return; + dislodgePropAndPlacePixelLeft(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + break; case Unknown: case Center: dislodgePropAndPlacePixelCenter(); @@ -85,6 +89,19 @@ public abstract class AutoBase extends LinearOpMode { // TODO Tommy: Park } + private void dislodgePropAndPlacePixelLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); + builder.lineToConstantHeading(new Vector2d(-42, 31)); + builder.addTemporalMarker(0.2, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + this.robot.getClaw().openSync(); + this.sleep(100); + this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); + } + private void dislodgePropAndPlacePixelRight() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); 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 337796c..38e7a13 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 @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.hardware; -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.hardware.RobotConfig.WEBCAM_NAME; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -16,8 +14,11 @@ import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; +import org.opencv.core.Point; +@Config public class Camera { + public static float PROP_REJECTION_VERTICAL = 300; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -48,39 +49,28 @@ public class Camera { return INVALID_DETECTION; } + Detection detection = null; switch (getAlliance()) { case Blue: Detection blue = this.prop.getBlue(); - return blue != null && blue.isValid() ? blue : INVALID_DETECTION; + detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION; + break; case Red: Detection red = this.prop.getRed(); - return red != null && red.isValid() ? red : INVALID_DETECTION; + 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 INVALID_DETECTION; } - public CenterStageCommon.PropLocation getPropLocation() { - Detection prop = this.getProp(); - if (prop == null || !prop.isValid()) { - return CenterStageCommon.PropLocation.Unknown; - } - - double x = prop.getCenter().x + 50; - - if (x <= DETECTION_LEFT_X) { - return CenterStageCommon.PropLocation.Left; - } - if (x <= DETECTION_CENTER_X) { - return CenterStageCommon.PropLocation.Center; - } - if (x <= DETECTION_RIGHT_X) { - return CenterStageCommon.PropLocation.Right; - } - - return CenterStageCommon.PropLocation.Unknown; - } - public AprilTagDetection getAprilTag(int id) { return this.aprilTag.getDetections() .stream() 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 de3f226..65d323b 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,5 +1,6 @@ package org.firstinspires.ftc.teamcode.vision; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL; 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; @@ -8,6 +9,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; @@ -24,6 +26,7 @@ import org.firstinspires.ftc.vision.VisionProcessor; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; @@ -99,20 +102,24 @@ public class PropDetectionPipeline implements VisionProcessor { @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - int detectionLeftXPx = (int)((DETECTION_LEFT_X / 100) * onscreenWidth); - int detectionCenterXPx = (int)((DETECTION_CENTER_X / 100) * onscreenWidth); - int detectionRightXPx = (int)((DETECTION_RIGHT_X / 100) * onscreenWidth); + canvas.drawLine(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE); - 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 (red != null && red.isValid()) { + Point center = red.getCenterPx(); + if (center.y < PROP_REJECTION_VERTICAL) { + 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); + } } - if (blue.isValid()) { - canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); + if (blue != null && blue.isValid()) { + Point center = blue.getCenterPx(); + if (center.y < PROP_REJECTION_VERTICAL) { + 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); + } } } } \ No newline at end of file