diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 19af9fb..833d6e3 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -1,7 +1,10 @@ package opmodes; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_DELTA; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN; 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; @@ -9,9 +12,11 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.hardware.Camera; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @@ -28,7 +33,9 @@ public class MainTeleOp extends OpMode { private boolean liftArmShouldBeUp = false; private boolean screwArmIsMoving = false; private FtcDashboard dashboard; - + private boolean leftWasPressed; + private boolean rightWasPressed; + private boolean isGantryFlipped = false; @Override public void init() { this.dashboard = FtcDashboard.getInstance(); @@ -45,12 +52,14 @@ public class MainTeleOp extends OpMode { this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); // Button Mappings + // Drive + boolean leftPressed = gamepad1.dpad_left; + boolean rightPressed = gamepad1.dpad_right; // Claw / Pickup boolean openClaw = gamepad2.b; // B boolean clawUp = gamepad2.y; // Y boolean clawDownSafe = gamepad2.dpad_down; // dpad-down boolean clawDown = gamepad2.a || clawDownSafe; // A - // Robot Drone Launch boolean robotDroneLaunch = gamepad1.left_bumper && gamepad1.right_bumper; // Change if Merck wants (LT) @@ -71,7 +80,7 @@ public class MainTeleOp extends OpMode { if (openClaw) { this.robot.getClaw().open(); this.screwArmIsMoving = false; - } else if (!clawUp && !clawDown && !this.screwArmIsMoving){ + } else if (!clawUp && !clawDown && !this.screwArmIsMoving) { this.robot.getClaw().close(); } if (clawUp) { @@ -88,13 +97,15 @@ public class MainTeleOp extends OpMode { } // Gantry - if (!previousScrewArmToggle && screwArmToggle) { + if (!previousScrewArmToggle && screwArmToggle ) { this.screwArmIsMoving = true; if (screwArmPos) { this.robot.getGantry().armIn(); + this.isGantryFlipped = false; } else { this.robot.getClaw().open(); this.robot.getGantry().armOut(); + this.isGantryFlipped = true; } this.robot.getClaw().open(); screwArmPos = !screwArmPos; @@ -107,17 +118,22 @@ public class MainTeleOp extends OpMode { this.robot.getGantry().stop(); } - if (slideUp) { + if (slideUp && isGantryFlipped) { this.robot.getGantry().setSlideTarget(SLIDE_UP); } else if (slideDown) { this.robot.getGantry().setSlideTarget(0); - } else if (previousSlideUp || previousSlideDown){ + } else if (previousSlideUp || previousSlideDown) { this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition()); - } + } else if (!isGantryFlipped) + this.telemetry.addData("GantryNotFlipped!", "Null"); - double gantryXPosition = X_CENTER + (gantryXInput * 0.5); - gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX)); - this.robot.getGantry().setX(gantryXPosition); + if (isGantryFlipped) { + double gantryXPosition = X_CENTER + (gantryXInput * 0.5); + gantryXPosition = Math.max(X_MIN, Math.min(gantryXPosition, X_MAX)); + this.robot.getGantry().setX(gantryXPosition); + } else { + this.telemetry.addData("GantryNotFlippedGoober", "Null"); + } // Robot Drone @@ -151,6 +167,22 @@ public class MainTeleOp extends OpMode { this.robot.getLift().stopReset(); } + Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + + if (poseFromAprilTag != null) { + if (leftPressed && !leftWasPressed) { + macroToScore(poseFromAprilTag, true); + } else if (rightPressed && !rightWasPressed) { + macroToScore(poseFromAprilTag, false); + } + } + + if (!leftPressed && !rightPressed) { // breaks drive as makes it so it continuously brakes, added !this.robot.getDrive().isBusy just for placement + this.robot.getDrive().breakFollowing(); + } + + this.leftWasPressed = leftPressed; + this.rightWasPressed = rightPressed; this.previousSlideUp = slideUp; this.previousScrewArmToggle = screwArmToggle; @@ -159,4 +191,18 @@ public class MainTeleOp extends OpMode { this.robot.update(); } + + private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + Pose2d target; // defines a new pose2d named target, position not yet given + Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading()); + double y = poseEstimate.getY() > 0 + ? left ? 40 : 30 + : left ? -30 : -40; + this.robot.getDrive().setPoseEstimate(poseEstimate); + target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(target); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + } + } diff --git a/TeamCode/src/main/java/opmodes/Test.java b/TeamCode/src/main/java/opmodes/Test.java new file mode 100644 index 0000000..d8a46a7 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/Test.java @@ -0,0 +1,97 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTANCE_FROM_APRIL_TAG; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.hardware.Camera; +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; + +@TeleOp(name = "Test", group = "Main") +public class Test extends OpMode { + private Robot robot; + + private FtcDashboard dashboard; + private boolean leftWasPressed; + private boolean rightWasPressed; + private int targetTagId; + + @Override + public void init() { + this.dashboard = FtcDashboard.getInstance(); + + this.robot = new Robot(this.hardwareMap, telemetry); + telemetry.addData("Status", "Initialized"); + } + + @Override + public void loop() { + robot.update(); + + Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); + dashboard.getTelemetry().update(); + + boolean leftPressed = gamepad1.dpad_left; + boolean rightPressed = gamepad1.dpad_right; + if (poseFromAprilTag != null) { + if (leftPressed && !leftWasPressed) { + macroToScore(poseFromAprilTag, true); + } else if (rightPressed && !rightWasPressed) { + macroToScore(poseFromAprilTag, false); + } + } else { + if (leftPressed && !leftWasPressed || rightPressed && !rightWasPressed) { + moveToStartSquare(); + } + } + + if (!leftPressed && !rightPressed) { + this.robot.getDrive().breakFollowing(); + } + + leftWasPressed = leftPressed; + rightWasPressed = rightPressed; + } + + private void moveToStartSquare() { + if (this.robot.getDrive().isBusy()) { + return; + } + + Pose2d currentPoseEstimate = this.robot.getDrive().getPoseEstimate(); + if (currentPoseEstimate.getX() < 0) { + return; + } + + this.targetTagId = currentPoseEstimate.getY() >= 0 ? 2 : 5; + + double y = targetTagId == 2 ? 36f : -36f; + + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(36, y, 0)); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + } + + private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + Pose2d target; + Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading()); + double y = poseEstimate.getY() > 0 + ? left ? 40 : 30 + : left ? -30 : -40; + this.robot.getDrive().setPoseEstimate(poseEstimate); + target = new Pose2d(Camera.tag2Pose.getX() - SCORING_DISTANCE_FROM_APRIL_TAG - FORWARD_OFFSET_IN, y - SIDE_OFFSET_IN, 0); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(target); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + } +} 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 fa29457..8857006 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,20 +1,32 @@ package org.firstinspires.ftc.teamcode.hardware; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FORWARD_OFFSET_IN; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SIDE_OFFSET_IN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.WEBCAM_NAME; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION; +import android.hardware.GeomagneticField; + +import androidx.annotation.NonNull; + import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; 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.hardware.roadrunner.drive.StandardTrackingWheelLocalizer; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.vision.Detection; 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.AprilTagPoseFtc; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import org.opencv.core.Point; +import org.jetbrains.annotations.NotNull; + +import java.util.Arrays; @Config public class Camera { @@ -24,7 +36,8 @@ public class Camera { private AprilTagProcessor aprilTag; private VisionPortal visionPortal; private Telemetry telemetry; - + public static final Vector2d tag2Pose = new Vector2d(60, 36); + public static final Vector2d tag5Pose = new Vector2d(60, -36); private boolean initialized; public Camera(HardwareMap hardwareMap, Telemetry telemetry) { @@ -62,10 +75,10 @@ public class Camera { return INVALID_DETECTION; } - public AprilTagDetection getAprilTag(int id) { + public AprilTagDetection getAprilTag(int ... ids) { return this.aprilTag.getDetections() .stream() - .filter(x -> x.id == id) + .filter(x -> Arrays.stream(ids).filter(id -> x.id == id).count() > 0) .findFirst() .orElse(null); } @@ -90,4 +103,37 @@ public class Camera { public CenterStageCommon.Alliance getAlliance() { return this.prop != null ? this.prop.getAlliance() : null; } + + public Vector2d getPoseFromAprilTag(int ... ids) { + if (ids == null || ids.length == 0) { + ids = new int[]{2, 5}; + } + + AprilTagDetection aprilTagDetection = getAprilTag(ids); + + if (aprilTagDetection == null) { + return null; + } + + AprilTagPoseFtc ftcPose = aprilTagDetection.ftcPose; + + double ourPoseX; + double ourPoseY; + switch (aprilTagDetection.id) { + case 2: + ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN; + ourPoseY = tag2Pose.getY() - ftcPose.x - SIDE_OFFSET_IN; + break; + case 5: + ourPoseX = tag5Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN; + ourPoseY = tag5Pose.getY() - ftcPose.x - SIDE_OFFSET_IN; + break; + default: + ourPoseX = 0; + ourPoseY = 0; + break; + } + + return new Vector2d(ourPoseX, ourPoseY); + } } \ 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 218b56d..28146f5 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 @@ -101,11 +101,11 @@ public class Gantry { } public void intake() { - this.screwServo.setPower(1); + this.screwServo.setPower(-1); } public void deposit() { - this.screwServo.setPower(-1); + this.screwServo.setPower(1); } public void stop() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java index fc6d451..3998864 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/RobotConfig.java @@ -72,4 +72,6 @@ public class RobotConfig { public static double DETECTION_CENTER_X = .5; public static double DETECTION_RIGHT_X = 1; public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f; + public static final double FORWARD_OFFSET_IN = 7.75; + public static final double SIDE_OFFSET_IN = 0.5; } 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 f97e1e7..47fa767 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 @@ -209,7 +209,11 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive public void breakFollowing() { trajectorySequenceRunner.breakFollowing(); + if (trajectorySequenceRunner.isBusy()) { + setWeightedDrivePower(new Pose2d()); + } } + public Pose2d getLastError() { return trajectorySequenceRunner.getLastPoseError(); }