diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 7ad6d20..49e0671 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; @@ -22,19 +23,22 @@ import org.firstinspires.ftc.teamcode.vision.Detection; @Config public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; - public static int SCORING_DURATION_MS = 5000; + public static double SCORING_DURATION_S = 5f; + public static double APRIL_TAG_RIGHT_DELTA = -8.5; + public static double APRIL_TAG_LEFT_DELTA = 7.0; + protected Robot robot; protected FtcDashboard dashboard; protected Telemetry dashboardTelemetry; protected CenterStageCommon.PropLocation propLocation; protected final Pose2d initialPosition; protected final CenterStageCommon.Alliance alliance; - protected final Vector2d rendezvous; + protected final Pose2d park; - protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) { + protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) { this.alliance = alliance; this.initialPosition = initialPosition; - this.rendezvous = rendezvous; + this.park = park; } @Override @@ -50,75 +54,103 @@ public abstract class AutoBase extends LinearOpMode { this.sleep(20); } + if (isStopRequested()) { + return; + } + // If the prop is visible at this point, then it must be in the center (2) position determinePropLocation(); + TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: - // TODO Tommy: Place the pixel on the left tape and move to rendezvous position + propLeft(); break; case Unknown: case Center: - dislodgePropAndPlacePixel(); + propCenter(); break; case Right: - // TODO Tommy: Place the pixel on the right tape and move to rendezvous position + propRight(); break; } moveToBackstage(); prepareToScore(); scorePreloadedPixel(); - - // TODO Tommy: Park + park(); } - private void scorePreloadedPixel() { + protected abstract void propLeft(); + + protected abstract void propCenter(); + + protected abstract void propRight(); + + protected void openAndLiftClaw() { + this.robot.getClaw().openSync(); + this.sleep(100); + this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); + } + + protected void scorePreloadedPixel() { this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); while(this.robot.getGantry().isIn()) { - this.robot.getGantry().update(); + this.robot.update(); sleep(20); } this.robot.getGantry().deposit(); - this.sleep(SCORING_DURATION_MS); + double startTime = this.getRuntime(); + while (this.getRuntime() < (startTime + SCORING_DURATION_S)) { + this.robot.update(); + } this.robot.getGantry().stop(); + this.robot.getGantry().setSlideTarget(0); + this.robot.getGantry().armInSync(); + } - private void prepareToScore() { + protected void prepareToScore() { + // At this point we know that Y = 38 + // For 2 -> Ydelta = 0 + // For 3 -> 3 5/8 + // For 1 -> - 3 5/8 + double delta = 0; + switch (this.propLocation) { + case Left: + delta = APRIL_TAG_LEFT_DELTA; + break; + case Unknown: + case Center: + delta = 0; + break; + case Right: + delta = APRIL_TAG_RIGHT_DELTA; + break; + } double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5); + Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + (distance - SCORING_DISTANCE_FROM_APRIL_TAG), this.robot.getDrive().getPoseEstimate().getY() + delta); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG); + builder.lineToConstantHeading(target); this.robot.getDrive().followTrajectorySequence(builder.build()); } - private void moveToBackstage() { + protected void moveToBackstage() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(36, 38, 0)); this.robot.getDrive().followTrajectorySequence(builder.build()); } - private void dislodgePropAndPlacePixel() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous); - builder.addDisplacementMarker(10, () -> { - 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 determinePropLocation() { - setPropLocationIfVisible(Center, null); + protected void determinePropLocation() { + setPropLocationIfVisible(Center, Unknown); if (this.propLocation != Center) { peekRight(); } } - private void peekRight() { + protected void peekRight() { TrajectorySequenceBuilder builder = this.robot.getDrive() .trajectorySequenceBuilder(initialPosition); builder.forward(5); @@ -127,20 +159,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; @@ -156,4 +174,12 @@ public abstract class AutoBase extends LinearOpMode { this.propLocation = ifNotVisible; } } + + public void park() { + double currentX = this.robot.getDrive().getPoseEstimate().getX(); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.strafeTo(new Vector2d(currentX, park.getY())); + builder.lineToConstantHeading(park.vec()); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } } diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java new file mode 100644 index 0000000..4c312ae --- /dev/null +++ b/TeamCode/src/main/java/opmodes/BlueBackStage.java @@ -0,0 +1,60 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp") +public class BlueBackStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(12, 11); + + public BlueBackStage() { + super( + CenterStageCommon.Alliance.Blue, + new Pose2d(12, 63, Math.toRadians(-90)), + new Pose2d(62, 62)); + } + + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } + + protected void propCenter() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(rendezvous.vec()); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0))); + builder.lineToConstantHeading(new Vector2d(19, 34)); + builder.addTemporalMarker(0.5, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } +} diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java new file mode 100644 index 0000000..02c6f86 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -0,0 +1,66 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp") +public class BlueFrontStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(-36, 11); + + public BlueFrontStage() { + super( + CenterStageCommon.Alliance.Blue, + new Pose2d(-36, 63, Math.toRadians(-90)), + new Pose2d(62, 12)); + } + + protected void propLeft() { + 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()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void propCenter() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(rendezvous.vec()); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } +} diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java deleted file mode 100644 index 3e235d5..0000000 --- a/TeamCode/src/main/java/opmodes/LeftAuto.java +++ /dev/null @@ -1,22 +0,0 @@ -package opmodes; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.util.CenterStageCommon; - -@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp") -public class LeftAuto extends AutoBase { - public LeftAuto() { - super( - CenterStageCommon.Alliance.Red, - new Pose2d(-36, 63, Math.toRadians(-90)), - new Vector2d(-36, 11)); - } - - @Override - public void runOpMode() { - super.runOpMode(); - } -} diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java new file mode 100644 index 0000000..d3c4371 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -0,0 +1,64 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Autonomous(name = "RedFrontStage", preselectTeleOp = "MainTeleOp") +public class RedFrontStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(-36, -11); + + public RedFrontStage() { + super( + CenterStageCommon.Alliance.Red, + new Pose2d(-36, -63, Math.toRadians(90)), + new Pose2d(-12, 62)); + } + + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, -17, Math.toRadians(123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propCenter() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(rendezvous.vec()); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180))); + builder.lineToConstantHeading(new Vector2d(-42, -31)); + builder.addTemporalMarker(0.5, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } +} 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..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 @@ -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,12 @@ 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_UPPER = 175; + public static float PROP_REJECTION_VERTICAL_LOWER = 300; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -60,27 +62,6 @@ public class Camera { 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/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java index 3ad07c4..ec695a2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -82,10 +82,24 @@ public class Gantry { this.armControllerTarget = GANTRY_ARM_MAX; } + public void armOutSync() { + this.armOut(); + while (this.armServo.getPosition() < this.armControllerTarget - 0.001) { + this.update(); + } + } + public void armIn() { this.armControllerTarget = GANTRY_ARM_MIN; } + public void armInSync() { + this.armIn(); + while (this.armServo.getPosition() > this.armControllerTarget + 0.001) { + this.update(); + } + } + public void intake() { this.screwServo.setPower(-1); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index a70f187..c7ae108 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -54,6 +54,7 @@ public class Robot { } public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { + this.drive.update(); return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate()); } 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 bcef2b6..9efe412 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 @@ -41,7 +41,7 @@ public class RobotConfig { public static double GANTRY_ARM_MIN = 0.435; public static double GANTRY_ARM_MAX = 0.94; public static int GANTRY_LIFT_DELTA = 50; - public static double GANTRY_ARM_KP = 0.06; + public static double GANTRY_ARM_KP = 0.1; public static double X_KP = 0.1; public static double X_MAX = 0.84; public static double X_MIN = 0.16; @@ -66,5 +66,5 @@ public class RobotConfig { public static double DETECTION_LEFT_X = 0; public static double DETECTION_CENTER_X = .5; public static double DETECTION_RIGHT_X = 1; - public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6.5; + public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index dcf1c98..bf936f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -67,8 +67,8 @@ public class DriveConstants { */ public static double MAX_VEL = 45; public static double MAX_ACCEL = 45; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + public static double MAX_ANG_VEL = Math.toRadians(720); + public static double MAX_ANG_ACCEL = Math.toRadians(720); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. 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..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,15 +1,16 @@ package org.firstinspires.ftc.teamcode.vision; +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; @@ -24,6 +25,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; @@ -88,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); @@ -99,20 +104,27 @@ 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_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, 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_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); + } } - 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_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); + } } } } \ No newline at end of file