From cc8cf18ff0c7bb2896d0533ac307032a8ab6f712 Mon Sep 17 00:00:00 2001 From: Scott Barnes Date: Sat, 18 Nov 2023 12:40:31 -0600 Subject: [PATCH] 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.