From 5238efc1ccbd7b55856b8730471ae17e8811449f Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Wed, 15 Nov 2023 15:21:21 -0600 Subject: [PATCH] Vision --- .../src/main/java/opmodes/MainTeleOp.java | 16 +-- .../ftc/teamcode/hardware/Camera.java | 71 ++++++++++-- .../ftc/teamcode/hardware/Claw.java | 16 +-- .../ftc/teamcode/hardware/Drive.java | 10 +- .../ftc/teamcode/hardware/Gantry.java | 28 ++--- .../{RobotConstants.java => RobotConfig.java} | 11 +- .../ftc/teamcode/hardware/RobotLift.java | 23 ++-- .../roadrunner/drive/MecanumDrive.java | 16 +-- .../drive/TwoWheelTrackingLocalizer.java | 4 +- .../ftc/teamcode/util/CenterStageCommon.java | 6 + .../ftc/teamcode/util/Colors.java | 22 ++++ .../ftc/teamcode/util/Configurables.java | 43 ------- .../ftc/teamcode/util/OpenCVUtil.java | 12 ++ .../ftc/teamcode/util/ScalarRange.java | 13 +++ .../vision/ColorDetectionPipeline.java | 92 --------------- .../ftc/teamcode/vision/Detection.java | 4 +- .../vision/PropDetectionPipeline.java | 109 ++++++++++++++++++ 17 files changed, 291 insertions(+), 205 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/{RobotConstants.java => RobotConfig.java} (88%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index fce8acd..66a82fa 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -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") diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java index 823a506..de7968f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java @@ -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); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index f97d1be..e932e66 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java index 6215c72..705f464 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Drive.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java index 9886426..133d311 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Gantry.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java similarity index 88% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index aca1b6d..e224a3a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java @@ -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; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java index afce43f..e33fbc3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotLift.java @@ -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() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java index 775c2d4..7c0ce18 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index 671234e..30df615 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java new file mode 100644 index 0000000..23e7560 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java @@ -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 } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java new file mode 100644 index 0000000..a40a8ce --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java @@ -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); +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java deleted file mode 100644 index a2f575b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ /dev/null @@ -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; - -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java index b757c6b..8cf53b8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java @@ -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); + } + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java new file mode 100644 index 0000000..0af987d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java @@ -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; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java deleted file mode 100644 index 4e59c4d..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java +++ /dev/null @@ -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 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; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java index 7db4bd9..b7962d7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java @@ -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) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java new file mode 100644 index 0000000..cb0628d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java @@ -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 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); + } +} \ No newline at end of file