From 6cfd2876bd132d8e49c5e30a51f69247dde6cfc2 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Thu, 8 Feb 2024 19:01:26 -0600 Subject: [PATCH] Speed up auto --- TeamCode/src/main/java/opmodes/AutoBase.java | 75 ++++++++++++------- .../ftc/teamcode/hardware/Claw.java | 1 + .../ftc/teamcode/hardware/Gantry.java | 3 +- .../ftc/teamcode/hardware/RobotConfig.java | 2 +- .../roadrunner/drive/DriveConstants.java | 4 +- .../roadrunner/drive/MecanumDrive.java | 2 +- 6 files changed, 56 insertions(+), 31 deletions(-) diff --git a/TeamCode/src/main/java/opmodes/AutoBase.java b/TeamCode/src/main/java/opmodes/AutoBase.java index 61c1688..93cde28 100644 --- a/TeamCode/src/main/java/opmodes/AutoBase.java +++ b/TeamCode/src/main/java/opmodes/AutoBase.java @@ -24,7 +24,7 @@ import org.firstinspires.ftc.teamcode.vision.Detection; @Config public abstract class AutoBase extends LinearOpMode { public static int DEPOSIT_HEIGHT = 100; - public static double SCORING_DURATION_S = 3f; // for spin of axle + public static double SCORING_DURATION_S = 2f; // for spin of axle public static double APRIL_TAG_RIGHT_DELTA = -8.5; public static double APRIL_TAG_LEFT_DELTA = 7.0; protected static double Delay = 5000; @@ -61,10 +61,11 @@ public abstract class AutoBase extends LinearOpMode { this.robot = new Robot(hardwareMap, telemetry, initialPosition, alliance); this.dashboard = FtcDashboard.getInstance(); this.dashboardTelemetry = dashboard.getTelemetry(); + this.park = parkLeft; + // Wait for match to start while(!isStarted() && !isStopRequested()) { this.robot.update(); - this.sleep(20); boolean leftPressed = gamepad1.dpad_left; boolean rightPressed = gamepad1.dpad_right; @@ -73,21 +74,22 @@ public abstract class AutoBase extends LinearOpMode { this.telemetry.addData("To select parking location, use the dpad right or left. To add delay, use the dpad up to increase delay, and dpad down to decrease delay", ""); if(leftPressed && !leftWasPressed) { this.park = parkLeft; - this.telemetry.addData("Park set to", "Left"); } else if(rightPressed && !rightWasPressed) { this.park = parkRight; - this.telemetry.addData("Park set to", "Right"); } else if(upPressed && !upWasPressed) { this.delay += 1000; - this.telemetry.addData("Delay increased by", "1000"); - this.sleep(200); } else if(downPressed && !downWasPressed) { this.delay -= 1000; - this.telemetry.addData("Delay decreased by", "1000"); - this.sleep(200); } + + this.leftWasPressed = leftPressed; + this.rightWasPressed = rightPressed; + this.upWasPressed = upPressed; + this.downWasPressed = downPressed; + this.telemetry.addData("Delay", this.delay); this.telemetry.addData("Park set to", this.park); + this.sleep(20); } if (isStopRequested()) { // remove later if nessacary as recent addition might be interefering return; @@ -109,7 +111,7 @@ public abstract class AutoBase extends LinearOpMode { break; } this.sleep(delay); - moveToEasel(); + moveBackstage(); prepareToScore(); scorePreloadedPixel(); park(); @@ -145,6 +147,35 @@ public abstract class AutoBase extends LinearOpMode { } protected void prepareToScore() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + + this.robot.getGantry().armOut(); + this.robot.getGantry().setSlideTarget(DEPOSIT_HEIGHT); + + if (this.alliance == CenterStageCommon.Alliance.Blue) { + builder.lineToLinearHeading(new Pose2d(35, 36, 0)); + } else if (this.alliance == CenterStageCommon.Alliance.Red) { + builder.lineToLinearHeading(new Pose2d(35, -35, 0)); + } + + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + + Pose2d inferredPos = null; + while(this.robot.getDrive().isBusy()) { + this.robot.update(); + + inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); + if (inferredPos != null) { + this.robot.getDrive().breakFollowing(); + break; + } + } + sleep(200); + inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); + if (inferredPos == null) { + inferredPos = this.robot.getDrive().getPoseEstimate(); + } + // At this point we know that Y = 38 // For 2 -> Ydelta = 0 // For 3 -> 3 5/8 @@ -163,20 +194,19 @@ public abstract class AutoBase extends LinearOpMode { break; } - Pose2d inferredPos = this.robot.getCamera().estimatePoseFromAprilTag(); -// inferredPos = inferredPos != null ? inferredPos : robot.getDrive().getPoseEstimate(); // new code (maybe get rid of) - this.robot.getDrive().setPoseEstimate(inferredPos); + builder = this.robot.getDrive().trajectorySequenceBuilder(inferredPos); Pose2d target = new Pose2d( 60 - SCORING_DISTANCE_FROM_APRIL_TAG - CAMERA_FORWARD_OFFSET_IN, // 60 is the X position of the april tag - inferredPos.getY() + delta, + 36 + delta, 0); - - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(target); - this.robot.getDrive().followTrajectorySequence(builder.build()); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while(this.robot.getDrive().isBusy()) { + this.robot.update(); + } } - protected void moveToEasel() { + protected void moveBackstage() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); if (!this.isBackstage()) { @@ -187,21 +217,14 @@ public abstract class AutoBase extends LinearOpMode { builder.lineToSplineHeading(new Pose2d(-40, -60, Math.PI)); builder.lineToLinearHeading(new Pose2d(12, -60, Math.PI)); } + this.robot.getDrive().followTrajectorySequence(builder.build()); } - - if (this.alliance == CenterStageCommon.Alliance.Blue) { - builder.lineToLinearHeading(new Pose2d(35, 36, 0)); - } else if (this.alliance == CenterStageCommon.Alliance.Red) { - builder.lineToLinearHeading(new Pose2d(35, -35, 0)); - } - - this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void determinePropLocation() { this.robot.getClaw().setArmPositionAsync(PICKUP_ARM_MIN); // changed from setArmPositionAsync - while (!this.robot.getClaw().isArmAtPosition()) { + while (!this.robot.getClaw().isArmAtPosition() && !this.robot.getCamera().getProp().isValid()) { this.robot.update(); sleep(20); } 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 63e57e7..2dce305 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 @@ -74,6 +74,7 @@ public class Claw implements Updatable { this.clawController.setP(CLAW_KP); this.armController.setSetPoint(this.armControllerTarget); + this.armController.setTolerance(0.008); this.armController.setP(CLAW_ARM_KP); if (!this.clawController.atSetPoint()) { 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 28146f5..2b6cf8a 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 @@ -41,6 +41,7 @@ public class Gantry { this.armServo = hardwareMap.get(Servo.class, GANTRY_ARM_NAME); this.screwServo = hardwareMap.get(CRServo.class, GANTRY_SCREW_NAME); this.armServo.setPosition(GANTRY_ARM_MIN); + this.armControllerTarget = GANTRY_ARM_MIN; this.liftLeft = hardwareMap.get(DcMotor.class, LEFT_SLIDE_MOTOR_NAME); this.liftLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -127,7 +128,7 @@ public class Gantry { public void update() { this.armController.setSetPoint(this.armControllerTarget); - this.armController.setTolerance(0.001); + this.armController.setTolerance(0.0001); this.armController.setP(GANTRY_ARM_KP); this.xController.setSetPoint(this.xControllerTarget); 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 acc967b..f7b99ba 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 CLAW_ARM_KP = 0.15; // Gantry - public static double GANTRY_ARM_MIN = 0.43; // Changed 0.42 --> 0.45 pickup position + public static double GANTRY_ARM_MIN = 0.435; // Changed 0.42 --> 0.45 pickup position public static double GANTRY_ARM_MAX = 0.96; public static int GANTRY_LIFT_DELTA = 50; public static double GANTRY_ARM_KP = 0.1; 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 95419ac..5c54715 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 @@ -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 = 45; - public static double MAX_ACCEL = 45; + public static double MAX_VEL = 55; + public static double MAX_ACCEL = 55; public static double MAX_ANG_VEL = Math.toRadians(720); public static double MAX_ANG_ACCEL = Math.toRadians(720); 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 9f0c394..c913718 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 @@ -94,7 +94,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER); follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID, - new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5); + new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.1); LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);