Skip to content
This repository has been archived by the owner on Sep 27, 2024. It is now read-only.

Team Prop Detection #90

Draft
wants to merge 1 commit into
base: master
Choose a base branch
from
Draft
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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.<ColorSensor>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;
//}
//}
Loading