From f8c5ba3296fdae19b779d08f63446dbe41972ec8 Mon Sep 17 00:00:00 2001 From: sihan Date: Sat, 13 Jan 2024 10:01:24 -0600 Subject: [PATCH] READY CODE LM3 --- .../roadrunner/drive/MecanumDrive.java | 2 +- .../ftc/teamcode/opmodes/AutoBlue.java | 194 ++++++--------- .../ftc/teamcode/opmodes/AutoBlueFar.java | 206 +++------------- .../teamcode/opmodes/AutoBlueTwoPlusTwo.java | 36 +-- .../ftc/teamcode/opmodes/AutoRed.java | 231 ++++++------------ .../ftc/teamcode/opmodes/AutoRedFar.java | 210 +++------------- .../teamcode/opmodes/AutoRedTwoPlusTwo.java | 37 +-- .../ftc/teamcode/util/Configurables.java | 8 +- 8 files changed, 276 insertions(+), 648 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 a4fdc26..c2055ad 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 @@ -76,7 +76,7 @@ public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr private TrajectoryFollower follower; - private DcMotorEx leftFront, leftRear, rightRear, rightFront; + public DcMotorEx leftFront, leftRear, rightRear, rightFront; private List motors; private IMU imu; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java index bd84c68..c784ced 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java @@ -1,111 +1,96 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; 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; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +@Config @Autonomous(name = "autoBlue") public class AutoBlue extends LinearOpMode { protected Pose2d initialPosition; - - protected Trajectory preloadOne; - protected Trajectory scoreOne; - protected Trajectory boardOne; - protected Trajectory backOffOne; - - - protected Trajectory preloadTwo; - protected Trajectory scoreTwo; - protected Trajectory backOffTwo; - - - protected Trajectory preloadThree; - protected Trajectory scoreThree; - protected Trajectory backOffThree; - - - protected Trajectory park1; - protected Trajectory park2; - - private Robot robot; private String randomization; - private int random; + + //Pose2ds + //Preloads + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); + //Board Scores + final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185)); + final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185)); + final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185)); + //Park + final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180)); + final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void boardScore() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_BOARD); + break; + case "LEFT": + builder.lineToLinearHeading(LEFT_BOARD); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + 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()); + } + @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(-29, -32, Math.toRadians(180))) - .build(); - - this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180))) - .addTemporalMarker(.2, robot.getArm()::armSecondaryScore) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) - .lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180))) - .build(); - - - //Randomization Two - this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(-35, -28, Math.toRadians(270))) - .build(); - - this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180))) - .addTemporalMarker(.2, robot.getArm()::armSecondaryScore) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) - .lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180))) - .build(); - - //Randomization Three - this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270))) - .build(); - - this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) - .lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180))) - .addTemporalMarker(.2, robot.getArm()::armSecondaryScore) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) - .lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180))) - .build(); - - //Park - this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) - .lineToLinearHeading(new Pose2d(-65, -55, 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(-80, -60, Math.toRadians(180))) - .build(); +// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) +// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) +// .build(); // Do super fancy chinese shit while (!this.isStarted()) { @@ -114,41 +99,12 @@ public class AutoBlue extends LinearOpMode { this.telemetry.update(); } - switch (randomization) { - case "RIGHT": - this.robot.getDrive().followTrajectory(preloadOne); - this.robot.getDrive().followTrajectory(scoreOne); - this.robot.getDrive().followTrajectory(boardOne); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffOne); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); - break; - case "CENTER": - this.robot.getDrive().followTrajectory(preloadTwo); - this.robot.getDrive().followTrajectory(scoreTwo); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffTwo); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); - break; - case "LEFT": - this.robot.getDrive().followTrajectory(preloadThree); - this.robot.getDrive().followTrajectory(scoreThree); - this.robot.getClaw().open(); - sleep(500); - this.robot.getDrive().followTrajectory(backOffThree); - sleep(500); - this.robot.getDrive().followTrajectory(park1); - this.robot.getDrive().followTrajectory(park2); - break; - } - - + scorePreloadOne(); + boardScore(); + sleep(250); + this.robot.getClaw().open(); + sleep(250); + park(); } } 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 index 393f31e..392734d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java @@ -1,150 +1,60 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; 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; - +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +@Config @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; + + //Pose2ds + //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, -29, Math.toRadians(360)); + final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD_ONE); + builder.lineToLinearHeading(LEFT_PRELOAD_TWO); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(initialPosition); + 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.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(-50, -28, Math.toRadians(180))) - .addTemporalMarker(.2, robot.getArm()::armSecondaryScore) - .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()::armSecondaryScore) - .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()::armSecondaryScore) - .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()); @@ -152,52 +62,8 @@ public class AutoBlueFar extends LinearOpMode { 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); - - + scorePreloadOne(); + goBackToWhereYouCameFrom(); } } 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 13e5e30..4d95360 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 @@ -8,10 +8,12 @@ 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; @Config -@Autonomous(name = "autoBLue2+2") +@Autonomous(name = "autoBlue2+2") public class AutoBlueTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; @@ -20,19 +22,19 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { //Pose2ds //Preloads final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(180)); final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); //Board Scores - final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185)); - final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185)); - final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185)); + final static Pose2d RIGHT_BOARD = new Pose2d(-75.7, -25.7, Math.toRadians(185)); + final static Pose2d CENTER_BOARD = new Pose2d(-75.7, -35, Math.toRadians(185)); + final static Pose2d LEFT_BOARD = new Pose2d(-75.7, -42, Math.toRadians(185)); //Stack Cycle final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180)); final static Pose2d TO_STACK = new Pose2d(40.75, -7.25, 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(-74.5, -28, Math.toRadians(180)); + final static Pose2d SCORE_STACK = new Pose2d(-73, -31, Math.toRadians(180)); //Park final static Pose2d BACK_OFF = new Pose2d(-60,-58,Math.toRadians(180)); final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180)); @@ -76,10 +78,11 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { protected void toStack() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(LEAVE_BOARD); - builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); + 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()::pickup); builder.lineToLinearHeading(TO_STACK); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -95,6 +98,15 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void scoreTest() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(-77, -31, Math.toRadians(180)), + MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + builder.addTemporalMarker(.2,this::clawSlowOpen); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void clawSlowOpen() { double currentPos = .86; double targetPos = .8; @@ -138,25 +150,21 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode { randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); this.telemetry.update(); } - scorePreloadOne(); boardScore(); - sleep(250); + sleep(100); this.robot.getClaw().open(); - sleep(250); toStack(); sleep(500); this.robot.getClaw().close(); - sleep(500); + sleep(250); this.robot.getArm().armRest(); scoreStack(); this.robot.getClaw().setPos(.86); - sleep(150); - clawSlowOpen(); - sleep(300); + scoreTest(); 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 a2d2018..1b3d7e5 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,121 +1,96 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; 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; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +@Config @Autonomous(name = "autoRed") public class AutoRed extends LinearOpMode { protected Pose2d initialPosition; - - protected Trajectory preloadOne; - protected Trajectory scoreOne; - protected Trajectory boardOne; - protected Trajectory backOffOne; - - - protected Trajectory preloadTwo; - protected Trajectory scoreTwo; - protected Trajectory backOffTwo; - - - protected Trajectory preloadThree; - protected Trajectory scoreThree; - protected Trajectory boardThree; - protected Trajectory backOffThree; - - - 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; - private int random; + + //Pose2ds + //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, -29, Math.toRadians(360)); + final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); + //Board Scores + final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355)); + final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355)); + final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355)); + + //Park + final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360)); + final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD_ONE); + builder.lineToLinearHeading(LEFT_PRELOAD_TWO); + 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); + break; + case "RIGHT": + builder.lineToLinearHeading(RIGHT_BOARD); + break; + } + builder.addTemporalMarker(.2, robot.getArm()::armScore); + builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); + builder.addTemporalMarker(.2, robot.getWrist()::wristScore); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + 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.getWrist()::wristPickup); + builder.addTemporalMarker(.1, robot.getSlides()::slideDown); + 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.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(29.5, -29, Math.toRadians(360))) - .build(); - - this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end()) - .lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end()) - .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360))) - .build(); - - - //Randomization Two - this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270))) - .build(); - - this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end()) - .lineToLinearHeading(new Pose2d(75, -36.3, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end()) - .lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360))) - .build(); - - //Randomization Three - this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition) - .lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270))) - .build(); - - this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end()) - .lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360))) - .addTemporalMarker(.2, robot.getArm()::armScore) - .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer) - .addTemporalMarker(.2, robot.getWrist()::wristScore) - .build(); - - this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end()) - .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360))) - .build(); - - //Park - this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end()) - .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, -57, Math.toRadians(360))) - .build(); +// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) +// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) +// .build(); // Do super fancy chinese shit while (!this.isStarted()) { @@ -124,68 +99,12 @@ public class AutoRed extends LinearOpMode { this.telemetry.update(); } - switch (randomization) { - case "LEFT": - 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); - this.robot.getSlides().slideDown(); - 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); - this.robot.getSlides().slideDown(); - 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); - this.robot.getSlides().slideDown(); - 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); - - - - + scorePreloadOne(); + boardScore(); + sleep(250); + this.robot.getClaw().open(); + sleep(250); + park(); } } 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 index 22ce900..78ea576 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java @@ -1,151 +1,64 @@ package org.firstinspires.ftc.teamcode.opmodes; import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; 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; +import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder; -@Autonomous(name = "autoRedFar") +@Config +@Autonomous(name = "autRedFar") 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; + + //Pose2ds + //Preloads + final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180)); + final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270)); + final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270)); + + protected void scorePreloadOne() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + switch (randomization) { + case "RIGHT": + builder.lineToLinearHeading(RIGHT_PRELOAD_ONE); + builder.lineToLinearHeading(RIGHT_PRELOAD_TWO); + break; + case "CENTER": + builder.lineToLinearHeading(CENTER_PRELOAD); + break; + case "LEFT": + builder.lineToLinearHeading(LEFT_PRELOAD); + break; + } + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + + protected void goBackToWhereYouCameFrom() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(initialPosition); + 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.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()::armSecondaryScore) - .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()::armSecondaryScore) - .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, -39, Math.toRadians(360))) - .addTemporalMarker(.02, robot.getArm()::armSecondaryScore) - .addTemporalMarker(.02, robot.getWrist()::wristScore) - .build(); - - this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end()) - .lineToLinearHeading(new Pose2d(40, -42, Math.toRadians(360))) - .build(); - - - //Park - this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end()) - .lineToLinearHeading(new Pose2d(40, -12, 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(10))) - .build(); +// this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end()) +// .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360))) +// .build(); // Do super fancy chinese shit while (!this.isStarted()) { @@ -153,52 +66,9 @@ public class AutoRedFar extends LinearOpMode { 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); - + scorePreloadOne(); + goBackToWhereYouCameFrom(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java index c100a8c..5524adb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java @@ -8,6 +8,8 @@ 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; @Config @Autonomous(name = "autoRed2+2") @@ -19,19 +21,19 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { //Pose2ds //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, -29, Math.toRadians(360)); + final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360)); final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270)); final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355)); - final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355)); - final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355)); + final static Pose2d LEFT_BOARD = new Pose2d(75.3, -26.5, Math.toRadians(360)); + final static Pose2d CENTER_BOARD = new Pose2d(75.3, -36.3, Math.toRadians(360)); + final static Pose2d RIGHT_BOARD = new Pose2d(75.3, -40, Math.toRadians(355)); //Stack Cycle final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360)); - final static Pose2d TO_STACK = new Pose2d(-40, -8, Math.toRadians(360)); + final static Pose2d TO_STACK = new Pose2d(-40, -8.4, Math.toRadians(360)); final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360)); - final static Pose2d APPROACHING_BOARD = new Pose2d(70, -28, Math.toRadians(360)); - final static Pose2d SCORE_STACK = new Pose2d(74.45, -28, Math.toRadians(360)); + final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360)); + final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360)); //Park final static Pose2d BACK_OFF = new Pose2d(60,-58,Math.toRadians(360)); final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360)); @@ -75,10 +77,11 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { protected void toStack() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToLinearHeading(LEAVE_BOARD); - builder.addTemporalMarker(.3, robot.getArm()::armPickupStack); + 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()::pickup); builder.lineToLinearHeading(TO_STACK); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -94,6 +97,15 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { this.robot.getDrive().followTrajectorySequence(builder.build()); } + protected void scoreTest() { + TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(new Pose2d(75, -31, Math.toRadians(360)), + MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(20)); + builder.addTemporalMarker(.2,this::clawSlowOpen); + this.robot.getDrive().followTrajectorySequence(builder.build()); + } + protected void clawSlowOpen() { double currentPos = .86; double targetPos = .78; @@ -138,21 +150,18 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { scorePreloadOne(); boardScore(); - sleep(250); + sleep(100); this.robot.getClaw().open(); - sleep(250); toStack(); sleep(500); this.robot.getClaw().close(); - sleep(500); + sleep(250); this.robot.getArm().armRest(); scoreStack(); this.robot.getClaw().setPos(.86); - sleep(500); - clawSlowOpen(); - sleep(300); + scoreTest(); 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 cc29aa8..066e999 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 @@ -17,7 +17,7 @@ public class Configurables { public static double ARMREST = 0.8; public static double ARMSCORE = 0.39; public static double ARMACCSCORE = .38; - public static double ARMPICKUPSTACK = 0.843; + public static double ARMPICKUPSTACK = 0.8415; public static double PICKUP = 0.84; public static double WRISTPICKUP = 0.28; public static double WRISTSCORE = .96; @@ -30,15 +30,15 @@ public class Configurables { //Drive Speed public static double SPEED = 1; - public static double SLOWMODE_SPEED = 0.5; - public static double TURN = 1; + public static double SLOWMODE_SPEED = 0.3; + public static double TURN = .75; public static double SLOWMODE_TURN = 0.3; //Motor Positions public static double SLIDE_POWER_UP = 1; public static double SLIDE_POWER_DOWN = .7; public static int SLIDELAYERONE = 150; - public static int SLIDEAUTOSTACKS = 225; + public static int SLIDEAUTOSTACKS = 350; public static int SLIDEUP = 1150; public static int HANGRELEASE = 2500; public static int HANG = 0;