diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 7ff6a09..fce8acd 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,7 +1,11 @@ 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; @@ -25,11 +29,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 +64,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 +105,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..823a506 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 @@ -2,54 +2,38 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.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.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.ColorDetectionPipeline; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; public class Camera { - private final HardwareMap hardwareMap; - private OpenCvCamera targetingCamera; - private TargetingPipeline targetingPipeline; - private boolean targetingCameraInitialized; + private ColorDetectionPipeline colorDetectionPipeline; + 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) { - - } - }); - } - - public void stopTargetingCamera() { - if (targetingCameraInitialized) { - targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false); - } + private void init(HardwareMap hardwareMap) { + this.aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + this.colorDetectionPipeline = new ColorDetectionPipeline(); + this.visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, WEBCAM_NAME), aprilTag, colorDetectionPipeline); + this.initialized = true; } public Detection getRed() { - return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION); + return (initialized ? colorDetectionPipeline.getRed() : INVALID_DETECTION); } } \ No newline at end of file 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..9886426 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 @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConstants.GANTRY_ARM_ 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; @@ -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/RobotConstants.java index 40c40f6..aca1b6d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConstants.java @@ -28,8 +28,6 @@ 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; @@ -47,7 +45,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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java similarity index 67% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java index 6a7fd95..4e59c4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/ColorDetectionPipeline.java @@ -11,38 +11,43 @@ 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 TargetingPipeline extends OpenCvPipeline { +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 redGoalLower1; - Scalar redGoalUpper1; - Scalar redGoalLower2; - Scalar redGoalUpper2; + Scalar redLower1; + Scalar redUpper1; + Scalar redLower2; + Scalar redUpper2; private Detection red; // Init @Override - public void init(Mat input) { - red = new Detection(input.size(), CV_MIN_GOAL_AREA); + 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 Mat processFrame(Mat input) { + public Object processFrame(Mat input, long captureTimeNanos) { Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0); Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV); @@ -51,15 +56,20 @@ public class TargetingPipeline extends OpenCvPipeline { 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 - 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); + 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);