From 8e1ca29b8f5e8943115b6d1f58c3e4028ea52c1f Mon Sep 17 00:00:00 2001 From: sihan Date: Mon, 18 Mar 2024 19:38:32 -0500 Subject: [PATCH] Goog --- .../roadrunner/drive/MecanumDrive.java | 2 +- .../ftc/teamcode/opmodes/AutoRed.java | 60 ++++++++++++------- .../ftc/teamcode/util/Configurables.java | 8 +-- 3 files changed, 42 insertions(+), 28 deletions(-) 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 b1dc65f..71b6776 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 @@ -60,7 +60,7 @@ import java.util.List; */ @Config public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(4, 0, 2); public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1); public static double LATERAL_MULTIPLIER = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index ac4459c..86092b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java @@ -1,11 +1,15 @@ package org.firstinspires.ftc.teamcode.opmodes; +import static org.firstinspires.ftc.teamcode.util.Configurables.AuToDeV.TEMP1; +import static org.firstinspires.ftc.teamcode.util.Configurables.AuToDeV.TEMP2; +import static org.firstinspires.ftc.teamcode.util.Configurables.AuToDeV.TEMP3; +import static org.firstinspires.ftc.teamcode.util.Configurables.AuToDeV.TEMP4; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.tearabite.ielib.common.Alliance; import org.firstinspires.ftc.teamcode.hardware.Robot; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; @@ -21,12 +25,12 @@ public class AutoRed extends LinearOpMode { //Preloads final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270)); final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360)); - final static Pose2d CENTER_PRELOAD = new Pose2d(33, -24, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -35, Math.toRadians(270)); + final static Pose2d CENTER_PRELOAD = new Pose2d(24, -28, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(36, -30, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(75.8, -26.5, Math.toRadians(358)); - final static Pose2d CENTER_BOARD = new Pose2d(80, -30.3, Math.toRadians(358)); - final static Pose2d RIGHT_BOARD = new Pose2d(75.8, -40, Math.toRadians(358)); + final static Pose2d LEFT_BOARD = new Pose2d(50, -26.5, Math.toRadians(358)); + final static Pose2d CENTER_BOARD = new Pose2d(50, -36.3, Math.toRadians(358)); + final static Pose2d RIGHT_BOARD = new Pose2d(50, -40, Math.toRadians(358)); //Park final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360)); @@ -42,7 +46,7 @@ public class AutoRed extends LinearOpMode { builder.lineToLinearHeading(LEFT_PRELOAD_TWO); break; case "CENTER": - builder.lineToLinearHeading(CENTER_PRELOAD); + builder.lineToLinearHeading(TEMP1); break; case "RIGHT": builder.lineToLinearHeading(RIGHT_PRELOAD); @@ -58,7 +62,7 @@ public class AutoRed extends LinearOpMode { builder.lineToLinearHeading(LEFT_BOARD); break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); + builder.lineToLinearHeading(TEMP2); break; case "RIGHT": builder.lineToLinearHeading(RIGHT_BOARD); @@ -77,8 +81,8 @@ public class AutoRed extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch(parkLocation) { case "LEFT": - builder.lineToLinearHeading(PARKLEFT); - builder.lineToLinearHeading(PARKLEFT2); + builder.lineToLinearHeading(TEMP3); + builder.lineToLinearHeading(TEMP4); break; case "RIGHT": builder.lineToLinearHeading(PARK); @@ -94,39 +98,49 @@ public class AutoRed extends LinearOpMode { } } - protected void parkLocation(){ + protected void parkLocation() { if (gamepad2.dpad_left) { - parkLocation="LEFT"; + parkLocation = "LEFT"; } else if (gamepad2.dpad_right) { parkLocation = "RIGHT"; - }} + } + } + + protected void startLocation() { + if (gamepad2.x) { + randomization = "LEFT"; + } else if (gamepad2.y) { + randomization = "CENTER"; + } else if (gamepad2.b) { + randomization = "RIGHT"; + } + } @Override public void runOpMode() throws InterruptedException { this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); this.robot = new Robot().init(hardwareMap); - this.robot.getCamera().setAlliance(Alliance.Blue); +// this.robot.getCamera().setAlliance(Alliance.Blue); // this.robot.getCamera().initTargetingCamera(); - this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); + this.initialPosition = new Pose2d(24, -60, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); // Do super fancy chinese shit while (!this.isStarted()) { - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); - randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); +// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); parkLocation(); -// randomization = "CENTER"; + startLocation(); + this.telemetry.addData("Starting Position", randomization); this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } - scorePreloadOne(); + scorePreloadOne(); boardScore(); - this.robot.refreshPoseEstimateFromAprilTag(); sleep(250); - this.robot.getClaw().open(); - sleep(250); - park(); + this.robot.getClaw().open(); + sleep(250); +// park(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java index b32fc58..ab92715 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java @@ -39,10 +39,10 @@ public class Configurables { @Config public static class AuToDeV { //Things - public static double X1 = 0, Y1 = 0, R1 = 0; - public static double X2 = 0, Y2 = 0, R2 = 0; - public static double X3 = 0, Y3 = 0, R3 = 0; - public static double X4 = 0, Y4 = 0, R4 = 0; + public static double X1 = 24, Y1 = -28, R1 = 270; + public static double X2 = 50, Y2 = -36, R2 = 360; + public static double X3 = 60, Y3 = -6, R3 = 360; + public static double X4 = 60, Y4 = -6, R4 = 360; public static double X5 = 0, Y5 = 0, R5 = 0; //Pose2d