From 1814413011ab99d18b38cbb796019c9125423838 Mon Sep 17 00:00:00 2001 From: sihan Date: Thu, 30 Nov 2023 21:04:29 -0600 Subject: [PATCH] Think all 4 autos work, thursday night before LM2 --- .../roadrunner/drive/MecanumDrive.java | 4 +- .../ftc/teamcode/opmodes/AutoBlueFar.java | 203 +++++++++++++++++ .../opmodes/{Auto.java => AutoRed.java} | 84 ++++++-- .../ftc/teamcode/opmodes/AutoRedFar.java | 204 ++++++++++++++++++ .../ftc/teamcode/util/Configurables.java | 10 +- 5 files changed, 480 insertions(+), 25 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/{Auto.java => AutoRed.java} (65%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java 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 e1847cd..a4fdc26 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,8 +60,8 @@ import java.util.List; */ @Config public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(25, 0, 2.5); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(15, 0, 1); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(20, 0, 1.5); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2); public static double LATERAL_MULTIPLIER = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java new file mode 100644 index 0000000..113e487 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java @@ -0,0 +1,203 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +@Autonomous(name = "autoBlueFar") +public class AutoBlueFar extends LinearOpMode { + protected Pose2d initialPosition; + + protected Trajectory preloadOne; + protected Trajectory scoreOne; + protected Trajectory boardOne; + protected Trajectory backOffOne; + protected Trajectory goGate1; + protected Trajectory passGate; + + protected Trajectory preloadTwo; + protected Trajectory scoreTwo; + protected Trajectory backOffTwo; + protected Trajectory tokyoDrift; + protected Trajectory tokyoDrift2; + protected Trajectory tokyoDrift3; + + + protected Trajectory preloadThree; + protected Trajectory boardThree; + protected Trajectory scoreThree; + protected Trajectory backOffThree; + protected Trajectory goGate3; + protected Trajectory goGate3Again; + + + protected Trajectory park1; + protected Trajectory park2; + + private Robot robot; + private String randomization; + private int random; + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + + //Trajectories + this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + //Randomization One + this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270))) + .build(); + + this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end()) + .lineToLinearHeading(new Pose2d(35, -20, Math.toRadians(150))) + .build(); + + this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) + .lineToLinearHeading(new Pose2d(31, -10, Math.toRadians(180))) + .build(); + + this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end()) + .lineToLinearHeading(new Pose2d(-40, -12, Math.toRadians(180))) + .build(); + + this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(-49.25, -28, Math.toRadians(180))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) + .lineToLinearHeading(new Pose2d(-40, -25, Math.toRadians(180))) + .build(); + + //Randomization Two + this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(36, -28, Math.toRadians(290))) + .build(); + + this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) + .lineToLinearHeading(new Pose2d(50, -38, Math.toRadians(270))) + .build(); + + this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end()) + .lineToLinearHeading(new Pose2d(50, -9, Math.toRadians(180))) + .build(); + + this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end()) + .lineToLinearHeading(new Pose2d(35, -9, Math.toRadians(180))) + .build(); + + this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(-50, -33, Math.toRadians(180))) + .addTemporalMarker(.02, robot.getArm()::armAccurateScore) + .addTemporalMarker(.02, robot.getWrist()::wristScore) + .build(); + + this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) + .lineToLinearHeading(new Pose2d(-40, -33, Math.toRadians(180))) + .build(); + + //Randomization Three + this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(43, -37.5, Math.toRadians(270))) + .build(); + + this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) + .lineToLinearHeading(new Pose2d(29, -32, Math.toRadians(335))) + .build(); + + this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) + .lineToLinearHeading(new Pose2d(40, -32, Math.toRadians(335))) + .build(); + + this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end()) + .lineToLinearHeading(new Pose2d(35, -10, Math.toRadians(180))) + .build(); + + this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(-50.5, -39, Math.toRadians(180))) + .addTemporalMarker(.2, robot.getArm()::armAccurateScore) + .addTemporalMarker(.2, robot.getWrist()::wristScore) + .build(); + + this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end()) + .lineToLinearHeading(new Pose2d(-40, -40, Math.toRadians(180))) + .build(); + + + //Park + this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end()) + .lineToLinearHeading(new Pose2d(-40, -10, Math.toRadians(180))) + .addTemporalMarker(.3, robot.getArm()::armRest) + .addTemporalMarker(.3, robot.getWrist()::wristPickup) + .build(); + this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) + .lineToLinearHeading(new Pose2d(-60, -10, Math.toRadians(170))) + .build(); + + // Do super fancy chinese shit + while (!this.isStarted()) { + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); + randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + this.telemetry.update(); + } + + sleep(5000); + + switch (randomization) { + case "RIGHT": + this.robot.getDrive().followTrajectory(preloadOne); + this.robot.getDrive().followTrajectory(scoreOne); + this.robot.getDrive().followTrajectory(goGate1); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(boardOne); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffOne); + sleep(300); + break; + case "CENTER": + this.robot.getDrive().followTrajectory(preloadTwo); + this.robot.getDrive().followTrajectory(tokyoDrift); + this.robot.getDrive().followTrajectory(tokyoDrift2); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(scoreTwo); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffTwo); + sleep(300); + break; + case "LEFT": + this.robot.getDrive().followTrajectory(preloadThree); + this.robot.getDrive().followTrajectory(scoreThree); + this.robot.getDrive().followTrajectory(goGate3); + this.robot.getDrive().followTrajectory(goGate3Again); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(boardThree); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffThree); + sleep(300); + break; + } + //Cycle + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); + + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java similarity index 65% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index c0ca2a7..257248c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/Auto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java @@ -9,8 +9,8 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.hardware.Robot; -@Autonomous(name = "ThisIsTheLongestFlippingOpModeNameEverLolIamSeahorse!") -public class Auto extends LinearOpMode { +@Autonomous(name = "autoRed") +public class AutoRed extends LinearOpMode { protected Pose2d initialPosition; protected Trajectory preloadOne; @@ -33,6 +33,12 @@ public class Auto extends LinearOpMode { protected Trajectory park1; protected Trajectory park2; + protected Trajectory goGate; + protected Trajectory goStack; + protected Trajectory backGate; + protected Trajectory approachBoard; + protected Trajectory scoreStack; + private Robot robot; private String randomization; @@ -58,13 +64,13 @@ public class Auto extends LinearOpMode { .build(); this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(73, -28, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(72, -28, Math.toRadians(360))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) - .lineToLinearHeading(new Pose2d(60, -26, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360))) .build(); @@ -74,13 +80,13 @@ public class Auto extends LinearOpMode { .build(); this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(73, -34.5, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(72, -33.3, Math.toRadians(360))) .addTemporalMarker(.2, robot.getArm()::armAccurateScore) .addTemporalMarker(.2, robot.getWrist()::wristScore) .build(); this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) - .lineToLinearHeading(new Pose2d(60, -34, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360))) .build(); //Randomization Three @@ -95,19 +101,37 @@ public class Auto extends LinearOpMode { .build(); this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) - .lineToLinearHeading(new Pose2d(60, -40, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360))) .build(); //Park this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) - .lineToLinearHeading(new Pose2d(65, -10, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(65, -55, Math.toRadians(360))) .addTemporalMarker(.3, robot.getArm()::armRest) .addTemporalMarker(.3, robot.getWrist()::wristPickup) .build(); this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) - .lineToLinearHeading(new Pose2d(80, -10, Math.toRadians(360))) + .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) .build(); +// //Cycle +// this.goGate = this.robot.getDrive().trajectoryBuilder(park1.end()) +// .lineToLinearHeading(new Pose2d(-37,-7, Math.toRadians(360))) +// .addTemporalMarker(.3, robot.getArm()::armRest) +// .addTemporalMarker(.3, robot.getWrist()::wristPickup) +// .build(); +// this.backGate = this.robot.getDrive().trajectoryBuilder(goGate.end()) +// .lineToLinearHeading(new Pose2d(50,-10, Math.toRadians(360))) +// .build(); +// this.approachBoard = this.robot.getDrive().trajectoryBuilder(backGate.end()) +// .lineToLinearHeading(new Pose2d(68, -28, Math.toRadians(360))) +// .addTemporalMarker(.2, robot.getArm()::armAccurateScore) +// .addTemporalMarker(.2, robot.getWrist()::wristScore) +// .build(); +// this.scoreStack = this.robot.getDrive().trajectoryBuilder(approachBoard.end()) +// .lineToLinearHeading(new Pose2d(72.5, -28, Math.toRadians(360))) +// .build(); + // Do super fancy chinese shit while (!this.isStarted()) { this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); @@ -120,34 +144,58 @@ public class Auto extends LinearOpMode { this.robot.getDrive().followTrajectory(preloadOne); this.robot.getDrive().followTrajectory(scoreOne); this.robot.getDrive().followTrajectory(boardOne); + sleep(500); this.robot.getClaw().open(); sleep(500); this.robot.getDrive().followTrajectory(backOffOne); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); + sleep(300); break; case "CENTER": this.robot.getDrive().followTrajectory(preloadTwo); this.robot.getDrive().followTrajectory(scoreTwo); + sleep(500); this.robot.getClaw().open(); sleep(500); this.robot.getDrive().followTrajectory(backOffTwo); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); + sleep(300); break; case "RIGHT": this.robot.getDrive().followTrajectory(preloadThree); this.robot.getDrive().followTrajectory(scoreThree); + sleep(500); this.robot.getClaw().open(); sleep(500); this.robot.getDrive().followTrajectory(backOffThree); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); + sleep(300); break; } + //Cycle + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); +// this.robot.getDrive().followTrajectory(goGate); +// sleep(120); +// this.robot.getClaw().close(); +// sleep(120); +// this.robot.getDrive().followTrajectory(backGate); +// this.robot.getDrive().followTrajectory(approachBoard); +// sleep(120); +// this.robot.getClaw().open(); +// sleep(120); +// +// this.robot.getDrive().followTrajectory(park1); +// this.robot.getDrive().followTrajectory(goGate); +// sleep(120); +// this.robot.getClaw().close(); +// sleep(120); +// this.robot.getDrive().followTrajectory(backGate); +// this.robot.getDrive().followTrajectory(approachBoard); +// sleep(120); +// this.robot.getClaw().open(); +// sleep(120); +// this.robot.getDrive().followTrajectory(park1); +// this.robot.getDrive().followTrajectory(park2); + + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java new file mode 100644 index 0000000..5df1f2b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java @@ -0,0 +1,204 @@ +package org.firstinspires.ftc.teamcode.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; + +@Autonomous(name = "autoRedFar") +public class AutoRedFar extends LinearOpMode { + protected Pose2d initialPosition; + + protected Trajectory preloadOne; + protected Trajectory scoreOne; + protected Trajectory boardOne; + protected Trajectory backOffOne; + protected Trajectory goGate1; + protected Trajectory passGate; + + protected Trajectory preloadTwo; + protected Trajectory scoreTwo; + protected Trajectory backOffTwo; + protected Trajectory tokyoDrift; + protected Trajectory tokyoDrift2; + protected Trajectory tokyoDrift3; + + protected Trajectory preloadThree; + protected Trajectory boardThree; + protected Trajectory scoreThree; + protected Trajectory backOffThree; + protected Trajectory goGate3; + protected Trajectory goGate3Again; + + + protected Trajectory park1; + protected Trajectory park2; + + private Robot robot; + private String randomization; + private int random; + + @Override + public void runOpMode() throws InterruptedException { + this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + this.robot = new Robot().init(hardwareMap); + this.robot.getCamera().initTargetingCamera(); + + //Trajectories + this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + //niggler + //Randomization One + this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270))) + .build(); + + this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end()) + .lineToLinearHeading(new Pose2d(-35, -25, Math.toRadians(30))) + .build(); + + this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) + .lineToLinearHeading(new Pose2d(-32, -10, Math.toRadians(360))) + .build(); + + this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end()) + .lineToLinearHeading(new Pose2d(40, -11, Math.toRadians(360))) + .addTemporalMarker(2, robot.getArm()::armScore) + .addTemporalMarker(2, robot.getWrist()::wristScore) + .build(); + + this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(50, -28, Math.toRadians(360))) + .addTemporalMarker(.02, robot.getArm()::armAccurateScore) + .addTemporalMarker(.02, robot.getWrist()::wristScore) + .build(); + + this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) + .lineToLinearHeading(new Pose2d(40, -25, Math.toRadians(360))) + .build(); + + + //Randomization Two + this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-36, -28, Math.toRadians(240))) + .build(); + + this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) + .lineToLinearHeading(new Pose2d(-50, -38, Math.toRadians(270))) + .build(); + + this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end()) + .lineToLinearHeading(new Pose2d(-50, -9, Math.toRadians(360))) + .build(); + + this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end()) + .lineToLinearHeading(new Pose2d(-35, -9, Math.toRadians(360))) + .build(); + + this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(50, -34, Math.toRadians(360))) + .addTemporalMarker(.02, robot.getArm()::armAccurateScore) + .addTemporalMarker(.02, robot.getWrist()::wristScore) + .build(); + + this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) + .lineToLinearHeading(new Pose2d(40, -33, Math.toRadians(360))) + .build(); + + //Randomization Three + this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) + .lineToLinearHeading(new Pose2d(-43, -37.5, Math.toRadians(270))) + .build(); + + this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) + .lineToLinearHeading(new Pose2d(-28, -32, Math.toRadians(180))) + .build(); + + this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) + .lineToSplineHeading(new Pose2d(-40, -32, Math.toRadians(180))) + .build(); + + this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end()) + .lineToSplineHeading(new Pose2d(-33, -10, Math.toRadians(360))) + .build(); + + this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end()) + .lineToLinearHeading(new Pose2d(50, -37, Math.toRadians(360))) + .addTemporalMarker(.02, robot.getArm()::armAccurateScore) + .addTemporalMarker(.02, robot.getWrist()::wristScore) + .build(); + + this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end()) + .lineToLinearHeading(new Pose2d(40, -47, Math.toRadians(360))) + .build(); + + + //Park + this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end()) + .lineToLinearHeading(new Pose2d(40, -10, Math.toRadians(360))) + .addTemporalMarker(.3, robot.getArm()::armRest) + .addTemporalMarker(.3, robot.getWrist()::wristPickup) + .build(); + this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) + .lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(360))) + .build(); + + // Do super fancy chinese shit + while (!this.isStarted()) { + this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); + randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); + this.telemetry.update(); + } + sleep(5000); + switch (randomization) { + case "LEFT": + this.robot.getDrive().followTrajectory(preloadOne); + this.robot.getDrive().followTrajectory(scoreOne); + this.robot.getDrive().followTrajectory(goGate1); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(boardOne); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffOne); + sleep(300); + break; + case "CENTER": + this.robot.getDrive().followTrajectory(preloadTwo); + this.robot.getDrive().followTrajectory(tokyoDrift); + this.robot.getDrive().followTrajectory(tokyoDrift2); + this.robot.getDrive().followTrajectory(tokyoDrift3); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(scoreTwo); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffTwo); + sleep(300); + break; + case "RIGHT": + this.robot.getDrive().followTrajectory(preloadThree); + this.robot.getDrive().followTrajectory(scoreThree); + this.robot.getDrive().followTrajectory(goGate3); + this.robot.getDrive().followTrajectory(goGate3Again); + this.robot.getDrive().followTrajectory(passGate); + this.robot.getDrive().followTrajectory(boardThree); + sleep(500); + this.robot.getClaw().open(); + sleep(500); + this.robot.getDrive().followTrajectory(backOffThree); + sleep(300); + break; + } + //Cycle + this.robot.getDrive().followTrajectory(park1); + this.robot.getDrive().followTrajectory(park2); + + + } + +} 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 db43889..8bde221 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 @@ -18,10 +18,10 @@ public class Configurables { public static double LOCKSPEED = .25; public static double UNLOCK = .6; public static double LOCK = 0.4; - public static double ARMREST = .88; - public static double ARMSCORE = 0.15; - public static double ARMACCSCORE = 0.02; - public static double PICKUP = .935; + public static double ARMREST = .91; + public static double ARMSCORE = 0.14 ; + public static double ARMACCSCORE = 0.04; + public static double PICKUP = 0.97; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96; public static double OPEN = 0.53; @@ -34,6 +34,6 @@ public class Configurables { public static double SPEED = 1; public static double SLOWMODE_SPEED = 0.5; public static double TURN = 1; - public static double SLOWMODE_TURN = 0.5; + public static double SLOWMODE_TURN = 0.3; } \ No newline at end of file