From 385c19ff9f950459726edf45fc07c058771b1955 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Wed, 15 Nov 2023 18:20:50 -0600 Subject: [PATCH 1/2] Fixed prop detector --- TeamCode/src/main/java/opmodes/AutoBase.java | 33 +++++++++++++++++++ TeamCode/src/main/java/opmodes/LeftAuto.java | 11 +++++++ .../ftc/teamcode/hardware/Camera.java | 24 +++++++++----- .../ftc/teamcode/hardware/RobotConfig.java | 4 +-- .../ftc/teamcode/util/Colors.java | 6 ++-- .../ftc/teamcode/vision/Detection.java | 2 +- .../vision/PropDetectionPipeline.java | 16 +++++++-- 7 files changed, 78 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/opmodes/AutoBase.java create mode 100644 TeamCode/src/main/java/opmodes/LeftAuto.java diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java new file mode 100644 index 0000000..81775e6 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -0,0 +1,33 @@ +package opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +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.util.CenterStageCommon; +import org.firstinspires.ftc.teamcode.vision.Detection; + +public abstract class AutoBase extends LinearOpMode { + protected Robot robot; + protected FtcDashboard dashboard; + protected Telemetry dashboardTelemetry; + protected CenterStageCommon.PropLocation propLocation; + + @Override + public void runOpMode() { + this.robot = new Robot(hardwareMap, telemetry); + this.dashboard = FtcDashboard.getInstance(); + this.dashboardTelemetry = dashboard.getTelemetry(); + + this.robot.getCamera().setAlliance(CenterStageCommon.Alliance.Red); + + while(!isStarted() && !isStopRequested()) { + this.propLocation = this.robot.getCamera().getPropLocation(); + Detection detection = this.robot.getCamera().getProp(); + this.dashboardTelemetry.addData("Prop", detection.getCenterPx()); + this.dashboardTelemetry.addData("Prop Location", this.propLocation); + this.dashboardTelemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/opmodes/LeftAuto.java b/TeamCode/src/main/java/opmodes/LeftAuto.java new file mode 100644 index 0000000..de2d102 --- /dev/null +++ b/TeamCode/src/main/java/opmodes/LeftAuto.java @@ -0,0 +1,11 @@ +package opmodes; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +@Autonomous(name = "Left Auto", preselectTeleOp = "MainTeleOp") +public class LeftAuto extends AutoBase { + @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 f5a05a2..3339346 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 @@ -21,9 +21,6 @@ import lombok.Getter; import lombok.Setter; public class Camera { - @Getter - @Setter - private CenterStageCommon.Alliance alliance; private PropDetectionPipeline prop; private AprilTagProcessor aprilTag; private VisionPortal visionPortal; @@ -50,16 +47,17 @@ public class Camera { } public Detection getProp() { - if (!initialized || alliance == null) { + if (!initialized || getAlliance() == null) { return INVALID_DETECTION; } - switch (alliance) { - + switch (getAlliance()) { case Blue: - return this.prop.getBlue(); + Detection blue = this.prop.getBlue(); + return blue != null && blue.isValid() ? blue : INVALID_DETECTION; case Red: - return this.prop.getRed(); + Detection red = this.prop.getRed(); + return red != null && red.isValid() ? red : INVALID_DETECTION; } return INVALID_DETECTION; @@ -67,7 +65,7 @@ public class Camera { public CenterStageCommon.PropLocation getPropLocation() { Detection prop = this.getProp(); - if (!prop.isValid()) { + if (prop == null || !prop.isValid()) { return CenterStageCommon.PropLocation.Unknown; } @@ -93,4 +91,12 @@ public class Camera { .findFirst() .orElse(null); } + + public void setAlliance(CenterStageCommon.Alliance alliance) { + this.prop.setAlliance(alliance); + } + + public CenterStageCommon.Alliance getAlliance() { + return this.prop != null ? this.prop.getAlliance() : null; + } } \ No newline at end of file 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 e224a3a..cb1d8bb 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 @@ -60,8 +60,8 @@ public class RobotConfig { // Vision public static double CAMERA_OFFSET_X = 0f; - public static double DETECTION_AREA_MIN = 0f; - public static double DETECTION_AREA_MAX = 1f; + public static double DETECTION_AREA_MIN = 0.02f; + public static double DETECTION_AREA_MAX = 0.3f; public static double DETECTION_LEFT_X = 0; public static double DETECTION_CENTER_X = .5; public static double DETECTION_RIGHT_X = 1; 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 a40a8ce..beeadc5 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 @@ -6,11 +6,11 @@ public class Colors { // CV Color Threshold Constants public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80); public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255); - public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER); - public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, FTC_RED_LOWER); + public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER); + public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2])); public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80); public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255); - public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_RED_LOWER); + public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER); public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40); public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255); 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 b7962d7..89fbb7e 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 @@ -67,7 +67,7 @@ public class Detection { // Check if the current Detection is valid public boolean isValid() { - return (this.contour != null) && (this.centerPx != INVALID_POINT) && (this.areaPx != INVALID_AREA); + return (this.contour != null) && (this.areaPx != INVALID_AREA); } // Get the current contour 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 cb0628d..35f4a99 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,6 +8,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; @@ -79,11 +80,12 @@ public class PropDetectionPipeline implements VisionProcessor { } private MatOfPoint detect(ScalarRange... colorRanges) { - if (!mask.empty()) { - mask.setTo(this.zeros(mask.size(), mask.type())); - } + mask.release(); for (ScalarRange colorRange : colorRanges) { Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask); + if (mask.empty() || mask.rows() <= 0) { + Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask); + } Core.add(mask, tmpMask, mask); } @@ -105,5 +107,13 @@ public class PropDetectionPipeline implements VisionProcessor { 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 (blue.isValid()) { + canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE); + } } } \ No newline at end of file From 94c9deae5b6246a18dc74203e5f86e15b68a4144 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Thu, 16 Nov 2023 20:23:33 -0600 Subject: [PATCH 2/2] 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;