diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java index e5ec6265..560f6954 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/colorSensor/PropDetection.java @@ -2,38 +2,143 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.ColorSensor; -import org.firstinspires.ftc.teamcode.subsystems.hardware.Hardware; -import org.firstinspires.ftc.teamcode.subsystems.hardware.HardwareElement; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.opencv.core.Core; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; +import org.openftc.easyopencv.OpenCvPipeline; + +@Autonomous(name = "Team Prop Detection") -@Autonomous(name = "Prop Dectection") public class PropDetection extends OpMode { + static int X = 145; //145 + static int Y = 168; + static int W = 30;//30 + static int H = 50;//50 - private Hardware robot; - int red; - int blue; - int colorTolerence = 30; - boolean teamPropDetected = false; + public PropDetection(int boundX, int boundY, int width, int height) { + X = boundX; + Y = boundY; + W = width; + H = height; + } @Override public void init() { - robot = new Hardware(hardwareMap, telemetry); - robot.introduce(new HardwareElement<>(ColorSensor.class, hardwareMap, "colorSensor")); - - telemetry.addData("Status", "Init"); + telemetry.addData("Camera", "initializing"); } @Override public void loop() { - red = robot.get("colorSensor").red(); - if (red - colorTolerence < red && red < red + colorTolerence) { - teamPropDetected = true; - } + } + + public enum ParkingPosition { + LEFT, + CENTER, + RIGHT + } + + + // TOPLEFT anchor point for the bounding box + private static Point PROP_TOPLEFT_ANCHOR_POINT = new Point(X, Y); + + // Width and height for the bounding box + public static int REGION_WIDTH = W; + public static int REGION_HEIGHT = H; + + // Lower and upper boundaries for colors + private static final Scalar + lower_blue_bounds = new Scalar(200, 200, 200, 255), + upper_blue_bounds = new Scalar(255, 255, 255, 255), + lower_red_bounds = new Scalar(0, 0, 0, 0), + upper_red_bounds = new Scalar(100, 100, 100, 255); + + // Color definitions + private final Scalar + RED = new Scalar(255, 0, 0), + BLUE = new Scalar(0, 0, 255); + + // Percent and mat definitions + private double redPercent, bluePercent; + private Mat redMat = new Mat(), blueMat = new Mat(), blurredMat = new Mat(); - if (blue - colorTolerence < blue && blue < blue + colorTolerence) { - teamPropDetected = true; + // Anchor point definitions + Point prop_pointA = new Point( + PROP_TOPLEFT_ANCHOR_POINT.x, + PROP_TOPLEFT_ANCHOR_POINT.y); + Point prop_pointB = new Point( + PROP_TOPLEFT_ANCHOR_POINT.x + REGION_WIDTH, + PROP_TOPLEFT_ANCHOR_POINT.y + REGION_HEIGHT); + + // Running variable storing the parking position + int position = 0; + + //@Override + public Mat processFrame(Mat input) { + // Noise reduction + Imgproc.blur(input, blurredMat, new Size(5, 5)); + blurredMat = blurredMat.submat(new Rect(prop_pointA, prop_pointB)); + + // Apply Morphology + Mat kernel = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(3, 3)); + Imgproc.morphologyEx(blurredMat, blurredMat, Imgproc.MORPH_CLOSE, kernel); + + // Gets channels from given source mat + Core.inRange(blurredMat, lower_red_bounds, upper_red_bounds, redMat); + Core.inRange(blurredMat, lower_blue_bounds, upper_blue_bounds, blueMat); + + // Gets color specific values + redPercent = Core.countNonZero(redMat); + bluePercent = Core.countNonZero(blueMat); + + + // Calculates the highest amount of pixels being covered on each side + double maxPercent = Math.max(redPercent, bluePercent); + + // Checks all percentages, will highlight bounding box in camera preview + // based on what color is being detected + + if (maxPercent == redPercent) { + position = 2; + Imgproc.rectangle( + input, + prop_pointA, + prop_pointB, + RED, + 2 + ); + telemetry.addData("Color", "red"); + } else if (maxPercent == bluePercent) { + position = 1; + Imgproc.rectangle( + input, + prop_pointA, + prop_pointB, + BLUE, + 2 + ); + + telemetry.addData("Color", "blue"); } + return input; } + // Memory cleanup + /*blurredMat.release(); + redMat.release(); + blueMat.release();*/ + + //return input; } + + +// Returns an enum being the current position where the robot will park +//public int getPosition() { +// return position; +//} +//}