From cc8cf18ff0c7bb2896d0533ac307032a8ab6f712 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 12:40:31 -0600 Subject: [PATCH 1/7] Working refactor --- TeamCode/src/main/java/opmodes/AutoBase.java | 17 ++++++++++++----- TeamCode/src/main/java/opmodes/LeftAuto.java | 2 +- .../ftc/teamcode/hardware/Gantry.java | 14 ++++++++++++++ .../ftc/teamcode/hardware/RobotConfig.java | 2 +- .../roadrunner/drive/DriveConstants.java | 4 ++-- 5 files changed, 30 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 7ad6d20..57b62c8 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -22,7 +22,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection; @Config public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; - public static int SCORING_DURATION_MS = 5000; + public static double SCORING_DURATION_S = 5f; protected Robot robot; protected FtcDashboard dashboard; protected Telemetry dashboardTelemetry; @@ -56,14 +56,14 @@ public abstract class AutoBase extends LinearOpMode { switch (this.propLocation) { case Left: // TODO Tommy: Place the pixel on the left tape and move to rendezvous position - break; + return; case Unknown: case Center: dislodgePropAndPlacePixel(); break; case Right: // TODO Tommy: Place the pixel on the right tape and move to rendezvous position - break; + return; } moveToBackstage(); @@ -77,12 +77,18 @@ public abstract class AutoBase extends LinearOpMode { this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); while(this.robot.getGantry().isIn()) { - this.robot.getGantry().update(); + this.robot.update(); sleep(20); } this.robot.getGantry().deposit(); - this.sleep(SCORING_DURATION_MS); + double startTime = this.getRuntime(); + while (this.getRuntime() < (startTime + SCORING_DURATION_S)) { + this.robot.update(); + } this.robot.getGantry().stop(); + this.robot.getGantry().setSlideTarget(0); + this.robot.getGantry().armInSync(); + } private void prepareToScore() { @@ -94,6 +100,7 @@ public abstract class AutoBase extends LinearOpMode { private void moveToBackstage() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(36, 38, 0)); this.robot.getDrive().followTrajectorySequence(builder.build()); diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java index 3e235d5..477d944 100644 --- a/TeamCode/src/main/java/opmodes/LeftAuto.java +++ b/TeamCode/src/main/java/opmodes/LeftAuto.java @@ -10,7 +10,7 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; public class LeftAuto extends AutoBase { public LeftAuto() { super( - CenterStageCommon.Alliance.Red, + CenterStageCommon.Alliance.Blue, new Pose2d(-36, 63, Math.toRadians(-90)), new Vector2d(-36, 11)); } 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 3ad07c4..ec695a2 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 @@ -82,10 +82,24 @@ public class Gantry { this.armControllerTarget = GANTRY_ARM_MAX; } + public void armOutSync() { + this.armOut(); + while (this.armServo.getPosition() < this.armControllerTarget - 0.001) { + this.update(); + } + } + public void armIn() { this.armControllerTarget = GANTRY_ARM_MIN; } + public void armInSync() { + this.armIn(); + while (this.armServo.getPosition() > this.armControllerTarget + 0.001) { + this.update(); + } + } + public void intake() { this.screwServo.setPower(-1); } 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 bcef2b6..d75970e 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 @@ -41,7 +41,7 @@ public class RobotConfig { public static double GANTRY_ARM_MIN = 0.435; public static double GANTRY_ARM_MAX = 0.94; public static int GANTRY_LIFT_DELTA = 50; - public static double GANTRY_ARM_KP = 0.06; + public static double GANTRY_ARM_KP = 0.1; public static double X_KP = 0.1; public static double X_MAX = 0.84; public static double X_MIN = 0.16; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index dcf1c98..bf936f5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -67,8 +67,8 @@ public class DriveConstants { */ public static double MAX_VEL = 45; public static double MAX_ACCEL = 45; - public static double MAX_ANG_VEL = Math.toRadians(60); - public static double MAX_ANG_ACCEL = Math.toRadians(60); + public static double MAX_ANG_VEL = Math.toRadians(720); + public static double MAX_ANG_ACCEL = Math.toRadians(720); /* * Adjust the orientations here to match your robot. See the FTC SDK documentation for details. From 3f9e39e45b82e430e2a80e1ef3d8fcf79bf282b5 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 13:59:57 -0600 Subject: [PATCH 2/7] Working right auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 60 ++++++++++++++++--- TeamCode/src/main/java/opmodes/LeftAuto.java | 2 +- .../ftc/teamcode/hardware/Robot.java | 1 + .../ftc/teamcode/hardware/RobotConfig.java | 2 +- 4 files changed, 54 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 57b62c8..676102c 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -23,15 +23,18 @@ import org.firstinspires.ftc.teamcode.vision.Detection; public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; public static double SCORING_DURATION_S = 5f; + public static double APRIL_TAG_RIGHT_DELTA = -8.5; + public static double APRIL_TAG_LEFT_DELTA = 4.0; + protected Robot robot; protected FtcDashboard dashboard; protected Telemetry dashboardTelemetry; protected CenterStageCommon.PropLocation propLocation; protected final Pose2d initialPosition; protected final CenterStageCommon.Alliance alliance; - protected final Vector2d rendezvous; + protected final Pose2d rendezvous; - protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) { + protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous) { this.alliance = alliance; this.initialPosition = initialPosition; this.rendezvous = rendezvous; @@ -53,17 +56,26 @@ public abstract class AutoBase extends LinearOpMode { // If the prop is visible at this point, then it must be in the center (2) position determinePropLocation(); + TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: // TODO Tommy: Place the pixel on the left tape and move to rendezvous position return; case Unknown: case Center: - dislodgePropAndPlacePixel(); + dislodgePropAndPlacePixelCenter(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); break; case Right: - // TODO Tommy: Place the pixel on the right tape and move to rendezvous position - return; + dislodgePropAndPlacePixelRight(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + break; } moveToBackstage(); @@ -73,6 +85,19 @@ public abstract class AutoBase extends LinearOpMode { // TODO Tommy: Park } + + private void dislodgePropAndPlacePixelRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + this.robot.getClaw().openSync(); + this.sleep(100); + this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); + } + private void scorePreloadedPixel() { this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); @@ -92,23 +117,40 @@ public abstract class AutoBase extends LinearOpMode { } private void prepareToScore() { + // At this point we know that Y = 38 + // For 2 -> Ydelta = 0 + // For 3 -> 3 5/8 + // For 1 -> - 3 5/8 + double delta = 0; + switch (this.propLocation) { + case Left: + delta = APRIL_TAG_LEFT_DELTA; + break; + case Unknown: + case Center: + delta = 0; + break; + case Right: + delta = APRIL_TAG_RIGHT_DELTA; + break; + } double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5); + 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.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG); + builder.lineToConstantHeading(target); this.robot.getDrive().followTrajectorySequence(builder.build()); } private void moveToBackstage() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(90)); builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(36, 38, 0)); this.robot.getDrive().followTrajectorySequence(builder.build()); } - private void dislodgePropAndPlacePixel() { + private void dislodgePropAndPlacePixelCenter() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(rendezvous); + builder.lineToLinearHeading(rendezvous); builder.addDisplacementMarker(10, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java index 477d944..2af6b6e 100644 --- a/TeamCode/src/main/java/opmodes/LeftAuto.java +++ b/TeamCode/src/main/java/opmodes/LeftAuto.java @@ -12,7 +12,7 @@ public class LeftAuto extends AutoBase { super( CenterStageCommon.Alliance.Blue, new Pose2d(-36, 63, Math.toRadians(-90)), - new Vector2d(-36, 11)); + new Pose2d(-36, 11)); } @Override 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 a70f187..c7ae108 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 @@ -54,6 +54,7 @@ public class Robot { } public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { + this.drive.update(); return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate()); } 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 d75970e..9efe412 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 @@ -66,5 +66,5 @@ 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 = 6.5; + public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f; } From c35ed27a099e60412901003fc17cd14125ea42b9 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 14:38:00 -0600 Subject: [PATCH 3/7] Working left auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 23 ++++++++-- .../ftc/teamcode/hardware/Camera.java | 42 +++++++------------ .../vision/PropDetectionPipeline.java | 29 ++++++++----- 3 files changed, 54 insertions(+), 40 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 676102c..ffebca3 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -24,7 +24,7 @@ public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; public static double SCORING_DURATION_S = 5f; public static double APRIL_TAG_RIGHT_DELTA = -8.5; - public static double APRIL_TAG_LEFT_DELTA = 4.0; + public static double APRIL_TAG_LEFT_DELTA = 8.0; protected Robot robot; protected FtcDashboard dashboard; @@ -59,8 +59,12 @@ public abstract class AutoBase extends LinearOpMode { TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: - // TODO Tommy: Place the pixel on the left tape and move to rendezvous position - return; + dislodgePropAndPlacePixelLeft(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + break; case Unknown: case Center: dislodgePropAndPlacePixelCenter(); @@ -85,6 +89,19 @@ public abstract class AutoBase extends LinearOpMode { // TODO Tommy: Park } + private void dislodgePropAndPlacePixelLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); + builder.lineToConstantHeading(new Vector2d(-42, 31)); + builder.addTemporalMarker(0.2, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + this.robot.getClaw().openSync(); + this.sleep(100); + this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); + } + private void dislodgePropAndPlacePixelRight() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); 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 337796c..38e7a13 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,11 +1,9 @@ package org.firstinspires.ftc.teamcode.hardware; -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.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -16,8 +14,11 @@ 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 org.opencv.core.Point; +@Config public class Camera { + public static float PROP_REJECTION_VERTICAL = 300; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -48,39 +49,28 @@ public class Camera { return INVALID_DETECTION; } + Detection detection = null; switch (getAlliance()) { case Blue: Detection blue = this.prop.getBlue(); - return blue != null && blue.isValid() ? blue : INVALID_DETECTION; + detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION; + break; case Red: Detection red = this.prop.getRed(); - return red != null && red.isValid() ? red : INVALID_DETECTION; + detection = red != null && red.isValid() ? red : INVALID_DETECTION; + break; + } + + if (detection != null && detection.isValid()) { + Point center = detection.getCenterPx(); + if (center.y < PROP_REJECTION_VERTICAL) { + return detection; + } } return INVALID_DETECTION; } - public CenterStageCommon.PropLocation getPropLocation() { - Detection prop = this.getProp(); - if (prop == null || !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() 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 de3f226..65d323b 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,5 +1,6 @@ package org.firstinspires.ftc.teamcode.vision; +import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL; 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; @@ -8,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.DETECTION_RIGH 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.RED; 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; @@ -24,6 +26,7 @@ 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.Point; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; @@ -99,20 +102,24 @@ public class PropDetectionPipeline implements VisionProcessor { @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(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE); - canvas.drawLine(detectionLeftXPx, 0, detectionLeftXPx, canvas.getHeight(), WHITE); - canvas.drawLine(detectionCenterXPx, 0, detectionCenterXPx, canvas.getHeight(), WHITE); - canvas.drawLine(detectionRightXPx, 0, detectionRightXPx, canvas.getHeight(), WHITE); - - if (red.isValid()) { - canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); + if (red != null && red.isValid()) { + Point center = red.getCenterPx(); + if (center.y < PROP_REJECTION_VERTICAL) { + canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); + } else { + canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED); + } } - if (blue.isValid()) { - canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); + if (blue != null && blue.isValid()) { + Point center = blue.getCenterPx(); + if (center.y < PROP_REJECTION_VERTICAL) { + canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); + } else { + canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED); + } } } } \ No newline at end of file From 1aa49e979d00122a0b2ec8ec694ca52fe293df06 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 15:17:17 -0600 Subject: [PATCH 4/7] Working auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 19 +++---------------- .../ftc/teamcode/hardware/Camera.java | 17 ++++------------- .../vision/PropDetectionPipeline.java | 19 ++++++++++++------- 3 files changed, 19 insertions(+), 36 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index ffebca3..8ae0c69 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -6,6 +6,7 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SCORING_DISTAN import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Center; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Left; import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Right; +import static org.firstinspires.ftc.teamcode.util.CenterStageCommon.PropLocation.Unknown; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; @@ -167,7 +168,7 @@ public abstract class AutoBase extends LinearOpMode { private void dislodgePropAndPlacePixelCenter() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(rendezvous); + builder.lineToConstantHeading(rendezvous.vec()); builder.addDisplacementMarker(10, () -> { this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); }); @@ -178,7 +179,7 @@ public abstract class AutoBase extends LinearOpMode { } private void determinePropLocation() { - setPropLocationIfVisible(Center, null); + setPropLocationIfVisible(Center, Unknown); if (this.propLocation != Center) { peekRight(); } @@ -193,20 +194,6 @@ public abstract class AutoBase extends LinearOpMode { setPropLocationIfVisible(Right, Left); } - protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) { - switch (propLocation) { - case Left: - return 1; - case Unknown: - case Center: - return 2; - case Right: - return 3; - } - - return 2; - } - protected void setPropLocationIfVisible(CenterStageCommon.PropLocation ifVisible, CenterStageCommon.PropLocation ifNotVisible) { float seenCount = 0; float samples = 10; 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 38e7a13..1c5c7bb 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 @@ -18,7 +18,8 @@ import org.opencv.core.Point; @Config public class Camera { - public static float PROP_REJECTION_VERTICAL = 300; + public static float PROP_REJECTION_VERTICAL_UPPER = 175; + public static float PROP_REJECTION_VERTICAL_LOWER = 300; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -49,23 +50,13 @@ public class Camera { return INVALID_DETECTION; } - Detection detection = null; switch (getAlliance()) { case Blue: Detection blue = this.prop.getBlue(); - detection = blue != null && blue.isValid() ? blue : INVALID_DETECTION; - break; + return blue != null && blue.isValid() ? blue : INVALID_DETECTION; case Red: Detection red = this.prop.getRed(); - detection = red != null && red.isValid() ? red : INVALID_DETECTION; - break; - } - - if (detection != null && detection.isValid()) { - Point center = detection.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { - return detection; - } + return red != null && red.isValid() ? red : INVALID_DETECTION; } return INVALID_DETECTION; 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 65d323b..36502d6 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,17 +1,16 @@ package org.firstinspires.ftc.teamcode.vision; -import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL; +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_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.RED; 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.BLACK; 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; @@ -91,6 +90,9 @@ public class PropDetectionPipeline implements VisionProcessor { Core.add(mask, tmpMask, mask); } + 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.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS); @@ -102,11 +104,13 @@ public class PropDetectionPipeline implements VisionProcessor { @Override public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) { - canvas.drawLine(0, PROP_REJECTION_VERTICAL, canvas.getWidth(), PROP_REJECTION_VERTICAL, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE); + canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE); if (red != null && red.isValid()) { Point center = red.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER + && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED); @@ -115,7 +119,8 @@ public class PropDetectionPipeline implements VisionProcessor { if (blue != null && blue.isValid()) { Point center = blue.getCenterPx(); - if (center.y < PROP_REJECTION_VERTICAL) { + if (center.y < PROP_REJECTION_VERTICAL_LOWER + && center.y > PROP_REJECTION_VERTICAL_UPPER) { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); } else { canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED); From 142256c19cc5fcc1b273733334a64884f161a075 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 15:35:16 -0600 Subject: [PATCH 5/7] Working auto with park --- TeamCode/src/main/java/opmodes/AutoBase.java | 39 +++++++++++-------- .../{LeftAuto.java => BlueFrontStage.java} | 8 ++-- 2 files changed, 27 insertions(+), 20 deletions(-) rename TeamCode/src/main/java/opmodes/{LeftAuto.java => BlueFrontStage.java} (74%) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 8ae0c69..6d09d86 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -34,11 +34,13 @@ public abstract class AutoBase extends LinearOpMode { protected final Pose2d initialPosition; protected final CenterStageCommon.Alliance alliance; protected final Pose2d rendezvous; + protected final Pose2d park; - protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous) { + protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) { this.alliance = alliance; this.initialPosition = initialPosition; this.rendezvous = rendezvous; + this.park = park; } @Override @@ -86,8 +88,7 @@ public abstract class AutoBase extends LinearOpMode { moveToBackstage(); prepareToScore(); scorePreloadedPixel(); - - // TODO Tommy: Park + park(); } private void dislodgePropAndPlacePixelLeft() { @@ -102,8 +103,17 @@ public abstract class AutoBase extends LinearOpMode { this.sleep(100); this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); } - - + private void dislodgePropAndPlacePixelCenter() { + 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()); + this.robot.getClaw().openSync(); + this.sleep(100); + this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); + } private void dislodgePropAndPlacePixelRight() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); @@ -166,17 +176,6 @@ public abstract class AutoBase extends LinearOpMode { this.robot.getDrive().followTrajectorySequence(builder.build()); } - private void dislodgePropAndPlacePixelCenter() { - 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()); - this.robot.getClaw().openSync(); - this.sleep(100); - this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); - } private void determinePropLocation() { setPropLocationIfVisible(Center, Unknown); @@ -209,4 +208,12 @@ public abstract class AutoBase extends LinearOpMode { this.propLocation = ifNotVisible; } } + + public void park() { + double currentX = this.robot.getDrive().getPoseEstimate().getX(); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.strafeTo(new Vector2d(currentX, park.getY())); + builder.lineToConstantHeading(park.vec()); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } } diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java similarity index 74% rename from TeamCode/src/main/java/opmodes/LeftAuto.java rename to TeamCode/src/main/java/opmodes/BlueFrontStage.java index 2af6b6e..f0fc919 100644 --- a/TeamCode/src/main/java/opmodes/LeftAuto.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -1,18 +1,18 @@ package opmodes; import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp") -public class LeftAuto extends AutoBase { - public LeftAuto() { +public class BlueFrontStage extends AutoBase { + public BlueFrontStage() { super( CenterStageCommon.Alliance.Blue, new Pose2d(-36, 63, Math.toRadians(-90)), - new Pose2d(-36, 11)); + new Pose2d(-36, 11), + new Pose2d(62, 12)); } @Override From fbb6eeca9b75f1e8517615176cade4981ef7f490 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 16:49:45 -0600 Subject: [PATCH 6/7] Working BlueBackstage auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 76 +++++-------------- .../src/main/java/opmodes/BlueBackStage.java | 60 +++++++++++++++ .../src/main/java/opmodes/BlueFrontStage.java | 54 +++++++++++-- 3 files changed, 130 insertions(+), 60 deletions(-) create mode 100644 TeamCode/src/main/java/opmodes/BlueBackStage.java diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 6d09d86..49e0671 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -25,7 +25,7 @@ public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; public static double SCORING_DURATION_S = 5f; public static double APRIL_TAG_RIGHT_DELTA = -8.5; - public static double APRIL_TAG_LEFT_DELTA = 8.0; + public static double APRIL_TAG_LEFT_DELTA = 7.0; protected Robot robot; protected FtcDashboard dashboard; @@ -33,13 +33,11 @@ public abstract class AutoBase extends LinearOpMode { protected CenterStageCommon.PropLocation propLocation; protected final Pose2d initialPosition; protected final CenterStageCommon.Alliance alliance; - protected final Pose2d rendezvous; protected final Pose2d park; - protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d rendezvous, Pose2d park) { + protected AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Pose2d park) { this.alliance = alliance; this.initialPosition = initialPosition; - this.rendezvous = rendezvous; this.park = park; } @@ -56,32 +54,24 @@ public abstract class AutoBase extends LinearOpMode { this.sleep(20); } + if (isStopRequested()) { + return; + } + // If the prop is visible at this point, then it must be in the center (2) position determinePropLocation(); TrajectorySequenceBuilder builder; switch (this.propLocation) { case Left: - dislodgePropAndPlacePixelLeft(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); + propLeft(); break; case Unknown: case Center: - dislodgePropAndPlacePixelCenter(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.turn(Math.toRadians(90)); - this.robot.getDrive().followTrajectorySequence(builder.build()); + propCenter(); break; case Right: - dislodgePropAndPlacePixelRight(); - - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(this.rendezvous); - this.robot.getDrive().followTrajectorySequence(builder.build()); + propRight(); break; } @@ -91,42 +81,19 @@ public abstract class AutoBase extends LinearOpMode { park(); } - private void dislodgePropAndPlacePixelLeft() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); - builder.lineToConstantHeading(new Vector2d(-42, 31)); - builder.addTemporalMarker(0.2, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); - this.robot.getDrive().followTrajectorySequence(builder.build()); - this.robot.getClaw().openSync(); - this.sleep(100); - this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); - } - private void dislodgePropAndPlacePixelCenter() { - 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()); - this.robot.getClaw().openSync(); - this.sleep(100); - this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); - } - private void dislodgePropAndPlacePixelRight() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); - builder.addDisplacementMarker(10, () -> { - this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); - }); - this.robot.getDrive().followTrajectorySequence(builder.build()); + protected abstract void propLeft(); + + protected abstract void propCenter(); + + protected abstract void propRight(); + + protected void openAndLiftClaw() { this.robot.getClaw().openSync(); this.sleep(100); this.robot.getClaw().setArmPosition(PICKUP_ARM_MAX); } - private void scorePreloadedPixel() { + protected void scorePreloadedPixel() { this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); while(this.robot.getGantry().isIn()) { @@ -144,7 +111,7 @@ public abstract class AutoBase extends LinearOpMode { } - private void prepareToScore() { + protected void prepareToScore() { // At this point we know that Y = 38 // For 2 -> Ydelta = 0 // For 3 -> 3 5/8 @@ -169,22 +136,21 @@ public abstract class AutoBase extends LinearOpMode { this.robot.getDrive().followTrajectorySequence(builder.build()); } - private void moveToBackstage() { + protected void moveToBackstage() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(new Pose2d(36, 11, 0)); builder.lineToLinearHeading(new Pose2d(36, 38, 0)); this.robot.getDrive().followTrajectorySequence(builder.build()); } - - private void determinePropLocation() { + protected void determinePropLocation() { setPropLocationIfVisible(Center, Unknown); if (this.propLocation != Center) { peekRight(); } } - private void peekRight() { + protected void peekRight() { TrajectorySequenceBuilder builder = this.robot.getDrive() .trajectorySequenceBuilder(initialPosition); builder.forward(5); diff --git a/TeamCode/src/main/java/opmodes/BlueBackStage.java b/TeamCode/src/main/java/opmodes/BlueBackStage.java new file mode 100644 index 0000000..4c312ae --- /dev/null +++ b/TeamCode/src/main/java/opmodes/BlueBackStage.java @@ -0,0 +1,60 @@ +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 = "BlueBackStage", preselectTeleOp = "MainTeleOp") +public class BlueBackStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(12, 11); + + public BlueBackStage() { + 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.lineToConstantHeading(rendezvous.vec()); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + 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(); + } +} diff --git a/TeamCode/src/main/java/opmodes/BlueFrontStage.java b/TeamCode/src/main/java/opmodes/BlueFrontStage.java index f0fc919..02c6f86 100644 --- a/TeamCode/src/main/java/opmodes/BlueFrontStage.java +++ b/TeamCode/src/main/java/opmodes/BlueFrontStage.java @@ -1,22 +1,66 @@ 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 = "Left Auto", preselectTeleOp = "MainTeleOp") +@Autonomous(name = "BlueFrontStage", preselectTeleOp = "MainTeleOp") public class BlueFrontStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(-36, 11); + public BlueFrontStage() { super( CenterStageCommon.Alliance.Blue, new Pose2d(-36, 63, Math.toRadians(-90)), - new Pose2d(-36, 11), new Pose2d(62, 12)); } - @Override - public void runOpMode() { - super.runOpMode(); + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-52, 31, Math.toRadians(-180))); + builder.lineToConstantHeading(new Vector2d(-42, 31)); + builder.addTemporalMarker(0.2, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void propCenter() { + 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(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, 17, Math.toRadians(-123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); } } From 296a382c56c11d74b1681d6fb8cff9de3b34552b Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 17:02:04 -0600 Subject: [PATCH 7/7] [WIP] RedFrontStageAuto --- .../src/main/java/opmodes/RedFrontStage.java | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 TeamCode/src/main/java/opmodes/RedFrontStage.java diff --git a/TeamCode/src/main/java/opmodes/RedFrontStage.java b/TeamCode/src/main/java/opmodes/RedFrontStage.java new file mode 100644 index 0000000..d3c4371 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/RedFrontStage.java @@ -0,0 +1,64 @@ +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 = "RedFrontStage", preselectTeleOp = "MainTeleOp") +public class RedFrontStage extends AutoBase { + private final Pose2d rendezvous = new Pose2d(-36, -11); + + public RedFrontStage() { + super( + CenterStageCommon.Alliance.Red, + new Pose2d(-36, -63, Math.toRadians(90)), + new Pose2d(-12, 62)); + } + + protected void propLeft() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-54, -17, Math.toRadians(123))); + builder.addDisplacementMarker(10, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(this.rendezvous); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propCenter() { + 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(); + + builder = this.robot.getTrajectorySequenceBuilder(); + builder.turn(Math.toRadians(90)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void propRight() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-52, -31, Math.toRadians(-180))); + builder.lineToConstantHeading(new Vector2d(-42, -31)); + builder.addTemporalMarker(0.5, () -> { + this.robot.getClaw().setArmPosition(PICKUP_ARM_MIN); + }); + this.robot.getDrive().followTrajectorySequence(builder.build()); + + openAndLiftClaw(); + } +}