Auto red zone 1 pixel push works, red other spikes have been coded and not tested. goog
This commit is contained in:
sihan
parent
e74e731085
commit
54b6548d36
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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<Integer> 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
|
||||
));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<Double> getWheelPositions() {
|
||||
return Arrays.asList(
|
||||
encoderTicksToInches(parallelEncoder.getCurrentPosition()) * X_MULTIPLIER,
|
||||
encoderTicksToInches(perpendicularEncoder.getCurrentPosition()) * Y_MULTIPLIER
|
||||
);
|
||||
}
|
||||
|
||||
@NonNull
|
||||
@Override
|
||||
public List<Double> 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
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
|
@ -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
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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<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(), 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;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue