diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 485317c..d37845b 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -23,7 +23,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection; @Config public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; - public static double SCORING_DURATION_S = 5f; + public static double SCORING_DURATION_S = 3f; // for spin of axle public static double APRIL_TAG_RIGHT_DELTA = -8.5; public static double APRIL_TAG_LEFT_DELTA = 7.0; protected static double Delay = 5000; @@ -55,7 +55,7 @@ public abstract class AutoBase extends LinearOpMode { this.robot.update(); this.sleep(20); } - if (isStopRequested()) { + if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering return; } @@ -130,7 +130,9 @@ public abstract class AutoBase extends LinearOpMode { break; } double distance = this.robot.getCamera().getDistanceToAprilTag(this.alliance == CenterStageCommon.Alliance.Blue ? 2:5, 25, 5); - Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + (distance - SCORING_DISTANCE_FROM_APRIL_TAG), this.robot.getDrive().getPoseEstimate().getY() + delta); + Vector2d target = new Vector2d(this.robot.getDrive().getPoseEstimate().getX() + + (distance - SCORING_DISTANCE_FROM_APRIL_TAG), + this.robot.getDrive().getPoseEstimate().getY() + delta); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(target); this.robot.getDrive().followTrajectorySequence(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/AutoSandbox.java b/TeamCode/src/main/java/opmodes/AutoSandbox.java new file mode 100644 index 0000000..d134e24 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/AutoSandbox.java @@ -0,0 +1,73 @@ +package opmodes; + +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; + +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Autonomous(name = "AutoSandbox", preselectTeleOp = "MainTeleOp") +public class AutoSandbox extends AutoBase { + private final Pose2d rendezvous = new Pose2d(12, 48, Math.toRadians(-89)); + + public AutoSandbox() { + super( + CenterStageCommon.Alliance.Blue, + new Pose2d(12, 63, Math.toRadians(-90)), + new Pose2d(62, 62)); + } + + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(36, 25, Math.toRadians(-33))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } + + protected void propCenter() { + + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(12,48, Math.toRadians(90))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + builder.lineToConstantHeading(new Vector2d(12,12)); + + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + +// builder = this.robot.getTrajectorySequenceBuilder(); +// builder.turn((Math.toRadians(90))); +// this.robot.getDrive().followTrajectorySequence(builder.build()); + + +// TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); +// builder.lineToConstantHeading(rendezvous.vec()); +// builder.addDisplacementMarker(10, () -> { +// this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); +// }); +// this.robot.getDrive().followTrajectorySequence(builder.build()); +// +// openAndLiftClaw(); + } + + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0))); + builder.lineToConstantHeading(new Vector2d(19, 34)); + builder.addTemporalMarker(0.5, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java index 4c312ae..f45dbeb 100644 --- a/TeamCode/src/main/java/opmodes/BlueBackStage.java +++ b/TeamCode/src/main/java/opmodes/BlueBackStage.java @@ -11,7 +11,7 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "BlueBackStage", preselectTeleOp = "MainTeleOp") public class BlueBackStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(12, 11); + private final Pose2d rendezvous = new Pose2d(12, 10); public BlueBackStage() { super( diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index 23c4f97..2aab26b 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -24,7 +24,7 @@ public class BlueFrontStage extends AutoBase { this.sleep(5000); TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); - builder.lineToConstantHeading(new Vector2d(-42, 31)); + builder.lineToConstantHeading(new Vector2d(-40, 31)); builder.addTemporalMarker(0.2, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); diff --git a/TeamCode/src/main/java/opmodes/MainTeleOp.java b/TeamCode/src/main/java/opmodes/MainTeleOp.java index 833d6e3..672b82f 100644 --- a/TeamCode/src/main/java/opmodes/MainTeleOp.java +++ b/TeamCode/src/main/java/opmodes/MainTeleOp.java @@ -33,9 +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; + private int targetTagId; + @Override public void init() { this.dashboard = FtcDashboard.getInstance(); @@ -122,6 +122,7 @@ public class MainTeleOp extends OpMode { this.robot.getGantry().setSlideTarget(SLIDE_UP); } else if (slideDown) { this.robot.getGantry().setSlideTarget(0); + this.robot.getGantry().stop(); } else if (previousSlideUp || previousSlideDown) { this.robot.getGantry().setSlideTarget(this.robot.getGantry().getSlidePosition()); } else if (!isGantryFlipped) @@ -168,22 +169,25 @@ public class MainTeleOp extends OpMode { } Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); + dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); + dashboard.getTelemetry().update(); if (poseFromAprilTag != null) { - if (leftPressed && !leftWasPressed) { + if (leftPressed) { macroToScore(poseFromAprilTag, true); - } else if (rightPressed && !rightWasPressed) { + } else if (rightPressed) { macroToScore(poseFromAprilTag, false); } + } else { + if (leftPressed || rightPressed) { + moveToStartSquare(); + } } - if (!leftPressed && !rightPressed) { // breaks drive as makes it so it continuously brakes, added !this.robot.getDrive().isBusy just for placement + if (!leftPressed && !rightPressed) { this.robot.getDrive().breakFollowing(); } - this.leftWasPressed = leftPressed; - this.rightWasPressed = rightPressed; - this.previousSlideUp = slideUp; this.previousScrewArmToggle = screwArmToggle; this.previousRobotLiftReset = robotLiftReset; @@ -192,7 +196,30 @@ public class MainTeleOp extends OpMode { this.robot.update(); } + 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(34, y, 0)); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + } + private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + if (this.robot.getDrive().isBusy()) { + return; + } + 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 diff --git a/TeamCode/src/main/java/opmodes/RedBackStage.java b/TeamCode/src/main/java/opmodes/RedBackStage.java index c729c66..d208a72 100644 --- a/TeamCode/src/main/java/opmodes/RedBackStage.java +++ b/TeamCode/src/main/java/opmodes/RedBackStage.java @@ -11,19 +11,19 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "RedBackStage", preselectTeleOp = "MainTeleOp") public class RedBackStage extends AutoBase { - private final Pose2d rendezvous = new Pose2d(12, 11); + private final Pose2d rendezvous = new Pose2d(12, -11); public RedBackStage() { super( CenterStageCommon.Alliance.Red, - new Pose2d(12, 63, Math.toRadians(-90)), + new Pose2d(12, -63, Math.toRadians(90)), new Pose2d(62, -62)); } protected void propLeft() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(32, 34, Math.toRadians(0))); - builder.lineToConstantHeading(new Vector2d(19, 34)); + builder.lineToLinearHeading(new Pose2d(32, -34, Math.toRadians(0))); + builder.lineToConstantHeading(new Vector2d(19, -34)); builder.addTemporalMarker(0.5, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); @@ -50,7 +50,7 @@ public class RedBackStage extends AutoBase { protected void propRight() { // red back side TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(-33))); + builder.lineToLinearHeading(new Pose2d(36, -25, Math.toRadians(33))); builder.addDisplacementMarker(10, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); diff --git a/TeamCode/src/main/java/opmodes/Test.java b/TeamCode/src/main/java/opmodes/Test.java index d8a46a7..606fa14 100644 --- a/TeamCode/src/main/java/opmodes/Test.java +++ b/TeamCode/src/main/java/opmodes/Test.java @@ -37,6 +37,10 @@ public class Test extends OpMode { public void loop() { robot.update(); + // Drive + boolean slowmode = gamepad1.right_bumper || gamepad1.y; + this.robot.getDrive().setInput(gamepad1, gamepad2, slowmode); + Vector2d poseFromAprilTag = this.robot.getCamera().getPoseFromAprilTag(2, 5); dashboard.getTelemetry().addData("Inferred Position", poseFromAprilTag); dashboard.getTelemetry().update(); @@ -44,13 +48,13 @@ public class Test extends OpMode { boolean leftPressed = gamepad1.dpad_left; boolean rightPressed = gamepad1.dpad_right; if (poseFromAprilTag != null) { - if (leftPressed && !leftWasPressed) { + if (leftPressed) { macroToScore(poseFromAprilTag, true); - } else if (rightPressed && !rightWasPressed) { + } else if (rightPressed) { macroToScore(poseFromAprilTag, false); } } else { - if (leftPressed && !leftWasPressed || rightPressed && !rightWasPressed) { + if (leftPressed || rightPressed) { moveToStartSquare(); } } @@ -78,11 +82,14 @@ public class Test extends OpMode { double y = targetTagId == 2 ? 36f : -36f; TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(36, y, 0)); + builder.lineToLinearHeading(new Pose2d(34, y, 0)); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); } private void macroToScore(Vector2d poseFromAprilTag, boolean left) { + if (this.robot.getDrive().isBusy()) { + return; + } Pose2d target; Pose2d poseEstimate = new Pose2d(poseFromAprilTag.getX(), poseFromAprilTag.getY(), this.robot.getDrive().getPoseEstimate().getHeading()); double y = poseEstimate.getY() > 0 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 8857006..d36bdee 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 @@ -32,6 +32,8 @@ import java.util.Arrays; public class Camera { public static float PROP_REJECTION_VERTICAL_UPPER = 150; public static float PROP_REJECTION_VERTICAL_LOWER = 275; + public static float PROP_REJECTION_HORIZONTAL_LEFT = 50; + public static float PROP_REJECTION_HORIZONTAL_RIGHT = 480 - PROP_REJECTION_HORIZONTAL_LEFT; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -122,11 +124,11 @@ public class Camera { switch (aprilTagDetection.id) { case 2: ourPoseX = tag2Pose.getX() - ftcPose.y - FORWARD_OFFSET_IN; - ourPoseY = tag2Pose.getY() - ftcPose.x - SIDE_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; + ourPoseY = tag5Pose.getY() + ftcPose.x - SIDE_OFFSET_IN; break; default: ourPoseX = 0; 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 3998864..5b6d54c 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 @@ -32,9 +32,9 @@ public class RobotConfig { // 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 PICKUP_ARM_MAX = 0.760; + public static double CLAW_MIN = 0.89; + public static double CLAW_MAX = 0.65; public static double CLAW_ARM_DELTA = 0.03; public static double CLAW_KP = 0.3; @@ -71,7 +71,7 @@ public class RobotConfig { public static double DETECTION_LEFT_X = 0; 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 double SCORING_DISTANCE_FROM_APRIL_TAG = 7f; 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 47fa767..9f0c394 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 @@ -208,8 +208,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive } public void breakFollowing() { + boolean wasBusy = trajectorySequenceRunner.isBusy(); trajectorySequenceRunner.breakFollowing(); - if (trajectorySequenceRunner.isBusy()) { + if (wasBusy) { setWeightedDrivePower(new Pose2d()); } } @@ -333,6 +334,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive } public void setInput(Gamepad gamepad1, Gamepad gamepad2, boolean slowmode) { + if (isBusy()) { + return; + } double speedScale = slowmode ? SLOW_MODE_SPEED_PCT : SPEED; double turnScale = slowmode ? SLOW_MODE_TURN_PCT : TURN; 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 index 36502d6..90ca238 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java @@ -1,9 +1,13 @@ package org.firstinspires.ftc.teamcode.vision; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_HORIZONTAL_LEFT; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_HORIZONTAL_RIGHT; import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_LOWER; import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER; 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_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; @@ -19,6 +23,8 @@ 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.hardware.Camera; +import org.firstinspires.ftc.teamcode.hardware.Gantry; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import org.firstinspires.ftc.teamcode.util.ScalarRange; import org.firstinspires.ftc.vision.VisionProcessor; @@ -33,6 +39,7 @@ import java.util.ArrayList; import lombok.Getter; import lombok.Setter; +import opmodes.Test; public class PropDetectionPipeline implements VisionProcessor { @Getter @@ -92,7 +99,9 @@ public class PropDetectionPipeline implements VisionProcessor { Imgproc.rectangle(mask, new Point(0,0), new Point(mask.cols() - 1, (int)PROP_REJECTION_VERTICAL_UPPER), BLACK, -1); Imgproc.rectangle(mask, new Point(0,(int)PROP_REJECTION_VERTICAL_LOWER), new Point(mask.cols() - 1, mask.rows() -1), BLACK, -1); - + Imgproc.rectangle(mask, new Point(0,0), new Point(PROP_REJECTION_HORIZONTAL_LEFT, mask.rows() - 1), BLACK, -1); + Imgproc.rectangle(mask, new Point(PROP_REJECTION_HORIZONTAL_RIGHT, 0), new Point(mask.cols() - 1, mask.rows() - 1), BLACK, -1); + Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);