From da3701612be7b776bd43b9a3aae320eda624e742 Mon Sep 17 00:00:00 2001 From: sihan Date: Wed, 20 Mar 2024 17:05:28 -0500 Subject: [PATCH] wed before states, 2 backstage mostly working 2+4s --- .../ftc/teamcode/hardware/Robot.java | 2 +- .../roadrunner/drive/MecanumDrive.java | 4 +- .../ftc/teamcode/opmodes/AutoBase.java | 44 --- .../teamcode/opmodes/AutoBlueTwoPlusTwo.java | 284 +++++++++------ .../ftc/teamcode/opmodes/AutoRed.java | 24 +- .../opmodes/AutoRedFarTwoPlusTwo.java | 323 ------------------ ...woPlusTwo.java => AutoRedTwoPlusFour.java} | 30 +- .../opmodes/AutoRedTwoPlusFourTest.java | 315 +++++++++++++++++ .../ftc/teamcode/opmodes/AutoTest.java | 44 --- 9 files changed, 529 insertions(+), 541 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/{AutoRedTwoPlusTwo.java => AutoRedTwoPlusFour.java} (94%) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java index 48f7a4f..d72c1a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java @@ -461,7 +461,7 @@ public class Robot { public static class Claw { //Values public static double OPEN = 0.65; - public static double BIGOPEN = .48; + public static double BIGOPEN = .42; public static double CLOSE = 0.75; public static double CLAW_MIN = 0; public static double CLAW_MAX = 1; 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 d75f764..86cc800 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 @@ -336,8 +336,8 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive } public void setInput(Gamepad gamepad1, Gamepad gamepad2) { - double speedScale = gamepad1.y ? SLOWMODE_SPEED : SPEED; - double turnScale = gamepad1.y ? SLOWMODE_TURN : TURN; + double speedScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_SPEED : SPEED; + double turnScale = gamepad1.y || gamepad1.right_bumper ? SLOWMODE_TURN : TURN; this.setWeightedDrivePower( new Pose2d( diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java deleted file mode 100644 index dfc01c2..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBase.java +++ /dev/null @@ -1,44 +0,0 @@ -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.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.opmodes.AutoRedFarTwoPlusTwo.autoState; - -public abstract class AutoBase extends LinearOpMode { - protected Pose2d initialPosition; - Robot robot; - String randomization; - String parkLocation; - double runtime; - - autoState state = autoState.PURPLE; - int delay; - - public abstract void followTrajectories(); - - @Override - public void runOpMode() { - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - this.robot = new Robot().init(hardwareMap); - this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); - this.robot.getDrive().setPoseEstimate(initialPosition); - - while (!this.isStarted()) { - parkLocation = "RIGHT"; - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPosition()); - randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); - this.telemetry.addData("Park Position", parkLocation); - this.telemetry.addData("Delay", delay); - this.telemetry.update(); - } - - while (state != autoState.STOP) { - followTrajectories(); - robot.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java index bd856b8..0a96fbe 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java @@ -1,8 +1,11 @@ package org.firstinspires.ftc.teamcode.opmodes; +import static org.firstinspires.ftc.teamcode.hardware.Robot.Slides.SLIDELAYERONE; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -15,36 +18,42 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra public class AutoBlueTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; - private String randomization; + private String randomization = "CENTER"; + private String parkLocation = "LEFT"; //Pose2ds //Preloads - final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(210)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-30.5, -31, Math.toRadians(210)); - final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26.8, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-32, -30, Math.toRadians(180)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -26, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270)); //Board Scores - final static Pose2d RIGHT_BOARD = new Pose2d(-75.5, -26.5, Math.toRadians(182)); - final static Pose2d CENTER_BOARD = new Pose2d(-77, -33.5, Math.toRadians(185)); - final static Pose2d LEFT_BOARD = new Pose2d(-76, -41, Math.toRadians(185)); - //Stack Cycle - final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180)); - final static Pose2d TO_STACK = new Pose2d(35, -6.5, Math.toRadians(180)); - final static Pose2d TO_STACK_SLOW = new Pose2d(40, -7.5, Math.toRadians(180)); - final static Pose2d TO_STACK_SLOW2 = new Pose2d(40, -8.5, Math.toRadians(183)); + final static Pose2d RIGHT_BOARD = new Pose2d(-76.5, -24, Math.toRadians(180)); + final static Pose2d CENTER_BOARD = new Pose2d(-78, -35, Math.toRadians(184)); + final static Pose2d LEFT_BOARD = new Pose2d(-78, -41, Math.toRadians(182)); + //Parka + final static Pose2d PARK = new Pose2d(-60, -58, Math.toRadians(180)); + final static Pose2d PARK2 = new Pose2d(-80, -60, Math.toRadians(180)); + final static Pose2d PARKLEFT = new Pose2d(-70, -15, Math.toRadians(180)); + final static Pose2d PARKLEFT2 = new Pose2d(-75, -12, Math.toRadians(180)); + //Cycles + final static Vector2d LEAVE_BOARD = new Vector2d(-50, -10); + final static Vector2d TO_STACK = new Vector2d(38, -10); + final static Vector2d TO_STACK_SLOW = new Vector2d(47, -9.5); + final static Pose2d TO_STACK_SLOW2 = new Pose2d(38.5, -8, Math.toRadians(180)); final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180)); - final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -28, Math.toRadians(180)); - final static Pose2d SCORE_STACK = new Pose2d(-73.5, -29, Math.toRadians(180)); - //Park - final static Pose2d BACK_OFF = new Pose2d(-68, -55,Math.toRadians(180)); - final static Pose2d PARK = new Pose2d(-80, -64, Math.toRadians(180)); + final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -31, Math.toRadians(180)); + final static Vector2d SCORE_STACK = new Vector2d(-72, -30); + final static Vector2d SCORE_STACK_SLOW = new Vector2d(-73, -30); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); - builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + builder.setReversed(true); + builder.splineToSplineHeading(RIGHT_PRELOAD_TWO, Math.toRadians(360), + MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); + builder.setReversed(false); break; case "CENTER": builder.lineToLinearHeading(CENTER_PRELOAD); @@ -53,148 +62,223 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(LEFT_PRELOAD); break; } - builder.addTemporalMarker(.5, robot.getArm()::armScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void boardScore() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20));; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD); break; case "CENTER": builder.lineToLinearHeading(CENTER_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); break; - case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD, + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(30)); break; } - builder.addTemporalMarker(.2, robot.getArm()::armScore); - builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); builder.addTemporalMarker(.2, robot.getWrist()::wristScore); - this.robot.getDrive().followTrajectorySequence(builder.build()); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } } protected void toStack() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(LEAVE_BOARD); - builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.setReversed(true); + builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(360)); + builder.lineToConstantHeading(TO_STACK, + MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.setReversed(false); + switch (randomization) { + case "RIGHT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 2)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + case "CENTER": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + case "LEFT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + } + builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); + builder.addTemporalMarker(.3, robot.getClaw()::close); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); - builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - builder.addTemporalMarker(1.5,robot.getClaw()::openStack); - builder.addTemporalMarker(1.5, robot.getArm()::pickup); - builder.lineToLinearHeading(TO_STACK); - builder.lineToLinearHeading(TO_STACK_SLOW, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - this.robot.getDrive().followTrajectorySequence(builder.build()); + builder.addTemporalMarker(2, robot.getClaw()::openStack); + builder.addTemporalMarker(.2, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } } - protected void toStackNoDrift() { + protected void toStackLower() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(LEAVE_BOARD); - builder.addTemporalMarker(.3, robot.getArm()::armRest); + builder.setReversed(true); + builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(360)); + builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)), + MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.setReversed(false); + switch (randomization) { + case "RIGHT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + case "CENTER": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + case "LEFT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(15, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(15)); + break; + } + builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow); + builder.addTemporalMarker(.3, robot.getClaw()::close); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); - builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - builder.addTemporalMarker(1.5,robot.getClaw()::openStack); - builder.addTemporalMarker(1.5, robot.getArm()::pickup); - builder.lineToLinearHeading(TO_STACK); - builder.lineToLinearHeading(TO_STACK_SLOW2, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - this.robot.getDrive().followTrajectorySequence(builder.build()); + builder.addTemporalMarker(.7, robot.getClaw()::openStack); + builder.addTemporalMarker(.2, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } } - protected void scoreStack() { + protected void toBoard() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(BACK_THROUGH_GATE); - builder.lineToLinearHeading(APPROACHING_BOARD); - builder.lineToLinearHeading(SCORE_STACK); - builder.addTemporalMarker(2.5, robot.getArm()::armSecondaryScore); + builder.lineToConstantHeading(LEAVE_BOARD); + builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(180), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy); + builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack); builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); - builder.addTemporalMarker(2.5, robot.getSlides()::slideScoreStack); - this.robot.getDrive().followTrajectorySequence(builder.build()); + builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void toStage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(PARKLEFT2.vec()); + builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } } protected void scoreTest() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(-77.5, -31, Math.toRadians(183)), - MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - builder.addTemporalMarker(.2,this::clawSlowOpen); + builder.lineToConstantHeading(SCORE_STACK_SLOW, + MecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(30)); + builder.addTemporalMarker(.3, this::clawSlowOpen); this.robot.getDrive().followTrajectorySequence(builder.build()); } protected void clawSlowOpen() { - double currentPos = .8; - double targetPos = .73; + double currentPos = .61; + double targetPos = .58; + int slidesPos = SLIDELAYERONE; + int slideDelta = 2; double delta = (targetPos - currentPos) / 100; for (int i = 0; i < 100; i++) { this.robot.getClaw().setPos(currentPos + (delta * i)); - sleep(30); + this.robot.getSlides().slideTo(slidesPos + (slideDelta * i), 1); + sleep(7); } } protected void park() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(BACK_OFF); - builder.lineToLinearHeading(PARK); - builder.addTemporalMarker(.1, robot.getArm()::armRest); - builder.addTemporalMarker(.1, robot.getClaw()::close); - builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); - builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - this.robot.getDrive().followTrajectorySequence(builder.build()); + builder.lineToLinearHeading(PARKLEFT); + builder.addTemporalMarker(.01, robot.getArm()::armRest); + builder.addTemporalMarker(.01, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.01, robot.getSlides()::slideDown); + builder.addTemporalMarker(.01, robot.getClaw()::open); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } } +// protected void parkLocation() { +// if (gamepad2.dpad_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().initTargetingCamera(); this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); // Do super fancy chinese shit while (!this.isStarted()) { - this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue()); - randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); +// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); +// parkLocation(); + startLocation(); + this.telemetry.addData("Starting Position", randomization); + this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } - + robot.update(); + this.robot.getClaw().close(); scorePreloadOne(); boardScore(); - - sleep(100); +// sleep(250); this.robot.getClaw().open(); - - switch (randomization) { - case "RIGHT": - toStackNoDrift(); - break; - case "CENTER": - toStack(); - break; - case "LEFT": - toStack(); - break; - } - - sleep(500); +// sleep(250); + toStack(); this.robot.getClaw().close(); - sleep(250); - this.robot.getArm().armRest(); - scoreStack(); - this.robot.getClaw().setPos(.83); - scoreTest(); + sleep(300); + toBoard(); + clawSlowOpen(); +// sleep(100); + toStackLower(); + this.robot.getClaw().close(); + sleep(300); + toBoard(); + clawSlowOpen(); park(); +// park(); } } 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 1d44706..1d88b78 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 @@ -24,11 +24,10 @@ public class AutoRed extends LinearOpMode { final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270)); final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(358)); - final static Pose2d CENTER_BOARD = new Pose2d(80, -32, Math.toRadians(358)); - final static Pose2d RIGHT_BOARD = new Pose2d(78, -39, Math.toRadians(358)); - - //Park + final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360)); + final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360)); + final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360)); + //Parka final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360)); final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360)); final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360)); @@ -41,7 +40,7 @@ public class AutoRed extends LinearOpMode { builder.setReversed(true); builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180), MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(70)); + MecanumDrive.getAccelerationConstraint(50)); builder.setReversed(false); break; case "CENTER": @@ -61,16 +60,17 @@ public class AutoRed extends LinearOpMode { builder.lineToLinearHeading(LEFT_BOARD); break; case "CENTER": - builder.lineToLinearHeading(CENTER_BOARD); + builder.lineToLinearHeading(CENTER_BOARD, + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); break; case "RIGHT": builder.lineToLinearHeading(RIGHT_BOARD, MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(50)); + MecanumDrive.getAccelerationConstraint(30)); break; } - builder.addTemporalMarker(.2, robot.getArm()::armScore); -// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); builder.addTemporalMarker(.2, robot.getWrist()::wristScore); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); while (this.robot.getDrive().isBusy()) { @@ -122,14 +122,11 @@ public class AutoRed extends LinearOpMode { this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); this.robot = new Robot().init(hardwareMap); -// this.robot.getCamera().setAlliance(Alliance.Blue); -// this.robot.getCamera().initTargetingCamera(); this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); this.robot.getDrive().setPoseEstimate(initialPosition); // Do super fancy chinese shit while (!this.isStarted()) { -// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); parkLocation(); startLocation(); this.telemetry.addData("Starting Position", randomization); @@ -140,7 +137,6 @@ public class AutoRed extends LinearOpMode { this.robot.getClaw().close(); scorePreloadOne(); boardScore(); -// sleep(250); this.robot.getClaw().open(); sleep(250); park(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java deleted file mode 100644 index c0826ba..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFarTwoPlusTwo.java +++ /dev/null @@ -1,323 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmodes; - -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - -import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; -import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; -import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; - -@Autonomous(name = "AutoRedFar2+2",preselectTeleOp = "Main TeleOp") -public class AutoRedFarTwoPlusTwo extends AutoBase { - //Pose2ds - //Preloads - final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -33.5, Math.toRadians(230)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(200)); - final static Pose2d RIGHT_PRELOAD_GETOUT = new Pose2d(-43, -42, Math.toRadians(0)); - final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-45, -27, Math.toRadians(270)); - //Ready Truss - final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); - final static Pose2d READY_TRUSS = new Pose2d(-43, -56.5, Math.toRadians(0)); - final static Pose2d TO_BOARD = new Pose2d(26, -56.5, Math.toRadians(0)); - final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); - final static Pose2d SCORE_BOARD_LEFT = new Pose2d(55, -27, Math.toRadians(5)); - final static Pose2d SCORE_BOARD_MID = new Pose2d(55.5, -32.2, Math.toRadians(5)); - final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(55, -39, Math.toRadians(5)); - final static Pose2d GET_STACK = new Pose2d(-47, -30, Math.toRadians(0)); - final static Pose2d PICKUP_STACK = new Pose2d(-61.5, -30.5, Math.toRadians(0)); - final static Pose2d PICKUP_STACK_MID = new Pose2d(-61.5, -32.5, Math.toRadians(0)); - final static Pose2d READY_SCORESTACK = new Pose2d(50, -41, Math.toRadians(0)); - final static Pose2d PICKUP_STACK_LEFT = new Pose2d(-60.5, -33, Math.toRadians(0)); - final static Pose2d PARK = new Pose2d(45, -60, Math.toRadians(0)); - final static Pose2d PARK2 = new Pose2d(60, -63, Math.toRadians(0)); - final static Pose2d PARKLEFT = new Pose2d(45, -12, Math.toRadians(0)); - final static Pose2d PARKLEFT2 = new Pose2d(60, -12, Math.toRadians(0)); - - protected enum autoState{ - STOP, - PURPLE, - YELLOW, - FLIPYELLOW, - SCOREYELLOW, - FIRSTCYCLE, - RETRACT, - SCORECYCLEONE, - } - - protected void scorePreloadOne() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD); - break; - case "CENTER": - builder.lineToLinearHeading(CENTER_PRELOAD); - break; - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); - builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); - break; - } - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void goBackToWhereYouCameFrom() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(LEAVE, - MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(90)); - builder.lineToLinearHeading(READY_TRUSS, - MecanumDrive.getVelocityConstraint(60, Math.toRadians(60), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(90)); - builder.lineToLinearHeading(TO_BOARD, - MecanumDrive.getVelocityConstraint(90, Math.toRadians(90), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(90)); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void scoreBoard(){ - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(READY_SCORE); - builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); - builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); - builder.addTemporalMarker(.2, robot.getWrist()::wristScore); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void score(){ - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - - switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(SCORE_BOARD_LEFT, - MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(40)); - break; - case "CENTER": - builder.lineToLinearHeading(SCORE_BOARD_MID, - MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - case "RIGHT": - builder.lineToLinearHeading(SCORE_BOARD_RIGHT, - MecanumDrive.getVelocityConstraint(40, 40, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(40)); - break; - } - builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); - builder.addTemporalMarker(.2,robot.getSlides()::slideAutoStacks); - builder.addTemporalMarker(.2, robot.getWrist()::wristScore); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void backTruss(){ - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(TO_BOARD); - builder.lineToLinearHeading(READY_TRUSS, - MecanumDrive.getVelocityConstraint(80, Math.toRadians(80), DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(80)); - builder.lineToLinearHeading(GET_STACK); - switch (randomization){ - case "MID": - builder.lineToLinearHeading(PICKUP_STACK_MID, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - case "RIGHT": - builder.lineToLinearHeading(PICKUP_STACK, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - default: - builder.lineToLinearHeading(PICKUP_STACK_LEFT, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - } - - builder.addTemporalMarker(.3, robot.getArm()::armRest); - builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); - builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - builder.addTemporalMarker(1.5,robot.getClaw()::openStack); - builder.addTemporalMarker(1.5, robot.getArm()::armPickupStack); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void goBackstage() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(READY_TRUSS); - builder.lineToLinearHeading(TO_BOARD); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void scoreBoardStack(){ - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(READY_SCORESTACK); - builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); - builder.addTemporalMarker(.2, robot.getSlides()::slideAutoStacks); - builder.addTemporalMarker(.2, robot.getWrist()::wristScore); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void scoreTest() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(new Pose2d(55, -41, Math.toRadians(0)), - MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - builder.addTemporalMarker(.3,this::clawSlowOpen); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void clawSlowOpen() { - double currentPos = .81; - double targetPos = .73; - double delta = (targetPos - currentPos) / 100; - for (int i = 0; i < 100; i++) { - this.robot.getClaw().setPos(currentPos + (delta * i)); - sleep(30); - } - } - - - protected void park(){ - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - switch(parkLocation) { - case "LEFT": - builder.lineToLinearHeading(PARKLEFT); - builder.lineToLinearHeading(PARKLEFT2); - break; - case "RIGHT": - builder.lineToLinearHeading(PARK); - builder.lineToLinearHeading(PARK2); - break; - } - builder.addTemporalMarker(.1, robot.getArm()::armRest); - builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); - builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - this.robot.getDrive().followTrajectorySequence(builder.build()); - - } - - protected void parkLocation(){ - if (gamepad2.dpad_left) { - parkLocation="LEFT"; - } else if (gamepad2.dpad_right) { - parkLocation = "RIGHT"; - }} - protected void delaySet(){ - if (gamepad2.dpad_up && delay<13000) { - delay+=1000; - sleep(100); - } else if (gamepad2.dpad_down && delay>0) { - delay-=1000; - sleep(100); - } - } - @Override - - - public void followTrajectories(){ - TrajectorySequenceBuilder builder; - switch (state){ - case PURPLE: - builder = this.robot.getTrajectorySequenceBuilder(); - switch (randomization) { - case "LEFT": - builder.lineToLinearHeading(LEFT_PRELOAD); - break; - case "CENTER": - builder.lineToLinearHeading(CENTER_PRELOAD); - break; - case "RIGHT": - builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); - builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); - break; - } - this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); - - if (!robot.getDrive().isBusy()){ - state = autoState.YELLOW; - } - break; - case YELLOW: - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(LEAVE); - builder.lineToLinearHeading(READY_TRUSS); - builder.lineToLinearHeading(TO_BOARD); - switch (randomization) { - case "LEFT": - builder.splineToConstantHeading(SCORE_BOARD_LEFT.vec(),Math.toRadians(0)); - break; - case "CENTER": - builder.splineToConstantHeading(SCORE_BOARD_MID.vec(),Math.toRadians(0)); - break; - case "RIGHT": - builder.splineToConstantHeading(SCORE_BOARD_RIGHT.vec(),Math.toRadians(0)); - break; - } - this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); - runtime = getRuntime(); - state = autoState.FLIPYELLOW; - break; - case FLIPYELLOW: - if (getRuntime() > runtime + 5) { - this.robot.getArm().armSecondaryScore(); - this.robot.getWrist().wristScore(); - state = autoState.SCOREYELLOW; - } - break; - case SCOREYELLOW: - if (!robot.getDrive().isBusy()){ - this.robot.getClaw().open(); - runtime = getRuntime(); - state = autoState.FIRSTCYCLE; - } - break; - case FIRSTCYCLE: - builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(TO_BOARD.vec(),Math.toRadians(0)); - builder.lineToLinearHeading(READY_TRUSS); - builder.splineToConstantHeading(GET_STACK.vec(),Math.toRadians(0)); - builder.lineToLinearHeading(PICKUP_STACK, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); - runtime = getRuntime(); - state = autoState.RETRACT; - break; - case RETRACT: - if (getRuntime() > runtime + .3) { - this.robot.getClaw().close(); - this.robot.getArm().armRest(); - this.robot.getWrist().wristPickup(); - this.robot.getSlides().slideDown(); - } - if (getRuntime() > runtime + 2) { - this.robot.getClaw().openStack(); - this.robot.getArm().armPickupStack(); - } - if (!robot.getDrive().isBusy()) { - this.robot.getClaw().close(); - state = autoState.SCORECYCLEONE; - } - break; - case SCORECYCLEONE: - break; - } - } - - -// scorePreloadOne(); -// goBackToWhereYouCameFrom(); -//// scoreBoard(); -// score(); -// this.robot.getClaw().open(); -// backTruss(); -// this.robot.getClaw().close(); -// sleep(200); -// goBackstage(); -// scoreBoardStack(); -// scoreTest(); -// park(); - } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFour.java similarity index 94% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFour.java index cc4579d..f7635fd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFour.java @@ -12,20 +12,21 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Autonomous(name = "autoRed2+2") -public class AutoRedTwoPlusTwo extends LinearOpMode { +@Autonomous(name = "autoRed2+4") +public class AutoRedTwoPlusFour extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; private String randomization = "CENTER"; private String parkLocation = "LEFT"; + //Pose2ds //Preloads final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360)); final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270)); final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360)); + final static Pose2d LEFT_BOARD = new Pose2d(78, -23.5, Math.toRadians(360)); final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360)); final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360)); //Parka @@ -36,7 +37,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { //Cycles final static Vector2d LEAVE_BOARD = new Vector2d(50, -10); final static Vector2d TO_STACK = new Vector2d(-35, -10); - final static Vector2d TO_STACK_SLOW = new Vector2d(-45, -10); + final static Vector2d TO_STACK_SLOW = new Vector2d(-47.5, -10); final static Pose2d TO_STACK_SLOW2 = new Pose2d(-38.5, -8, Math.toRadians(360)); final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360)); final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360)); @@ -83,7 +84,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { break; } builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); -// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); builder.addTemporalMarker(.2, robot.getWrist()::wristScore); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); while (this.robot.getDrive().isBusy()) { @@ -106,12 +106,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { MecanumDrive.getAccelerationConstraint(40)); break; case "CENTER": - builder.lineToConstantHeading(TO_STACK_SLOW, + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 3.2)), MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(40)); break; case "RIGHT": - builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)), + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0.25)), MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(40)); break; @@ -142,12 +142,12 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { MecanumDrive.getAccelerationConstraint(40)); break; case "CENTER": - builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -.50)), + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)), MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(40)); break; case "RIGHT": - builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -2)), + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -3)), MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(40)); break; @@ -155,7 +155,7 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow); builder.addTemporalMarker(.3, robot.getClaw()::close); builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); - builder.addTemporalMarker(2, robot.getClaw()::openStack); + builder.addTemporalMarker(.7, robot.getClaw()::openStack); builder.addTemporalMarker(.2, robot.getSlides()::slideDown); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); while (this.robot.getDrive().isBusy()) { @@ -170,8 +170,8 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), MecanumDrive.getAccelerationConstraint(40)); builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy); - builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack); - builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); + builder.addTemporalMarker(2.2, robot.getArm()::armScoreStack); + builder.addTemporalMarker(2.4, robot.getWrist()::wristScore); builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); while (this.robot.getDrive().isBusy()) { @@ -276,11 +276,13 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { sleep(250); toStack(); this.robot.getClaw().close(); + sleep(300); toBoard(); clawSlowOpen(); // sleep(100); toStackLower(); this.robot.getClaw().close(); + sleep(300); switch (randomization) { default: toBoard(); @@ -292,7 +294,9 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { } while (!isStopRequested()) { this.robot.getArm().armRest(true); - this.robot.getClaw().openStack(); + this.robot.getWrist().wristPickup(); + this.robot.getSlides().slideDown(); + this.robot.getClaw().open(); robot.update(); } // park(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java new file mode 100644 index 0000000..817e91d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusFourTest.java @@ -0,0 +1,315 @@ +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.geometry.Vector2d; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.hardware.Robot; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; + +@Autonomous(name = "autoRed2+4Test") +public class AutoRedTwoPlusFourTest extends LinearOpMode { + protected Pose2d initialPosition; + GamepadEx controller1; + GamepadEx controller2; + private Robot robot; + private String randomization = "CENTER"; + private Boolean board1 = true; + private Boolean board2 = true; + private String parkLocation = "LEFT"; + + //Pose2ds + //Preloads + final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(27, -26, Math.toRadians(360)); + final static Pose2d CENTER_PRELOAD = new Pose2d(33, -26, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270)); + //Board Scores + final static Pose2d LEFT_BOARD = new Pose2d(78, -22, Math.toRadians(360)); + final static Pose2d CENTER_BOARD = new Pose2d(78, -33, Math.toRadians(360)); + final static Pose2d RIGHT_BOARD = new Pose2d(76, -39, Math.toRadians(360)); + //Parka + final static Pose2d PARK = new Pose2d(60, -58, Math.toRadians(360)); + final static Pose2d PARK2 = new Pose2d(80, -60, Math.toRadians(360)); + final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360)); + final static Pose2d PARKLEFT2 = new Pose2d(75, -10, Math.toRadians(360)); + //Cycles + final static Vector2d LEAVE_BOARD = new Vector2d(50, -10); + final static Vector2d TO_STACK = new Vector2d(-35, -10); + final static Vector2d TO_STACK_SLOW = new Vector2d(-45, -10); + final static Vector2d SCORE_STACK = new Vector2d(72.5, -35); + + //Trajectory Builders + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.setReversed(true); + builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180), + MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); + builder.setReversed(false); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void boardScore() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_BOARD, + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD, + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(30)); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armSecondaryScore); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void toStack() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.setReversed(true); + builder.splineToConstantHeading(LEAVE_BOARD, Math.toRadians(180)); + builder.lineToConstantHeading(TO_STACK, + MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.setReversed(false); + switch (randomization) { + case "LEFT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 1)), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "CENTER": + builder.lineToConstantHeading(TO_STACK_SLOW, + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "RIGHT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -1)), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + } + builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); + builder.addTemporalMarker(.3, robot.getClaw()::close); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(2, robot.getClaw()::openStack); + builder.addTemporalMarker(.2, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void toStackLower() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.setReversed(true); + builder.splineToConstantHeading(LEAVE_BOARD.plus(new Vector2d(0, -1)), Math.toRadians(180)); + builder.lineToConstantHeading(TO_STACK.plus(new Vector2d(0, -1)), + MecanumDrive.getVelocityConstraint(80, Math.toRadians(360), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(80)); + builder.setReversed(false); + switch (randomization) { + case "LEFT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, 0)), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "CENTER": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -.50)), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + case "RIGHT": + builder.lineToConstantHeading(TO_STACK_SLOW.plus(new Vector2d(0, -2)), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + break; + } + builder.addTemporalMarker(.3, robot.getArm()::armPickupStackLow); + builder.addTemporalMarker(.3, robot.getClaw()::close); + builder.addTemporalMarker(.3, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.5, robot.getClaw()::openStack); + builder.addTemporalMarker(.2, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void toBoard() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(LEAVE_BOARD); + builder.splineToConstantHeading(SCORE_STACK, Math.toRadians(360), + MecanumDrive.getVelocityConstraint(40, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(40)); + builder.addTemporalMarker(.1, robot.getArm()::armPickdaUpy); + builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack); + builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); + builder.addTemporalMarker(2.5, robot.getSlides()::slideFirstLayer); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void toStage() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(PARKLEFT2.vec()); + builder.addTemporalMarker(.01, robot.getArm()::armPickdaUpy); + builder.addTemporalMarker(2.5, robot.getArm()::armScoreStack); + builder.addTemporalMarker(2.5, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + protected void clawSlowOpen() { + double currentPos = .62; + double targetPos = .6; + double delta = (targetPos - currentPos) / 100; + for (int i = 0; i < 100; i++) { + this.robot.getClaw().setPos(currentPos + (delta * i)); + this.robot.getSlides().slideFirstLayer(); + sleep(5); + } + } + + protected void park() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (parkLocation) { + case "LEFT": + builder.lineToLinearHeading(PARKLEFT); + builder.lineToLinearHeading(PARKLEFT2); + break; + case "RIGHT": + builder.lineToLinearHeading(PARK); + builder.lineToLinearHeading(PARK2); + break; + } + builder.addTemporalMarker(.1, robot.getArm()::armRest); + builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); + while (this.robot.getDrive().isBusy()) { + robot.update(); + } + } + + //Init Customization + protected void parkLocation() { + if (gamepad2.dpad_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"; + } + } + + protected void cycles() { + if (gamepad2.left_bumper) { + if (!board1) board1 = true; + else board1 = false; + sleep(500); + } else if (gamepad2.right_bumper) { + if (!board2) board2 = true; + else board2 = false; + sleep(500); + } + } + + //Super fancy chinese shit + @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().initTargetingCamera(); + this.controller2 = new GamepadEx(this.gamepad2); + this.controller1 = new GamepadEx(this.gamepad1); + this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270)); + this.robot.getDrive().setPoseEstimate(initialPosition); + + // Do super fancy chinese shit + while (!this.isStarted()) { +// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + cycles(); + parkLocation(); + startLocation(); + this.telemetry.addData("Randomization", randomization); + this.telemetry.addData("Park Position", parkLocation); + this.telemetry.addData("Board 1", board1); + this.telemetry.addData("Board 2", board2); + this.telemetry.update(); + } + robot.update(); + this.robot.getClaw().close(); + scorePreloadOne(); + boardScore(); + this.robot.getClaw().open(); + sleep(250); + toStack(); + this.robot.getClaw().close(); + if (board1) { + toBoard(); + clawSlowOpen(); + } else { + toStage(); + this.robot.getClaw().openStack(); + } + toStackLower(); + this.robot.getClaw().close(); + if (board2) { + toBoard(); + clawSlowOpen(); + } else { + toStage(); + this.robot.getClaw().openStack(); + } + while (!isStopRequested()) { + this.robot.getArm().armRest(true); + this.robot.getWrist().wristPickup(); + this.robot.getSlides().slideDown(); + this.robot.getClaw().open(); + robot.update(); + } + } + +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java deleted file mode 100644 index 4e9dfdf..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoTest.java +++ /dev/null @@ -1,44 +0,0 @@ -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.geometry.Vector2d; -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.hardware.Robot; -import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; - -@Autonomous(name = "Test Stuff") -public class AutoTest extends LinearOpMode { - protected Pose2d initialPosition; - private Robot robot; - - //Pose2ds - //Preloads - final static Pose2d FIRSTMOVE = new Pose2d(10, 10, Math.toRadians(0)); - - protected void sequenceOne() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.splineToConstantHeading(new Vector2d(10, 10), Math.toRadians(0)); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - @Override - public void runOpMode() throws InterruptedException { - - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - this.robot = new Robot().init(hardwareMap); - this.initialPosition = new Pose2d(0, 0, Math.toRadians(0)); - this.robot.getDrive().setPoseEstimate(initialPosition); - - // Do super fancy chinese shit - while (!this.isStarted()) { - } - - sequenceOne(); - - } - -}