diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 7ff6a09..66a82fa 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -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) { 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 ca79a12..f5a05a2 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,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); } } \ 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 62c2b59..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,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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index d799b15..1d4318f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -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() { 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 85% 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 40c40f6..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"; @@ -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; } 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/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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java deleted file mode 100644 index 6a7fd95..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java +++ /dev/null @@ -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 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