From 420fa25c31c18e2e9526ad82b2777c1bce1d3727 Mon Sep 17 00:00:00 2001 From: sihan Date: Wed, 8 Nov 2023 20:40:33 -0600 Subject: [PATCH] Red and Blue auto works, Teleop works, everything works. --- .../ftc/teamcode/opmodes/Auto.java | 6 +- .../ftc/teamcode/opmodes/AutoBlue.java | 154 ++++++++++++++++++ .../ftc/teamcode/vision/Camera.java | 38 +++++ .../vision/ColorDetectionPipeline.java | 40 +++++ 4 files changed, 235 insertions(+), 3 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java index 6e8e6e4..8c4d89f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java @@ -58,7 +58,7 @@ public class Auto extends LinearOpMode { .build(); this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(72, -26, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(73, -26, Math.toRadians(360))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); @@ -74,7 +74,7 @@ public class Auto extends LinearOpMode { .build(); this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(72, -34, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(73, -34, Math.toRadians(360))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); @@ -89,7 +89,7 @@ public class Auto extends LinearOpMode { .build(); this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) - .lineToLinearHeading(new Pose2d(72, -42, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(73, -42, Math.toRadians(360))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java new file mode 100644 index 0000000..08eeeb6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java @@ -0,0 +1,154 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +@Autonomous(name = "autoBlue") +public class AutoBlue extends LinearOpMode { + protected Pose2d initialPosition; + + protected Trajectory preloadOne; + protected Trajectory scoreOne; + protected Trajectory boardOne; + protected Trajectory backOffOne; + + + protected Trajectory preloadTwo; + protected Trajectory scoreTwo; + protected Trajectory backOffTwo; + + + protected Trajectory preloadThree; + protected Trajectory scoreThree; + protected Trajectory backOffThree; + + + protected Trajectory park1; + protected Trajectory park2; + + + private Robot robot; + private String randomization; + private int random; + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + + //Trajectories + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + //Randomization One + this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270))) + .build(); + + this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end()) + .lineToLinearHeading(new Pose2d(-29, -30, Math.toRadians(180))) + .build(); + + this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) + .lineToLinearHeading(new Pose2d(-72, -26, Math.toRadians(180))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) + .lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180))) + .build(); + + + //Randomization Two + this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-35, -27, Math.toRadians(270))) + .build(); + + this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) + .lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) + .lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180))) + .build(); + + //Randomization Three + this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270))) + .build(); + + this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) + .lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) + .lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180))) + .build(); + + //Park + this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) + .lineToLinearHeading(new Pose2d(-65, -10, Math.toRadians(180))) + .addTemporalMarker(.3, robot.getArm()::armRest) + .addTemporalMarker(.3, robot.getWrist()::wristPickup) + .build(); + this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) + .lineToLinearHeading(new Pose2d(-80, -10, Math.toRadians(180))) + .build(); + + // Do super fancy chinese shit + while (!this.isStarted()) { + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); + randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + this.telemetry.update(); + } + + switch (randomization) { + case "RIGHT": + this.robot.getDrive().followTrajectory(preloadOne); + this.robot.getDrive().followTrajectory(scoreOne); + this.robot.getDrive().followTrajectory(boardOne); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffOne); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); + break; + case "CENTER": + this.robot.getDrive().followTrajectory(preloadTwo); + this.robot.getDrive().followTrajectory(scoreTwo); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffTwo); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); + break; + case "LEFT": + this.robot.getDrive().followTrajectory(preloadThree); + this.robot.getDrive().followTrajectory(scoreThree); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffThree); + sleep(500); + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); + break; + } + + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java index 4e87299..ca10f10 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java @@ -79,11 +79,49 @@ public class Camera { return StarterPosition.RIGHT; } + + return StarterPosition.UNKNOWN; } + public Detection getBlue() { + return targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION; + } + + public StarterPositionBlue getStartingPositionBlue() { + if (!targetingCameraInitialized) { + return StarterPositionBlue.UNKNOWN; + } + + Detection detection = this.getBlue(); + if (detection == null || !detection.isValid()) { + return StarterPositionBlue.UNKNOWN; + } + Point center = detection.getCenter(); + if (center == null) { + return StarterPositionBlue.UNKNOWN; + } + + double centerX = this.getBlue().getCenter().x + 50; + if (centerX < 33) { + return StarterPositionBlue.LEFT; + } else if (centerX < 66) { + return StarterPositionBlue.CENTER; + } else if (centerX < 100) { + return StarterPositionBlue.RIGHT; + } + + + + return StarterPositionBlue.UNKNOWN; + } + public enum StarterPosition { UNKNOWN, LEFT, CENTER, RIGHT } + public enum StarterPositionBlue { + UNKNOWN, LEFT, CENTER, RIGHT + } + } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java index 080c9cd..b6ae7cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java @@ -1,8 +1,11 @@ package org.firstinspires.ftc.teamcode.vision; +import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_LOWER; +import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_BLUE_UPPER; import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_LOWER; import static org.firstinspires.ftc.teamcode.util.Configurables.FTC_RED_UPPER; import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR; +import static org.firstinspires.ftc.teamcode.util.Constants.BLUE; 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.RED; @@ -30,14 +33,26 @@ public class ColorDetectionPipeline extends OpenCvPipeline { Scalar redGoalLower2; Scalar redGoalUpper2; + Mat blueMask1 = new Mat(); + Mat blueMask2 = new Mat(); + Mat blueMask = new Mat(); + Scalar blueGoalLower1; + Scalar blueGoalUpper1; + Scalar blueGoalLower2; + Scalar blueGoalUpper2; + private Detection red; + private Detection blue; + // Init @Override public void init(Mat input) { red = new Detection(input.size(), 0); + blue = new Detection(input.size(), 0); } + // Process each frame that is received from the webcam @Override public Mat processFrame(Mat input) { @@ -45,6 +60,7 @@ public class ColorDetectionPipeline extends OpenCvPipeline { Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); updateRed(input); + updateBlue(input); return input; } @@ -74,7 +90,31 @@ public class ColorDetectionPipeline extends OpenCvPipeline { red.fill(input, RED); } + private void updateBlue(Mat input) { + // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise + blueGoalLower1 = new Scalar(FTC_BLUE_LOWER.getH(), FTC_BLUE_LOWER.getS(), FTC_BLUE_LOWER.getV()); + blueGoalUpper1 = new Scalar(FTC_BLUE_UPPER.getH(), FTC_BLUE_UPPER.getS(), FTC_BLUE_UPPER.getV()); + Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask); + Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + + ArrayList contours = new ArrayList<>(); + Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE); + for (int i = 0; i < contours.size(); i++) { + Detection newDetection = new Detection(input.size(), 0, 1f); + newDetection.setContour(contours.get(i)); + newDetection.draw(input, BLUE); + } + + blue.setContour(getLargestContour(contours)); + blue.fill(input, BLUE); + } + public Detection getRed() { return this.red; } + + public Detection getBlue() { + return this.blue; + } } \ No newline at end of file