From 94c9deae5b6246a18dc74203e5f86e15b68a4144 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Thu, 16 Nov 2023 20:23:33 -0600 Subject: [PATCH] Center auto mostly working --- TeamCode/src/main/java/opmodes/AutoBase.java | 125 +++++++++++++++++- .../ftc/teamcode/hardware/Claw.java | 7 + .../ftc/teamcode/hardware/Gantry.java | 8 +- .../ftc/teamcode/hardware/RobotConfig.java | 6 +- .../roadrunner/drive/DriveConstants.java | 12 +- .../roadrunner/drive/MecanumDrive.java | 13 +- .../drive/TwoWheelTrackingLocalizer.java | 6 +- .../TrajectorySequenceRunner.java | 5 + .../vision/PropDetectionPipeline.java | 1 - 9 files changed, 157 insertions(+), 26 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 81775e6..bef89bb 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -1,14 +1,30 @@ package opmodes; +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.SCORING_DISTANCE_FROM_APRIL_TAG; +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 com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.robotcore.external.Telemetry; 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; + protected Robot robot; protected FtcDashboard dashboard; protected Telemetry dashboardTelemetry; @@ -20,14 +36,113 @@ public abstract class AutoBase extends LinearOpMode { this.dashboard = FtcDashboard.getInstance(); this.dashboardTelemetry = dashboard.getTelemetry(); - this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Red); + this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Blue); + + this.robot.getDrive().setPoseEstimate(new Pose2d(-36, 63, Math.toRadians(-90))); while(!isStarted() && !isStopRequested()) { - this.propLocation = this.robot.getCamera().getPropLocation(); + this.robot.update(); + this.sleep(20); + } + + setPropLocationIfVisible(Center, null); + + 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); + } + + 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()); + + 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()); + + this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); + this.robot.getGantry().armOut(); + while(this.robot.getGantry().isIn()) { + this.robot.getGantry().update(); + sleep(20); + } + this.robot.getGantry().deposit(); + + 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 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; + } + + 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; + for (int i = 0; i < samples; i++) { Detection detection = this.robot.getCamera().getProp(); - this.dashboardTelemetry.addData("Prop", detection.getCenterPx()); - this.dashboardTelemetry.addData("Prop Location", this.propLocation); - this.dashboardTelemetry.update(); + if (detection.isValid()) { + seenCount++; + } + } + if (seenCount / samples > 0.5) { + this.propLocation = ifVisible; + } else { + this.propLocation = ifNotVisible; } } } 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 e932e66..145c107 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 @@ -41,6 +41,13 @@ public class Claw implements Updatable { this.clawControllerTarget = CLAW_MAX; } + public void openSync() { + this.clawControllerTarget = CLAW_MAX; + while (Math.abs(this.claw.getPosition() - CLAW_MAX) > 0.001) { + this.update(); + } + } + public void close() { this.clawControllerTarget = CLAW_MIN; } 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 133d311..4a9c9eb 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 @@ -53,7 +53,7 @@ public class Gantry { this.liftRight.setMode(DcMotor.RunMode.RUN_TO_POSITION); this.liftRight.setDirection(DcMotorSimple.Direction.REVERSE); - this.xControllerTarget = X_MIN; + this.xControllerTarget = X_CENTER; } public Gantry(HardwareMap hardwareMap, Telemetry telemetry) { @@ -87,13 +87,11 @@ public class Gantry { } public void intake() { - this.screwServo.setPower(1); - this.screwServo.setDirection(DcMotorSimple.Direction.FORWARD); + this.screwServo.setPower(-1); } public void deposit() { - this.screwServo.setPower(1); - this.screwServo.setDirection(DcMotorSimple.Direction.REVERSE); + 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 cb1d8bb..bcef2b6 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 @@ -19,14 +19,15 @@ public class RobotConfig { public static final String ROBOT_LIFT_ROTATION_NAME = "liftRotation"; public static final String ROBOT_LIFT_LIFT_NAME = "liftLift"; public static final String WEBCAM_NAME = "webcam"; - public static final String PARALLEL_DRIVE_ODOMETRY = BACK_RIGHT_NAME; - public static final String PERPENDICULAR_DRIVE_ODOMETRY = FRONT_RIGHT_NAME; + public static final String PARALLEL_DRIVE_ODOMETRY = FRONT_LEFT_NAME; + public static final String PERPENDICULAR_DRIVE_ODOMETRY = BACK_LEFT_NAME; // Drive public static double SLOW_MODE_SPEED_PCT = 0.25; public static double SLOW_MODE_TURN_PCT = 0.25; public static double SPEED = 1f; public static double TURN = 1f; + public static double AUTO_STRAFE_SLOWDOWN = 4; // Arm public static double PICKUP_ARM_MIN = 0.185; @@ -65,4 +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; } 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 f029065..dcf1c98 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 @@ -21,8 +21,8 @@ public class DriveConstants { /* * These are motor constants that should be listed online for your motors. */ - public static final double TICKS_PER_REV = 1; - public static final double MAX_RPM = 1; + public static final double TICKS_PER_REV = 384.5; + public static final double MAX_RPM = 435; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -44,9 +44,9 @@ public class DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - public static double WHEEL_RADIUS = 2; // in + public static double WHEEL_RADIUS = 3.779528 / 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed - public static double TRACK_WIDTH = 1; // in + public static double TRACK_WIDTH = 12.04724; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -65,8 +65,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 30; - public static double MAX_ACCEL = 30; + 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); 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 7c0ce18..cda38e6 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.hardware.roadrunner.drive; +import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.AUTO_STRAFE_SLOWDOWN; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_LEFT_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.BACK_RIGHT_NAME; import static org.firstinspires.ftc.teamcode.hardware.RobotConfig.FRONT_LEFT_NAME; @@ -64,8 +65,8 @@ import java.util.List; */ @Config public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(10, 0, 1.5); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(6, 0, 0); public static double LATERAL_MULTIPLIER = 1; @@ -75,8 +76,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive private TrajectorySequenceRunner trajectorySequenceRunner; - private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); - private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL); + public static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH); + public static final TrajectoryVelocityConstraint SLOW_VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL / AUTO_STRAFE_SLOWDOWN, MAX_ANG_VEL / AUTO_STRAFE_SLOWDOWN, TRACK_WIDTH); + public static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL); private TrajectoryFollower follower; @@ -206,6 +208,9 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive waitForIdle(); } + public void breakFollowing() { + trajectorySequenceRunner.breakFollowing(); + } public Pose2d getLastError() { return trajectorySequenceRunner.getLastPoseError(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java index 30df615..d41dc40 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/TwoWheelTrackingLocalizer.java @@ -38,14 +38,14 @@ import java.util.List; */ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer { public static double TICKS_PER_REV = 2000; - public static double WHEEL_RADIUS = 0.944882; // in + public static double WHEEL_RADIUS = 1.89 / 2; // in public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed public static double PARALLEL_X = -0.269685; // X is the up and down direction - public static double PARALLEL_Y = -4.409449; // Y is the strafe direction + public static double PARALLEL_Y = 4.409449; // Y is the strafe direction public static double PERPENDICULAR_X = 0.6299213; - public static double PERPENDICULAR_Y = -0.1122047; + public static double PERPENDICULAR_Y = 0.1122047; public static double X_MULTIPLIER = 1; // Multiplier in the X direction public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction 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 fb7b6b1..0c7bf57 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 @@ -304,4 +304,9 @@ public class TrajectorySequenceRunner { public boolean isBusy() { return currentTrajectorySequence != null; } + + public void breakFollowing() { + currentTrajectorySequence = null; + remainingMarkers.clear(); + } } 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 35f4a99..de3f226 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 @@ -8,7 +8,6 @@ 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;