eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

WORKING TELEOP AND SUBSYSTEMS. NO AUTO NO ODOMETRY

This commit is contained in:
sihan 2023-11-06 10:49:35 -06:00
parent 6f2ed777c2
commit e74e731085
11 changed files with 195 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -1,55 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
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.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.TargetingPipeline;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
public class Camera {
private final HardwareMap hardwareMap;
private OpenCvCamera targetingCamera;
private TargetingPipeline targetingPipeline;
private boolean targetingCameraInitialized;
public Camera(HardwareMap hardwareMap) {
this.hardwareMap = hardwareMap;
}
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 TargetingPipeline();
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) {
}
});
}
public void stopTargetingCamera() {
if (targetingCameraInitialized) {
targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false);
}
}
public Detection getRed() {
return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION);
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java
View File

@ -6,7 +6,6 @@ import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.WHEEL_FRONT_RIGHT;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class Drive {
@ -54,12 +53,4 @@ public class Drive {
backLeft.setPower(blPower);
backRight.setPower(brPower);
}
public void setInput(Gamepad gamepad1, Gamepad gamepad2) {
double x = gamepad1.left_stick_x;
double y = -gamepad1.left_stick_y;
double z = gamepad1.right_stick_x;
setInput(x, y, z);
}
}

119
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -1,11 +1,23 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMDOWN;
import static org.firstinspires.ftc.teamcode.util.Configurables.ARMUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.BIGOPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.CLOSE;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.LOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.OPEN;
import static org.firstinspires.ftc.teamcode.util.Configurables.PICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCK;
import static org.firstinspires.ftc.teamcode.util.Configurables.UNLOCKSPEED;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFT;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHT;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTPICKUP;
import static org.firstinspires.ftc.teamcode.util.Configurables.WRISTSCORE;
import static org.firstinspires.ftc.teamcode.util.Constants.CLAW;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.LEFTHANG;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTARM;
import static org.firstinspires.ftc.teamcode.util.Constants.RIGHTHANG;
import static org.firstinspires.ftc.teamcode.util.Constants.WRIST;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@ -14,25 +26,29 @@ import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
import lombok.Getter;
public class Robot {
@Getter
private MecanumDrive drive;
private Hang hang;
private Intake intake;
// private Hang hang;
@Getter private Intake intake;
@Getter private Arm arm;
@Getter private Wrist wrist;
@Getter private Claw claw;
@Getter private Hang hang;
public Robot init(HardwareMap hardwareMap) {
this.drive = new MecanumDrive(hardwareMap);
this.hang = new Hang().init(hardwareMap);
this.intake = new Intake().init(hardwareMap);
this.arm = new Arm().init(hardwareMap);
this.wrist = new Wrist().init(hardwareMap);
this.claw = new Claw().init(hardwareMap);
return this;
}
public Intake getIntake() {return this.intake;}
public Hang getHang() {return this.hang;}
public MecanumDrive getDrive() {return this.drive;}
public static class Intake {
public static class Intake{
private DcMotor intake = null;
public Intake init(HardwareMap hardwareMap) {
@ -45,6 +61,9 @@ public class Robot {
public void spinIn() {this.intake.setPower(0.7);}
public void spinOut() {this.intake.setPower(-0.7);}
public void stop() {this.intake.setPower(0);}
}
public static class Hang {
@ -52,8 +71,10 @@ public class Robot {
private Servo hangRight;
public Hang init(HardwareMap hardwareMap) {
this.hangLeft = hardwareMap.get(Servo.class, LEFT);
this.hangRight = hardwareMap.get(Servo.class, RIGHT);
this.hangLeft = hardwareMap.get(Servo.class, LEFTHANG);
this.hangRight = hardwareMap.get(Servo.class, RIGHTHANG);
this.hangLeft.setPosition(LOCKSPEED);
this.hangRight.setPosition(LOCK);
return this;
}
@ -68,4 +89,74 @@ public class Robot {
}
}
public static class Arm {
private Servo leftArm;
private Servo rightArm;
public Arm init(HardwareMap hardwareMap){
this.leftArm = hardwareMap.get(Servo.class, LEFTARM);
this.rightArm = hardwareMap.get(Servo.class, RIGHTARM);
this.rightArm.setDirection(Servo.Direction.REVERSE);
this.rightArm.setPosition(ARMUP);
this.leftArm.setPosition(ARMUP);
return this;
}
public void pickup() {
this.leftArm.setPosition(PICKUP);
this.rightArm.setPosition(PICKUP);
}
public void rest() {
this.leftArm.setPosition(ARMDOWN);
this.rightArm.setPosition(ARMDOWN);
}
public void scoreArm() {
this.leftArm.setPosition(ARMUP);
this.rightArm.setPosition(ARMUP);
}
}
public static class Wrist {
private Servo wrist;
public Wrist init(HardwareMap hardwareMap){
this.wrist = hardwareMap.get(Servo.class, WRIST);
this.wrist.setPosition(WRISTPICKUP);
return this;
}
public void wristPickup() {
this.wrist.setPosition(WRISTPICKUP);
}
public void wristScore() {
this.wrist.setPosition(WRISTSCORE);
}
}
public static class Claw {
private Servo claw;
public Claw init (HardwareMap hardwareMap) {
this.claw = hardwareMap.get(Servo.class, CLAW);
return this;
}
public void close() {
this.claw.setPosition(CLOSE);
}
public void open() {
this.claw.setPosition(OPEN);
}
public void openScore() {
this.claw.setPosition(BIGOPEN);
}
}
}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java
View File

@ -21,8 +21,8 @@ public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 1;
public static final double MAX_RPM = 1;
public static final double TICKS_PER_REV = 384.5;
public static final double MAX_RPM = 435;
/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
@ -44,10 +44,13 @@ public class DriveConstants {
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 2; // in
public static double WHEEL_RADIUS = 1.88976; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 1; // in
public static double TRACK_WIDTH = 12.244; // in
// public static double WHEEL_RADIUS = 1.88976; // in
// public static double GEAR_RATIO = 652.5/435; // output (wheel) speed / input (motor) speed
// public static double TRACK_WIDTH = 13; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
@ -65,10 +68,10 @@ public class DriveConstants {
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
public static double MAX_VEL = 30;
public static double MAX_ACCEL = 30;
public static double MAX_ANG_VEL = Math.toRadians(60);
public static double MAX_ANG_ACCEL = Math.toRadians(60);
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);
/*
* Adjust the orientations here to match your robot. See the FTC SDK documentation for details.

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java
View File

@ -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.SPEED;
import static org.firstinspires.ftc.teamcode.util.Configurables.TURN;
import androidx.annotation.NonNull;
@ -56,8 +58,8 @@ import java.util.List;
*/
@Config
public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(25, 0, 2.5);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1);
public static double LATERAL_MULTIPLIER = 1;
@ -105,6 +107,14 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
this.leftFront.setDirection(DcMotor.Direction.REVERSE);
this.rightFront.setDirection(DcMotor.Direction.FORWARD);
this.leftRear.setDirection(DcMotor.Direction.REVERSE);
this.rightRear.setDirection(DcMotor.Direction.FORWARD);
this.leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
this.rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
@ -315,9 +325,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
double heading = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
this.setWeightedDrivePower(
new Pose2d(
gamepad1.left_stick_y* -1,
gamepad1.left_stick_x*-1,
-gamepad1.right_stick_x
gamepad1.left_stick_y* -1 * SPEED,
gamepad1.left_stick_x*-1 * SPEED,
-gamepad1.right_stick_x * TURN
));
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/StandardTrackingWheelLocalizer.java
View File

@ -8,7 +8,7 @@ import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.Encoder;
import java.util.Arrays;
import java.util.List;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java
View File

@ -74,16 +74,6 @@ public final class LogFiles {
public double mecHeadingD = MecanumDrive.HEADING_PID.kD;
public double mecLateralMultiplier = MecanumDrive.LATERAL_MULTIPLIER;
public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;
public double tankAxialD = SampleTankDrive.AXIAL_PID.kD;
public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP;
public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI;
public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD;
public double tankHeadingP = SampleTankDrive.HEADING_PID.kP;
public double tankHeadingI = SampleTankDrive.HEADING_PID.kI;
public double tankHeadingD = SampleTankDrive.HEADING_PID.kD;
public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;

50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/MainTeleOp.java
View File

@ -4,17 +4,16 @@ 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 = "CenterStage", group = "Development")
@com.qualcomm.robotcore.eventloop.opmode.TeleOp(name = "Main TeleOp", group = "Development")
public class MainTeleOp extends OpMode {
private Robot robot;
private double speedMultiplier = 1.0; // Start at 100% speed
private MecanumDrive drive;
@Override
public void init() {
this.robot = new Robot().init(hardwareMap);
this.robot.getHang().lock();
}
@Override
@ -22,19 +21,48 @@ public class MainTeleOp extends OpMode {
//Drive
robot.getDrive().setInput(gamepad1, gamepad2);
//Hang
if (gamepad1.b) {
if (gamepad2.dpad_up) {
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();
// 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) {
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();
}
//Claw
if (gamepad2.b) {
this.robot.getClaw().open();
} else if (gamepad2.y) {
this.robot.getClaw().openScore();
} else {
this.robot.getClaw().close();
}
//Wrist
if (gamepad2.left_bumper || gamepad2.x) {
this.robot.getWrist().wristPickup();
} else if (gamepad2.right_bumper || gamepad2.a) {
this.robot.getWrist().wristScore();
}
//SLOWMO
if (gamepad1.y) {
Configurables.SPEED = .5;
Configurables.TURN = .75;
} else {
Configurables.SPEED = 1;
Configurables.TURN = 1;
}
}
}

49
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -1,25 +1,9 @@
package org.firstinspires.ftc.teamcode.util;
import com.acmerobotics.dashboard.config.Config;
@Config
public class Configurables {
// Robot Constants
public static double R_ARM_POWER = 0.2;
public static double R_ARM_SPEED = 20;
public static int R_ARM_DEFAULT_POS = 0;
public static int R_ARM_UP_POS = 221;
public static int R_ARM_ALMOST_DOWN_POS = 650;
public static int R_ARM_DOWN_POS = 750;
public static double R_CLAW_CLOSED = 0.13;
public static double R_CLAW_OPEN = 0.7;
public static double R_INTAKE_SPEED = 0.9;
public static double R_INTAKE_SHIELD_UP = 0.17;//0.05
public static double R_INTAKE_SHIELD_DOWN = 0.68;//0.95
public static double R_INTAKE_SHIELD_SPEED = 0.04;
public static double R_SHOOTER_GOAL_POWER = 0.66;
public static double R_SHOOTER_MID_GOAL_POWER = 0.54;
public static double R_SHOOTER_POWERSHOT_POWER = 0.57;
public static double R_PUSHER_CLOSED = 0.35;
public static double R_PUSHER_OPEN = 0.55;
public static double R_PUSHER_DELAY = 0.15;
// CV Color Threshold Constants
public static Color FTC_RED_LOWER = new Color(165, 80, 80);
@ -29,17 +13,22 @@ public class Configurables {
public static Color FTC_WHITE_LOWER = new Color(0, 0, 40);
public static Color FTC_WHITE_UPPER = new Color(180, 30, 255);
// CV Detection Constants
public static double CV_MIN_STARTERSTACK_AREA = 0;
public static double CV_MIN_STARTERSTACK_SINGLE_AREA = 0.08;
public static double CV_MIN_STARTERSTACK_QUAD_AREA = 1.3;
public static double CV_MIN_GOAL_AREA = 0;
public static double CV_MAX_GOAL_AREA = 0.3;
//Servo Positions
public static double UNLOCKSPEED = .5;
public static double LOCKSPEED = .1;
public static double UNLOCK = .5;
public static double LOCK = .1;
public static double UNLOCKSPEED = .1;
public static double LOCKSPEED = .25;
public static double UNLOCK = .6;
public static double LOCK = 0.4;
public static double ARMUP = .88;
public static double ARMDOWN = 0.15;
public static double PICKUP = .935;
public static double WRISTPICKUP = 0.28;
public static double WRISTSCORE = .96;
public static double OPEN = 0.53;
public static double BIGOPEN = 0.45;
public static double CLOSE = 0.6;
//Drive Speed
public static double SPEED = 1;
public static double TURN = 1;
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -51,6 +51,9 @@ public class Constants {
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
public static final String LEFT = "leftHang";
public static final String RIGHT = "rightHang";
public static final String LEFTHANG = "leftHang";
public static final String RIGHTHANG = "rightHang";
public static final String LEFTARM = "leftArm";
public static final String RIGHTARM = "rightArm";
public static final String WRIST = "wrist";
}

82
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java
View File

@ -1,82 +0,0 @@
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.Configurables.CV_MAX_GOAL_AREA;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA;
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 TargetingPipeline 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(), CV_MIN_GOAL_AREA);
}
// 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<MatOfPoint> 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(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
newDetection.setContour(contours.get(i));
newDetection.draw(input, RED);
}
red.setContour(getLargestContour(contours));
red.fill(input, RED);
}
public Detection getRed() {
return this.red;
}
}