Merge branch 'bumblebee_dev' of https://github.com/IronEaglesRobotics/CenterStage into bumblebee_dev
This commit is contained in:
Thomas
commit
4301b8fbe8
|
|
@ -1,16 +1,16 @@
|
|||
package opmodes;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_UP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
|
||||
|
||||
import com.acmerobotics.dashboard.FtcDashboard;
|
||||
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.hardware.Robot;
|
||||
|
||||
@TeleOp(name = "MainTeleOp", group = "Main")
|
||||
|
|
@ -25,11 +25,11 @@ public class MainTeleOp extends OpMode {
|
|||
private boolean previousRobotLiftExtend = false;
|
||||
private boolean liftArmShouldBeUp = false;
|
||||
private boolean screwArmIsMoving = false;
|
||||
private Telemetry dashboard;
|
||||
private FtcDashboard dashboard;
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
this.dashboard = FtcDashboard.getInstance().getTelemetry();
|
||||
this.dashboard = FtcDashboard.getInstance();
|
||||
this.clawArmPosition = PICKUP_ARM_MAX;
|
||||
|
||||
this.robot = new Robot(this.hardwareMap, telemetry);
|
||||
|
|
@ -60,8 +60,7 @@ public class MainTeleOp extends OpMode {
|
|||
boolean screwIntake = gamepad2.left_bumper; // LB
|
||||
boolean slideUp = gamepad2.left_stick_y < -0.25; // Left Y Axis Joystick
|
||||
boolean slideDown = gamepad2.left_stick_y > 0.25; // Left Y Axis Joystick
|
||||
boolean gantryLeft = gamepad2.dpad_left; // dpad left
|
||||
boolean gantryRight = gamepad2.dpad_right; // dpad right
|
||||
double gantryXInput = -gamepad2.right_stick_x;
|
||||
|
||||
// Claw
|
||||
if (openClaw) {
|
||||
|
|
@ -102,19 +101,19 @@ public class MainTeleOp extends OpMode {
|
|||
} else {
|
||||
this.robot.getGantry().stop();
|
||||
}
|
||||
// if ((!previousSlideUp && slideUp) || (!previousSlideDown && slideDown)) {
|
||||
// int currentPosition = this.robot.getGantry().getSlidePosition();
|
||||
// this.robot.getGantry().setTarget(currentPosition + GANTRY_LIFT_DELTA);
|
||||
// }
|
||||
//
|
||||
if (gantryRight) {
|
||||
this.robot.getGantry().setX(X_MIN);
|
||||
} else if (gantryLeft) {
|
||||
this.robot.getGantry().setX(X_MAX);
|
||||
} else {
|
||||
this.robot.getGantry().center();
|
||||
|
||||
if (slideUp) {
|
||||
this.robot.getGantry().setSlideTarget(SLIDE_UP);
|
||||
} else if (slideDown) {
|
||||
this.robot.getGantry().setSlideTarget(0);
|
||||
} else if (previousSlideUp || previousSlideDown){
|
||||
this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition());
|
||||
}
|
||||
|
||||
double gantryXPosition = X_CENTER + (gantryXInput * 0.5);
|
||||
gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX));
|
||||
this.robot.getGantry().setX(gantryXPosition);
|
||||
|
||||
// Robot Lift
|
||||
|
||||
if (robotLiftRotation || this.liftArmShouldBeUp) {
|
||||
|
|
|
|||
|
|
@ -1,55 +1,96 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.WEBCAM_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_CENTER_X;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_LEFT_X;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGHT_X;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
|
||||
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.Telemetry;
|
||||
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
|
||||
import org.firstinspires.ftc.teamcode.vision.Detection;
|
||||
import org.firstinspires.ftc.teamcode.vision.TargetingPipeline;
|
||||
import org.openftc.easyopencv.OpenCvCamera;
|
||||
import org.openftc.easyopencv.OpenCvCameraFactory;
|
||||
import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
|
||||
import org.firstinspires.ftc.vision.VisionPortal;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
|
||||
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
|
||||
|
||||
import lombok.Getter;
|
||||
import lombok.Setter;
|
||||
|
||||
public class Camera {
|
||||
private final HardwareMap hardwareMap;
|
||||
private OpenCvCamera targetingCamera;
|
||||
private TargetingPipeline targetingPipeline;
|
||||
private boolean targetingCameraInitialized;
|
||||
@Getter
|
||||
@Setter
|
||||
private CenterStageCommon.Alliance alliance;
|
||||
private PropDetectionPipeline prop;
|
||||
private AprilTagProcessor aprilTag;
|
||||
private VisionPortal visionPortal;
|
||||
private Telemetry telemetry;
|
||||
|
||||
public Camera(HardwareMap hardwareMap) {
|
||||
this.hardwareMap = hardwareMap;
|
||||
private boolean initialized;
|
||||
|
||||
public Camera(HardwareMap hardwareMap, Telemetry telemetry) {
|
||||
this.telemetry = telemetry;
|
||||
this.init(hardwareMap);
|
||||
}
|
||||
|
||||
public void initTargetingCamera() {
|
||||
int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
|
||||
this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, WEBCAM_NAME), 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) {
|
||||
|
||||
}
|
||||
});
|
||||
private void init(HardwareMap hardwareMap) {
|
||||
this.aprilTag = new AprilTagProcessor.Builder()
|
||||
.setDrawAxes(true)
|
||||
.setDrawCubeProjection(true)
|
||||
.setDrawTagID(true)
|
||||
.setDrawTagOutline(true)
|
||||
.build();
|
||||
this.prop = new PropDetectionPipeline();
|
||||
this.visionPortal = VisionPortal.easyCreateWithDefaults(
|
||||
hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, prop);
|
||||
this.initialized = true;
|
||||
}
|
||||
|
||||
public void stopTargetingCamera() {
|
||||
if (targetingCameraInitialized) {
|
||||
targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false);
|
||||
public Detection getProp() {
|
||||
if (!initialized || alliance == null) {
|
||||
return INVALID_DETECTION;
|
||||
}
|
||||
|
||||
switch (alliance) {
|
||||
|
||||
case Blue:
|
||||
return this.prop.getBlue();
|
||||
case Red:
|
||||
return this.prop.getRed();
|
||||
}
|
||||
|
||||
return INVALID_DETECTION;
|
||||
}
|
||||
|
||||
public Detection getRed() {
|
||||
return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION);
|
||||
public CenterStageCommon.PropLocation getPropLocation() {
|
||||
Detection prop = this.getProp();
|
||||
if (!prop.isValid()) {
|
||||
return CenterStageCommon.PropLocation.Unknown;
|
||||
}
|
||||
|
||||
double x = prop.getCenter().x + 50;
|
||||
|
||||
if (x <= DETECTION_LEFT_X) {
|
||||
return CenterStageCommon.PropLocation.Left;
|
||||
}
|
||||
if (x <= DETECTION_CENTER_X) {
|
||||
return CenterStageCommon.PropLocation.Center;
|
||||
}
|
||||
if (x <= DETECTION_RIGHT_X) {
|
||||
return CenterStageCommon.PropLocation.Right;
|
||||
}
|
||||
|
||||
return CenterStageCommon.PropLocation.Unknown;
|
||||
}
|
||||
|
||||
public AprilTagDetection getAprilTag(int id) {
|
||||
return this.aprilTag.getDetections()
|
||||
.stream()
|
||||
.filter(x -> x.id == id)
|
||||
.findFirst()
|
||||
.orElse(null);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,13 +1,13 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_NAME;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PController;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
|
|
|||
|
|
@ -1,16 +1,14 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_RIGHT_NAME;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import java.util.Locale;
|
||||
|
||||
public class Drive {
|
||||
private final DcMotor frontLeft;
|
||||
private final DcMotor frontRight;
|
||||
|
|
|
|||
|
|
@ -1,18 +1,19 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_DELTA_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_CENTER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_SCREW_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_X_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LEFT_SLIDE_MOTOR_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.RIGHT_SLIDE_MOTOR_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_DELTA_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_SCREW_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_X_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LEFT_SLIDE_MOTOR_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.RIGHT_SLIDE_MOTOR_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PController;
|
||||
import com.qualcomm.robotcore.hardware.CRServo;
|
||||
|
|
@ -28,7 +29,7 @@ public class Gantry {
|
|||
private final Servo armServo;
|
||||
private final CRServo screwServo;
|
||||
private final DcMotor liftLeft;
|
||||
private final DcMotor right;
|
||||
private final DcMotor liftRight;
|
||||
PController armController = new PController(GANTRY_ARM_KP);
|
||||
private double armControllerTarget;
|
||||
PController xController = new PController(X_KP);
|
||||
|
|
@ -43,11 +44,14 @@ public class Gantry {
|
|||
|
||||
this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME);
|
||||
this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.liftLeft.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
this.liftLeft.setTargetPosition(0);
|
||||
this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
|
||||
this.right = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME);
|
||||
this.right.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.right.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
this.liftRight = hardwareMap.get(DcMotor.class, RIGHT_SLIDE_MOTOR_NAME);
|
||||
this.liftRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
|
||||
this.liftRight.setTargetPosition(0);
|
||||
this.liftRight.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
this.liftRight.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
this.xControllerTarget = X_MIN;
|
||||
}
|
||||
|
|
@ -59,12 +63,10 @@ public class Gantry {
|
|||
|
||||
public void setSlideTarget(int target) {
|
||||
this.liftLeft.setTargetPosition(target);
|
||||
this.liftLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
this.liftLeft.setPower(1);
|
||||
this.liftLeft.setPower(SLIDE_POWER);
|
||||
|
||||
this.right.setTargetPosition(target);
|
||||
this.right.setMode(DcMotor.RunMode.RUN_TO_POSITION);
|
||||
this.right.setPower(1);
|
||||
this.liftRight.setTargetPosition(target);
|
||||
this.liftRight.setPower(SLIDE_POWER);
|
||||
}
|
||||
|
||||
public void setX(double x)
|
||||
|
|
@ -102,6 +104,10 @@ public class Gantry {
|
|||
this.setX(X_CENTER);
|
||||
}
|
||||
|
||||
public int getSlidePosition() {
|
||||
return this.liftLeft.getCurrentPosition();
|
||||
}
|
||||
|
||||
public boolean isIn() {
|
||||
double fudge = (GANTRY_ARM_MAX - GANTRY_ARM_MIN) * .75;
|
||||
return this.armServo.getPosition() < GANTRY_ARM_MIN + fudge;
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d;
|
|||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.checkerframework.checker.units.qual.C;
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
|
||||
|
||||
|
|
@ -23,6 +24,9 @@ public class Robot {
|
|||
@Getter
|
||||
private RobotLift lift;
|
||||
|
||||
@Getter
|
||||
private Camera camera;
|
||||
|
||||
private final Telemetry telemetry;
|
||||
|
||||
public Robot(HardwareMap hardwareMap, Telemetry telemetry) {
|
||||
|
|
@ -36,6 +40,7 @@ public class Robot {
|
|||
this.gantry = new Gantry(hardwareMap, telemetry);
|
||||
this.claw = new Claw(hardwareMap, telemetry);
|
||||
this.lift = new RobotLift(hardwareMap, telemetry);
|
||||
this.camera = new Camera(hardwareMap, telemetry);
|
||||
}
|
||||
|
||||
public void update() {
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.hardware;
|
|||
import com.acmerobotics.dashboard.config.Config;
|
||||
|
||||
@Config
|
||||
public class RobotConstants {
|
||||
public class RobotConfig {
|
||||
public static final String FRONT_LEFT_NAME = "frontLeft";
|
||||
public static final String FRONT_RIGHT_NAME = "frontRight";
|
||||
public static final String BACK_LEFT_NAME = "backLeft";
|
||||
|
|
@ -28,14 +28,13 @@ public class RobotConstants {
|
|||
public static double SPEED = 1f;
|
||||
public static double TURN = 1f;
|
||||
|
||||
// Slide
|
||||
|
||||
// Arm
|
||||
public static double PICKUP_ARM_MIN = 0.185;
|
||||
public static double PICKUP_ARM_MAX = 0.755;
|
||||
public static double CLAW_MIN = 0.92;
|
||||
public static double CLAW_MAX = 0.6;
|
||||
public static double CLAW_ARM_DELTA = 0.03;
|
||||
public static double CLAW_KP = 0.3;
|
||||
|
||||
// Gantry
|
||||
public static double GANTRY_ARM_MIN = 0.435;
|
||||
|
|
@ -47,7 +46,8 @@ public class RobotConstants {
|
|||
public static double X_MIN = 0.16;
|
||||
public static double X_CENTER = 0.54;
|
||||
public static double GANTRY_ARM_DELTA_MAX = 0.013;
|
||||
public static int SLIDE_UP = 100;
|
||||
public static int SLIDE_UP = 820;
|
||||
public static double SLIDE_POWER = 0.5;
|
||||
|
||||
|
||||
// Robot Lift
|
||||
|
|
@ -58,5 +58,11 @@ public class RobotConstants {
|
|||
public static double LIFT_ARM_KP = 0.1;
|
||||
public static double LIFT_POWER = 1f;
|
||||
|
||||
public static double CLAW_KP = 0.3;
|
||||
// Vision
|
||||
public static double CAMERA_OFFSET_X = 0f;
|
||||
public static double DETECTION_AREA_MIN = 0f;
|
||||
public static double DETECTION_AREA_MAX = 1f;
|
||||
public static double DETECTION_LEFT_X = 0;
|
||||
public static double DETECTION_CENTER_X = .5;
|
||||
public static double DETECTION_RIGHT_X = 1;
|
||||
}
|
||||
|
|
@ -1,12 +1,13 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ARM_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_EXTEND_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_RETRACT_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_DOWN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.LIFT_ROTATION_UP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_LIFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.ROBOT_LIFT_ROTATION_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_ARM_KP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_EXTEND_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_POWER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_RETRACT_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_ROTATION_DOWN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LIFT_ROTATION_UP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.ROBOT_LIFT_LIFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.ROBOT_LIFT_ROTATION_NAME;
|
||||
|
||||
import com.arcrobotics.ftclib.controller.PController;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
|
|
@ -15,7 +16,7 @@ import com.qualcomm.robotcore.hardware.Servo;
|
|||
|
||||
import org.firstinspires.ftc.robotcore.external.Telemetry;
|
||||
|
||||
public class RobotLift implements Updatable{
|
||||
public class RobotLift implements Updatable {
|
||||
private Servo rotation;
|
||||
private DcMotor lift;
|
||||
PController armController = new PController(LIFT_ARM_KP);
|
||||
|
|
@ -53,17 +54,17 @@ public class RobotLift implements Updatable{
|
|||
|
||||
public void retract() {
|
||||
this.armControllerTarget = LIFT_ROTATION_UP;
|
||||
int liftTarget = (int)(LIFT_EXTEND_MAX * (1 - LIFT_RETRACT_PCT));
|
||||
int liftTarget = (int) (LIFT_EXTEND_MAX * (1 - LIFT_RETRACT_PCT));
|
||||
int target = this.lift.getCurrentPosition() < liftTarget ? 0 : liftTarget;
|
||||
this.lift.setTargetPosition(target);
|
||||
|
||||
this.lift.setPower(1);
|
||||
this.lift.setPower(LIFT_POWER);
|
||||
}
|
||||
|
||||
public void startReset() {
|
||||
this.armControllerTarget = LIFT_ROTATION_DOWN;
|
||||
this.lift.setTargetPosition(-1 * LIFT_EXTEND_MAX);
|
||||
this.lift.setPower(1);
|
||||
this.lift.setPower(LIFT_POWER);
|
||||
}
|
||||
|
||||
public void stopReset() {
|
||||
|
|
|
|||
|
|
@ -1,13 +1,13 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.BACK_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.FRONT_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_SPEED_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLOW_MODE_TURN_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SPEED;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.TURN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_LEFT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_RIGHT_NAME;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLOW_MODE_SPEED_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLOW_MODE_TURN_PCT;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SPEED;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.TURN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ACCEL;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_ACCEL;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.MAX_ANG_VEL;
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PARALLEL_DRIVE_ODOMETRY;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PERPENDICULAR_DRIVE_ODOMETRY;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PARALLEL_DRIVE_ODOMETRY;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PERPENDICULAR_DRIVE_ODOMETRY;
|
||||
|
||||
import androidx.annotation.NonNull;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,6 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
public class CenterStageCommon {
|
||||
public enum Alliance { Blue, Red }
|
||||
public enum PropLocation { Unknown, Left, Center, Right }
|
||||
}
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
import org.opencv.core.Scalar;
|
||||
import android.graphics.Color;
|
||||
public class Colors {
|
||||
// CV Color Threshold Constants
|
||||
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
|
||||
public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
|
||||
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER);
|
||||
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER);
|
||||
public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
|
||||
public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
|
||||
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_RED_LOWER);
|
||||
public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
|
||||
public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);
|
||||
|
||||
public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED);
|
||||
|
||||
public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE);
|
||||
public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK);
|
||||
public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE);
|
||||
}
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
import org.opencv.core.Point;
|
||||
import org.opencv.core.Rect;
|
||||
import org.opencv.core.Size;
|
||||
|
||||
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);
|
||||
public static Color FTC_RED_UPPER = new Color(15, 255, 255);
|
||||
public static Color FTC_BLUE_LOWER = new Color(75, 40, 80);
|
||||
public static Color FTC_BLUE_UPPER = new Color(120, 255, 255);
|
||||
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;
|
||||
|
||||
}
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
import android.graphics.Paint;
|
||||
|
||||
import org.opencv.core.Mat;
|
||||
import org.opencv.core.MatOfInt;
|
||||
import org.opencv.core.MatOfPoint;
|
||||
|
|
@ -87,4 +89,14 @@ public class OpenCVUtil {
|
|||
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
|
||||
return contours.subList(0, Math.min(numContours, contours.size()));
|
||||
}
|
||||
|
||||
public static class LinePaint extends Paint
|
||||
{
|
||||
public LinePaint(int color)
|
||||
{
|
||||
setColor(color);
|
||||
setAntiAlias(true);
|
||||
setStrokeCap(Paint.Cap.ROUND);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,13 @@
|
|||
package org.firstinspires.ftc.teamcode.util;
|
||||
|
||||
import org.opencv.core.Scalar;
|
||||
|
||||
import lombok.AllArgsConstructor;
|
||||
import lombok.Data;
|
||||
|
||||
@Data
|
||||
@AllArgsConstructor
|
||||
public class ScalarRange {
|
||||
private Scalar upper;
|
||||
private Scalar lower;
|
||||
}
|
||||
|
|
@ -35,10 +35,10 @@ public class Detection {
|
|||
this.maxAreaPx = frameSize.area();
|
||||
}
|
||||
|
||||
public Detection(Size frameSize, double minAreaFactor, double maxSizeFactor) {
|
||||
public Detection(Size frameSize, double minAreaFactor, double maxAreaFactor) {
|
||||
this.maxSizePx = frameSize;
|
||||
this.minAreaPx = frameSize.area() * minAreaFactor;
|
||||
this.maxAreaPx = frameSize.area() * maxSizeFactor;
|
||||
this.maxAreaPx = frameSize.area() * maxAreaFactor;
|
||||
}
|
||||
|
||||
public void setMinArea(double minAreaFactor) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,109 @@
|
|||
package org.firstinspires.ftc.teamcode.vision;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_AREA_MIN;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_CENTER_X;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_LEFT_X;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGHT_X;
|
||||
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE;
|
||||
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1;
|
||||
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2;
|
||||
import static org.firstinspires.ftc.teamcode.util.Colors.WHITE;
|
||||
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.STRUCTURING_ELEMENT;
|
||||
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
|
||||
|
||||
import android.graphics.Canvas;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
|
||||
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
|
||||
import org.firstinspires.ftc.teamcode.util.ScalarRange;
|
||||
import org.firstinspires.ftc.vision.VisionProcessor;
|
||||
import org.opencv.core.Core;
|
||||
import org.opencv.core.Mat;
|
||||
import org.opencv.core.MatOfPoint;
|
||||
import org.opencv.core.Size;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import lombok.Getter;
|
||||
import lombok.Setter;
|
||||
|
||||
public class PropDetectionPipeline implements VisionProcessor {
|
||||
@Getter
|
||||
@Setter
|
||||
CenterStageCommon.Alliance alliance;
|
||||
private Mat blurred = new Mat();
|
||||
private Mat hsv = new Mat();
|
||||
private Mat mask = new Mat();
|
||||
private Mat tmpMask = new Mat();
|
||||
@Getter
|
||||
private Detection red;
|
||||
@Getter
|
||||
private Detection blue;
|
||||
|
||||
// Init
|
||||
@Override
|
||||
public void init(int width, int height, CameraCalibration calibration) {
|
||||
this.red = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
|
||||
this.blue = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
|
||||
}
|
||||
|
||||
// Process each frame that is received from the webcam
|
||||
@Override
|
||||
public Object processFrame(Mat input, long captureTimeNanos) {
|
||||
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
|
||||
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
|
||||
|
||||
if (alliance == CenterStageCommon.Alliance.Red) {
|
||||
red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
|
||||
}
|
||||
|
||||
if (alliance == CenterStageCommon.Alliance.Blue) {
|
||||
blue.setContour(detect(FTC_BLUE_RANGE));
|
||||
}
|
||||
|
||||
return input;
|
||||
}
|
||||
|
||||
private Mat zeros;
|
||||
private Mat zeros(Size size, int type) {
|
||||
if (this.zeros == null) {
|
||||
this.zeros = Mat.zeros(size, type);
|
||||
}
|
||||
|
||||
return this.zeros;
|
||||
}
|
||||
|
||||
private MatOfPoint detect(ScalarRange... colorRanges) {
|
||||
if (!mask.empty()) {
|
||||
mask.setTo(this.zeros(mask.size(), mask.type()));
|
||||
}
|
||||
for (ScalarRange colorRange : colorRanges) {
|
||||
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
|
||||
Core.add(mask, tmpMask, mask);
|
||||
}
|
||||
|
||||
Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
|
||||
Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
|
||||
|
||||
ArrayList<MatOfPoint> contours = new ArrayList<>();
|
||||
Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
|
||||
|
||||
return getLargestContour(contours);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
|
||||
int detectionLeftXPx = (int)((DETECTION_LEFT_X / 100) * onscreenWidth);
|
||||
int detectionCenterXPx = (int)((DETECTION_CENTER_X / 100) * onscreenWidth);
|
||||
int detectionRightXPx = (int)((DETECTION_RIGHT_X / 100) * onscreenWidth);
|
||||
|
||||
canvas.drawLine(detectionLeftXPx, 0, detectionLeftXPx, canvas.getHeight(), WHITE);
|
||||
canvas.drawLine(detectionCenterXPx, 0, detectionCenterXPx, canvas.getHeight(), WHITE);
|
||||
canvas.drawLine(detectionRightXPx, 0, detectionRightXPx, canvas.getHeight(), WHITE);
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue