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 8ab6567..36ee78c 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 @@ -25,18 +25,18 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.vision.Camera; import lombok.Getter; public class Robot { - @Getter - private MecanumDrive drive; -// private Hang hang; + @Getter private MecanumDrive drive; @Getter private Intake intake; @Getter private Arm arm; @Getter private Wrist wrist; @Getter private Claw claw; @Getter private Hang hang; + @Getter private Camera camera; public Robot init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); @@ -45,6 +45,7 @@ public class Robot { this.arm = new Arm().init(hardwareMap); this.wrist = new Wrist().init(hardwareMap); this.claw = new Claw().init(hardwareMap); + this.camera = new Camera(hardwareMap); return this; } @@ -108,12 +109,12 @@ public class Robot { this.rightArm.setPosition(PICKUP); } - public void rest() { + public void armScore() { this.leftArm.setPosition(ARMDOWN); this.rightArm.setPosition(ARMDOWN); } - public void scoreArm() { + public void armRest() { this.leftArm.setPosition(ARMUP); this.rightArm.setPosition(ARMUP); } @@ -156,7 +157,5 @@ public class Robot { public void openScore() { this.claw.setPosition(BIGOPEN); } - } - } \ No newline at end of file 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 b1d495e..b811e28 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 @@ -68,18 +68,18 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 60; - public static double MAX_ACCEL = 60; - public static double MAX_ANG_VEL = Math.toRadians(90); - public static double MAX_ANG_ACCEL = Math.toRadians(90); + public static double MAX_VEL = 40; + public static double MAX_ACCEL = 40; + public static double MAX_ANG_VEL = Math.toRadians(60); + public static double MAX_ANG_ACCEL = Math.toRadians(60); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. */ public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = - RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.LogoFacingDirection.LEFT; public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = - RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot.UsbFacingDirection.UP; public static double encoderTicksToInches(double ticks) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index 944229c..e1847cd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -11,6 +11,8 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; +import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_SPEED; +import static org.firstinspires.ftc.teamcode.util.Configurables.SLOWMODE_TURN; import static org.firstinspires.ftc.teamcode.util.Configurables.SPEED; import static org.firstinspires.ftc.teamcode.util.Configurables.TURN; @@ -140,7 +142,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr List lastTrackingEncVels = new ArrayList<>(); // TODO: if desired, use setLocalizer() to change the localization method - // setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels)); + setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this)); trajectorySequenceRunner = new TrajectorySequenceRunner( follower, HEADING_PID, batteryVoltageSensor, @@ -307,7 +309,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr @Override public Double getExternalHeadingVelocity() { - return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate; + return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).xRotationRate; } public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) { @@ -322,13 +324,14 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr } public void setInput(Gamepad gamepad1, Gamepad gamepad2) { - double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES); + double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED; + double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN; + this.setWeightedDrivePower( new Pose2d( - gamepad1.left_stick_y* -1 * SPEED, - gamepad1.left_stick_x*-1 * SPEED, - -gamepad1.right_stick_x * TURN + gamepad1.left_stick_y* -1 * speedScale, + gamepad1.left_stick_x*-1 * speedScale, + -gamepad1.right_stick_x * turnScale )); } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java index 9702f5e..93fbe22 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -28,8 +28,8 @@ import java.util.List; */ @Config public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { - public static double TICKS_PER_REV = 0; - public static double WHEEL_RADIUS = 2; // in + public static double TICKS_PER_REV = 2000; + public static double WHEEL_RADIUS = 0.944882; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed public static double LATERAL_DISTANCE = 10; // in; distance between the left and right wheels diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java new file mode 100644 index 0000000..db854a5 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive; + +import androidx.annotation.NonNull; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * ^ + * | + * | ( x direction) + * | + * v + * <----( y direction )----> + + * (forward) + * /--------------\ + * | ____ | + * | ---- | <- Perpendicular Wheel + * | || | + * | || | <- Parallel Wheel + * | | + * | | + * \--------------/ + * + */ +public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { + public static double TICKS_PER_REV = 2000; + public static double WHEEL_RADIUS = 0.944882; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double PARALLEL_X = 5.786; // X is the up and down direction + public static double PARALLEL_Y = -4.118; // Y is the strafe direction + + public static double PERPENDICULAR_X = 3.209; + public static double PERPENDICULAR_Y = -.199; + + public static double X_MULTIPLIER = 1; // Multiplier in the X direction + public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction + // Parallel/Perpendicular to the forward axis + // Parallel wheel is parallel to the forward axis + // Perpendicular is perpendicular to the forward axis + private final Encoder parallelEncoder; + private final Encoder perpendicularEncoder; + + private final MecanumDrive drive; + + public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, MecanumDrive drive) { + super(Arrays.asList( + new Pose2d(PARALLEL_X, PARALLEL_Y, 0), + new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90)) + )); + + this.drive = drive; + + parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightRear")); + perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "rightFront")); + + // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE) + parallelEncoder.setDirection(Encoder.Direction.REVERSE); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @Override + public double getHeading() { + return drive.getRawExternalHeading(); + } + + @Override + public Double getHeadingVelocity() { + return drive.getExternalHeadingVelocity(); + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + // TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other + // competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a + // compensation method + + return Arrays.asList( + encoderTicksToInches(parallelEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity()) * Y_MULTIPLIER + ); + } +} \ No newline at end of file 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 new file mode 100644 index 0000000..231f99c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java @@ -0,0 +1,77 @@ +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 = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!") +public class Auto extends LinearOpMode { + private Robot robot; + private String randomization; + private int random; + protected Pose2d initialPosition; + protected Trajectory preloadOne; + protected Trajectory scoreOne; + protected Trajectory preloadTwo; + protected Trajectory preloadThree; + + + @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); + + //Preload 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(33,-30, Math.toRadians(360))) + .build(); + + //Preload Two + this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(40,-25, Math.toRadians(270))) + .build(); + + //Preload Three + this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(47.5,-37.5, Math.toRadians(270))) + .build(); + + // Do super fancy chinese shit + while (!this.isStarted()) { + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.update(); + } + + switch (randomization) { + case "LEFT": + this.robot.getDrive().followTrajectory(preloadOne); + this.robot.getDrive().followTrajectory(scoreOne); + break; + case "CENTER": + this.robot.getDrive().followTrajectory(preloadTwo); + break; + case "RIGHT": + this.robot.getDrive().followTrajectory(preloadThree); + break; + } + + + + } + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java index 69a8a46..caa8e17 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java @@ -4,7 +4,6 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; -import org.firstinspires.ftc.teamcode.util.Configurables; @com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development") public class MainTeleOp extends OpMode { @@ -18,51 +17,38 @@ public class MainTeleOp extends OpMode { @Override public void loop() { + boolean hang = gamepad2.dpad_up; + boolean restArm = gamepad2.dpad_left || gamepad2.x; + boolean pickupArm = gamepad2.dpad_down; + boolean scoreArm = gamepad2.dpad_right || gamepad2.a; + boolean claw = gamepad2.b; + boolean pickupWrist = gamepad2.left_bumper || gamepad2.x; + boolean scoreWrist = gamepad2.right_bumper || gamepad2.a; //Drive robot.getDrive().setInput(gamepad1, gamepad2); //Hang - if (gamepad2.dpad_up) { + if (hang) { this.robot.getHang().release(); - } else { - this.robot.getHang().lock(); } -//Intake -// if (gamepad1.x) { -// this.robot.getIntake().spinIn(); -// } else if (gamepad1.y) { -// this.robot.getIntake().spinOut(); -// } else { -// this.robot.getIntake().stop(); -// } //Arm - if (gamepad2.dpad_down) { + if (pickupArm) { this.robot.getArm().pickup(); - } else if (gamepad2.dpad_right ||gamepad2.x) { - this.robot.getArm().scoreArm(); - } else if (gamepad2.dpad_left || gamepad2.a) { - this.robot.getArm().rest(); + } else if (restArm) { + this.robot.getArm().armRest(); + } else if (scoreArm) { + this.robot.getArm().armScore(); } //Claw - if (gamepad2.b) { + if (claw) { this.robot.getClaw().open(); - } else if (gamepad2.y) { - this.robot.getClaw().openScore(); } else { this.robot.getClaw().close(); } //Wrist - if (gamepad2.left_bumper || gamepad2.x) { + if (pickupWrist) { this.robot.getWrist().wristPickup(); - } else if (gamepad2.right_bumper || gamepad2.a) { + } else if (scoreWrist) { this.robot.getWrist().wristScore(); } -//SLOWMO - if (gamepad1.y) { - Configurables.SPEED = .5; - Configurables.TURN = .75; - } else { - Configurables.SPEED = 1; - Configurables.TURN = 1; - } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index 01a9b2f..ce3ad05 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -29,6 +29,8 @@ public class Configurables { //Drive Speed public static double SPEED = 1; + public static double SLOWMODE_SPEED = 0.5; public static double TURN = 1; + public static double SLOWMODE_TURN = 0.75; } \ No newline at end of file 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 new file mode 100644 index 0000000..4e87299 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Camera.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.teamcode.vision; + +import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; +import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM; +import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT; +import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION; +import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.opencv.core.Point; +import org.openftc.easyopencv.OpenCvCamera; +import org.openftc.easyopencv.OpenCvCameraFactory; + +public class Camera { + private final HardwareMap hardwareMap; + private OpenCvCamera targetingCamera; + private ColorDetectionPipeline targetingPipeline; + private boolean targetingCameraInitialized; + + // Constructor + public Camera(HardwareMap hardwareMap) { + this.hardwareMap = hardwareMap; + } + + // Initiate the Targeting Camera + public void initTargetingCamera() { + int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); + this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM), targetingCameraMonitorViewId); + this.targetingPipeline = new ColorDetectionPipeline(); + targetingCamera.setPipeline(targetingPipeline); + targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { + @Override + public void onOpened() { + targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION); + targetingCameraInitialized = true; + } + + @Override + public void onError(int errorCode) { + + } + }); + } + + // Close the Targeting Camera + public void stopTargetingCamera() { + if (targetingCameraInitialized) { + targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false); + } + } + + // Get the Red Goal Detection + public Detection getRed() { + return targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION; + } + + public StarterPosition getStartingPosition() { + if (!targetingCameraInitialized) { + return StarterPosition.UNKNOWN; + } + + Detection detection = this.getRed(); + if (detection == null || !detection.isValid()) { + return StarterPosition.UNKNOWN; + } + Point center = detection.getCenter(); + if (center == null) { + return StarterPosition.UNKNOWN; + } + + double centerX = this.getRed().getCenter().x + 50; + if (centerX < 33) { + return StarterPosition.LEFT; + } else if (centerX < 66) { + return StarterPosition.CENTER; + } else if (centerX < 100) { + return StarterPosition.RIGHT; + } + + return StarterPosition.UNKNOWN; + } + + public enum StarterPosition { + 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 new file mode 100644 index 0000000..080c9cd --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java @@ -0,0 +1,80 @@ +package org.firstinspires.ftc.teamcode.vision; + +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.BLUR_SIZE; +import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS; +import static org.firstinspires.ftc.teamcode.util.Constants.RED; +import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT; +import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour; + +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvPipeline; + +import java.util.ArrayList; + +public class ColorDetectionPipeline extends OpenCvPipeline { + Mat blurred = new Mat(); + Mat hsv = new Mat(); + Mat redMask1 = new Mat(); + Mat redMask2 = new Mat(); + Mat redMask = new Mat(); + Mat whiteMask = new Mat(); + Scalar redGoalLower1; + Scalar redGoalUpper1; + Scalar redGoalLower2; + Scalar redGoalUpper2; + + private Detection red; + + // Init + @Override + public void init(Mat input) { + red = new Detection(input.size(), 0); + } + + // Process each frame that is received from the webcam + @Override + public Mat processFrame(Mat input) { + Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); + Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); + + updateRed(input); + + return input; + } + + // Update the Red Goal Detection + private void updateRed(Mat input) { + // take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise + redGoalLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV()); + redGoalUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV()); + redGoalLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV()); + redGoalUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV()); + Core.inRange(hsv, redGoalLower1, redGoalUpper1, redMask1); + Core.inRange(hsv, redGoalLower2, redGoalUpper2, redMask2); + Core.add(redMask1, redMask2, redMask); + Imgproc.erode(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + Imgproc.dilate(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); + + ArrayList contours = new ArrayList<>(); + Imgproc.findContours(redMask, 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, RED); + } + + red.setContour(getLargestContour(contours)); + red.fill(input, RED); + } + + public Detection getRed() { + return this.red; + } +} \ No newline at end of file