Vision
This commit is contained in:
parent
0dfcef62af
commit
5238efc1cc
|
|
@ -1,20 +1,16 @@
|
|||
package opmodes;
|
||||
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.CLAW_ARM_DELTA;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_LIFT_DELTA;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.PICKUP_ARM_MAX;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_POWER;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.SLIDE_UP;
|
||||
import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.X_CENTER;
|
||||
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")
|
||||
|
|
|
|||
|
|
@ -1,19 +1,30 @@
|
|||
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 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.ColorDetectionPipeline;
|
||||
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 ColorDetectionPipeline colorDetectionPipeline;
|
||||
@Getter
|
||||
@Setter
|
||||
private CenterStageCommon.Alliance alliance;
|
||||
private PropDetectionPipeline colorDetectionPipeline;
|
||||
private AprilTagProcessor aprilTag;
|
||||
private VisionPortal visionPortal;
|
||||
private Telemetry telemetry;
|
||||
|
|
@ -26,14 +37,60 @@ public class Camera {
|
|||
}
|
||||
|
||||
private void init(HardwareMap hardwareMap) {
|
||||
this.aprilTag = AprilTagProcessor.easyCreateWithDefaults();
|
||||
this.colorDetectionPipeline = new ColorDetectionPipeline();
|
||||
this.aprilTag = new AprilTagProcessor.Builder()
|
||||
.setDrawAxes(true)
|
||||
.setDrawCubeProjection(true)
|
||||
.setDrawTagID(true)
|
||||
.setDrawTagOutline(true)
|
||||
.build();
|
||||
this.colorDetectionPipeline = new PropDetectionPipeline();
|
||||
this.visionPortal = VisionPortal.easyCreateWithDefaults(
|
||||
hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, colorDetectionPipeline);
|
||||
this.initialized = true;
|
||||
}
|
||||
|
||||
public Detection getRed() {
|
||||
return (initialized ? colorDetectionPipeline.getRed() : INVALID_DETECTION);
|
||||
public Detection getProp() {
|
||||
if (!initialized || alliance == null) {
|
||||
return INVALID_DETECTION;
|
||||
}
|
||||
|
||||
switch (alliance) {
|
||||
|
||||
case Blue:
|
||||
return this.colorDetectionPipeline.getBlue();
|
||||
case Red:
|
||||
return this.colorDetectionPipeline.getRed();
|
||||
}
|
||||
|
||||
return 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,19 +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.SLIDE_POWER;
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
|
|
@ -34,6 +34,7 @@ public class RobotConstants {
|
|||
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;
|
||||
|
|
@ -57,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;
|
||||
}
|
||||
|
|
@ -1,92 +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 android.graphics.Canvas;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
|
||||
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.Scalar;
|
||||
import org.opencv.core.Size;
|
||||
import org.opencv.imgproc.Imgproc;
|
||||
import org.openftc.easyopencv.OpenCvPipeline;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class ColorDetectionPipeline implements VisionProcessor {
|
||||
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 redLower1;
|
||||
Scalar redUpper1;
|
||||
Scalar redLower2;
|
||||
Scalar redUpper2;
|
||||
|
||||
private Detection red;
|
||||
|
||||
// Init
|
||||
@Override
|
||||
public void init(int width, int height, CameraCalibration calibration) {
|
||||
red = new Detection(new Size(width, height), CV_MIN_GOAL_AREA);
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
updateRed(input);
|
||||
|
||||
return input;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
|
||||
|
||||
}
|
||||
|
||||
// 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
|
||||
redLower1 = new Scalar(FTC_RED_LOWER.getH(), FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
|
||||
redUpper1 = new Scalar(180, FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
|
||||
redLower2 = new Scalar(0, FTC_RED_LOWER.getS(), FTC_RED_LOWER.getV());
|
||||
redUpper2 = new Scalar(FTC_RED_UPPER.getH(), FTC_RED_UPPER.getS(), FTC_RED_UPPER.getV());
|
||||
Core.inRange(hsv, redLower1, redUpper1, redMask1);
|
||||
Core.inRange(hsv, redLower2, redUpper2, 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue