From 506069cc64b4f549bced7199726b11f1b282c147 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Fri, 17 Nov 2023 14:11:53 -0600 Subject: [PATCH] Lots of nice abstraction! --- TeamCode/src/main/java/opmodes/AutoBase.java | 132 ++++++++++-------- TeamCode/src/main/java/opmodes/LeftAuto.java | 11 ++ .../ftc/teamcode/hardware/Camera.java | 16 ++- .../ftc/teamcode/hardware/Claw.java | 6 +- .../ftc/teamcode/hardware/Gantry.java | 4 +- .../ftc/teamcode/hardware/Robot.java | 21 ++- .../roadrunner/drive/MecanumDrive.java | 7 +- .../TrajectorySequenceRunner.java | 1 - .../hardware/roadrunner/util/LogFiles.java | 1 - .../ftc/teamcode/util/Colors.java | 3 +- .../ftc/teamcode/vision/Detection.java | 14 +- 11 files changed, 133 insertions(+), 83 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index bef89bb..6c0f808 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -18,68 +18,63 @@ 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.teamcode.vision.Detection; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; -import org.opencv.core.Point; @Config public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; + public static int SCORING_DURATION_MS = 5000; 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 AutoBase(CenterStageCommon.Alliance alliance, Pose2d initialPosition, Vector2d rendezvous) { + this.alliance = alliance; + this.initialPosition = initialPosition; + this.rendezvous = rendezvous; + } @Override public void runOpMode() { - this.robot = new Robot(hardwareMap, telemetry); + // Initialize Robot and Dashboard + this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance); this.dashboard = FtcDashboard.getInstance(); this.dashboardTelemetry = dashboard.getTelemetry(); - this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue); - - this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90))); - + // Wait for match to start while(!isStarted() && !isStopRequested()) { this.robot.update(); this.sleep(20); } - setPropLocationIfVisible(Center, null); + // If the prop is visible at this point, then it must be in the center (2) position + determinePropLocation(); - TrajectorySequenceBuilder builder = this.robot.getDrive() - .trajectorySequenceBuilder(new Pose2d(-36, 63, Math.toRadians(-90))); - if (this.propLocation != CenterStageCommon.PropLocation.Center) { - builder.forward(5); - builder.turn(Math.toRadians(-33)); - this.robot.getDrive().followTrajectorySequence(builder.build()); - - setPropLocationIfVisible(Right, Left); - return; - } else { - // Center - builder.lineToConstantHeading(new Vector2d(-36, 11)); - 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); + switch (this.propLocation) { + case Left: + // TODO Tommy: Place the pixel on the left tape and move to rendezvous position + break; + case Unknown: + case Center: + dislodgePropAndPlacePixel(); + break; + case Right: + // TODO Tommy: Place the pixel on the right tape and move to rendezvous position + break; } - builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(-36, 11)); - builder.lineToLinearHeading(new Pose2d(36, 11, 0)); - builder.lineToLinearHeading(new Pose2d(36, 38, 0)); - this.robot.getDrive().followTrajectorySequence(builder.build()); + moveToBackstage(); + prepareToScore(2); + scorePreloadedPixel(); - double distance = getDistanceToAprilTag(); - - builder = this.robot.getDrive().trajectorySequenceBuilder(new Pose2d(36, 38, 0)); - builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG); - this.robot.getDrive().followTrajectorySequence(builder.build()); + // TODO Tommy: Park + } + private void scorePreloadedPixel() { this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); this.robot.getGantry().armOut(); while(this.robot.getGantry().isIn()) { @@ -87,33 +82,50 @@ public abstract class AutoBase extends LinearOpMode { sleep(20); } this.robot.getGantry().deposit(); + this.sleep(SCORING_DURATION_MS); + this.robot.getGantry().stop(); + } - while(opModeIsActive()) { - this.robot.update(); - AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2); - if (aprilTagDetection != null) { - Point center = aprilTagDetection.center; - this.dashboardTelemetry.addData("center", center); - this.dashboardTelemetry.addData("x", aprilTagDetection.ftcPose.x); - this.dashboardTelemetry.addData("y", aprilTagDetection.ftcPose.y); - this.dashboardTelemetry.addData("z", aprilTagDetection.ftcPose.z); - this.dashboardTelemetry.update(); - } - sleep(20); + private void prepareToScore(int tagId) { + double distance = this.robot.getCamera().getDistanceToAprilTag(2, 25, 5); + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.forward(distance - SCORING_DISTANCE_FROM_APRIL_TAG); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + private 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 dislodgePropAndPlacePixel() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(rendezvous); + 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, null); + if (this.propLocation != Center) { + peekRight(); } } - private double getDistanceToAprilTag() { - double minDistance = Double.MAX_VALUE; - for (int i = 0; i < 10; i++) { - AprilTagDetection aprilTagDetection = this.robot.getCamera().getAprilTag(2); - if (aprilTagDetection != null) { - if (aprilTagDetection.ftcPose.y < minDistance) { - minDistance = aprilTagDetection.ftcPose.y; - } - } - } - return minDistance; + private void peekRight() { + TrajectorySequenceBuilder builder = this.robot.getDrive() + .trajectorySequenceBuilder(initialPosition); + builder.forward(5); + builder.turn(Math.toRadians(-33)); + this.robot.getDrive().followTrajectorySequence(builder.build()); + setPropLocationIfVisible(Right, Left); } protected static int getExpectedAprilTagId(CenterStageCommon.PropLocation propLocation) { diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java index de2d102..3e235d5 100644 --- a/TeamCode/src/main/java/opmodes/LeftAuto.java +++ b/TeamCode/src/main/java/opmodes/LeftAuto.java @@ -1,9 +1,20 @@ 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() { + super( + CenterStageCommon.Alliance.Red, + new Pose2d(-36, 63, Math.toRadians(-90)), + new Vector2d(-36, 11)); + } + @Override public void runOpMode() { super.runOpMode(); 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 3339346..337796c 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 @@ -17,9 +17,6 @@ import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; -import lombok.Getter; -import lombok.Setter; - public class Camera { private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; @@ -92,6 +89,19 @@ public class Camera { .orElse(null); } + public double getDistanceToAprilTag(int id, double rejectAbove, double rejectBelow) { + for (int i = 0; i < 10; i++) { + AprilTagDetection aprilTagDetection = getAprilTag(id); + if (aprilTagDetection != null) { + if (aprilTagDetection.ftcPose.y < rejectAbove + && aprilTagDetection.ftcPose.y > rejectBelow) { + return aprilTagDetection.ftcPose.y; + } + } + } + return Double.MAX_VALUE; + } + public void setAlliance(CenterStageCommon.Alliance alliance) { this.prop.setAlliance(alliance); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java index 145c107..ba54bc3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Claw.java @@ -1,13 +1,13 @@ package org.firstinspires.ftc.teamcode.hardware; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_LEFT_NAME; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_ARM_RIGHT_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.CLAW_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MAX; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.PICKUP_ARM_MIN; import com.arcrobotics.ftclib.controller.PController; import com.qualcomm.robotcore.hardware.HardwareMap; 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 4a9c9eb..3ad07c4 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,12 +5,12 @@ import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_MIN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_ARM_NAME; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER; -import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_SCREW_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.GANTRY_X_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.LEFT_SLIDE_MOTOR_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.RIGHT_SLIDE_MOTOR_NAME; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.SLIDE_POWER; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_CENTER; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_KP; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MAX; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.X_MIN; 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 1d4318f..a70f187 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,9 +4,10 @@ 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; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; import lombok.Getter; @@ -27,6 +28,9 @@ public class Robot { @Getter private Camera camera; + @Getter + CenterStageCommon.Alliance alliance; + private final Telemetry telemetry; public Robot(HardwareMap hardwareMap, Telemetry telemetry) { @@ -34,6 +38,12 @@ public class Robot { this.init(hardwareMap); } + public Robot(HardwareMap hardwareMap, Telemetry telemetry, Pose2d initialPosition, CenterStageCommon.Alliance alliance) { + this(hardwareMap, telemetry); + this.getDrive().setPoseEstimate(initialPosition); + this.setAlliance(alliance); + } + private void init(HardwareMap hardwareMap) { this.drive = new MecanumDrive(hardwareMap); this.drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); @@ -43,6 +53,15 @@ public class Robot { this.camera = new Camera(hardwareMap, telemetry); } + public TrajectorySequenceBuilder getTrajectorySequenceBuilder() { + return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate()); + } + + public void setAlliance(CenterStageCommon.Alliance alliance) { + this.alliance = alliance; + this.camera.setAlliance(alliance); + } + public void update() { this.gantry.update(); this.lift.update(); 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 cda38e6..f97e1e7 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 @@ -17,6 +17,9 @@ import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveCons import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.RUN_USING_ENCODER; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.TRACK_WIDTH; import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.encoderTicksToInches; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; +import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; import androidx.annotation.NonNull; @@ -51,10 +54,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceRunner; import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LynxModuleUtil; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kV; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kA; -import static org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants.kStatic; - import java.util.ArrayList; import java.util.Arrays; import java.util.List; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java index 0c7bf57..20dbd8b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/trajectorysequence/TrajectorySequenceRunner.java @@ -25,7 +25,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.seq import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.DashboardUtil; import org.firstinspires.ftc.teamcode.hardware.roadrunner.util.LogFiles; - import java.util.ArrayList; import java.util.Collections; import java.util.LinkedList; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java index 5dbb502..946cabb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/util/LogFiles.java @@ -20,7 +20,6 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.StandardTrackingWheelLocalizer; - import java.io.File; import java.io.FileInputStream; import java.io.IOException; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java index beeadc5..6e7af45 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java @@ -1,7 +1,8 @@ package org.firstinspires.ftc.teamcode.util; -import org.opencv.core.Scalar; import android.graphics.Color; + +import org.opencv.core.Scalar; public class Colors { // CV Color Threshold Constants public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java index 89fbb7e..a148fb0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java @@ -1,12 +1,5 @@ package org.firstinspires.ftc.teamcode.vision; -import org.opencv.core.Mat; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Point; -import org.opencv.core.Scalar; -import org.opencv.core.Size; -import org.opencv.imgproc.Imgproc; - import static org.firstinspires.ftc.teamcode.util.Constants.GREEN; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_AREA; import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_POINT; @@ -17,6 +10,13 @@ import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomLeftOfCont import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomRightOfContour; import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getCenterOfContour; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Scalar; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + // Class for a Detection public class Detection { private double minAreaPx;