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 2c1beff..48f7a4f 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 @@ -168,7 +168,7 @@ public class Robot { //Values public static double SLIDE_POWER_UP = .7; public static double SLIDE_POWER_DOWN = .2; - public static int SLIDELAYERONE = 60; + public static int SLIDELAYERONE = 150; public static int SLIDEAUTOSTACKS = 250; public static int SLIDEUP = 630; public static int SLIDELAYERTWO = 350; @@ -302,9 +302,10 @@ public class Robot { public static double DIFFY_TOL = 0.01; public static double ARM_MAX_DELTA = 0.02; public static double ARMREST = 0.89; - public static double ARMSCORE = 0.4; + public static double ARMSCORESTACK = 0.53; public static double ARMACCSCORE = .57; - public static double ARMPICKUPSTACK = 0.815; + public static double ARMPICKUPSTACK = 0.91; + public static double ARMPICKUPSTACKLOW = 0.93; public static double PICKUP = 0.92; //PControler public PController armPController; @@ -332,6 +333,11 @@ public class Robot { moveArm(PICKUP, now); } + public void armPickdaUpy() { + moveArm(.83, false); + } + + public void armScore() { this.armSecondaryScore(); } @@ -340,10 +346,19 @@ public class Robot { moveArm(ARMACCSCORE, false); } + public void armScoreStack() { + moveArm(ARMSCORESTACK, false); + } + public void armPickupStack() { moveArm(ARMPICKUPSTACK, false); } + public void armPickupStackLow() { + moveArm(ARMPICKUPSTACKLOW, false); + } + + public void armRest() { armRest(false); } @@ -446,8 +461,8 @@ public class Robot { public static class Claw { //Values public static double OPEN = 0.65; - public static double BIGOPEN = 0f; - public static double CLOSE = 0.73; + public static double BIGOPEN = .48; + public static double CLOSE = 0.75; public static double CLAW_MIN = 0; public static double CLAW_MAX = 1; //Servo diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java index 6dec286..f93e1d8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/DriveConstants.java @@ -68,8 +68,8 @@ public class DriveConstants { * small and gradually increase them later after everything is working. All distance units are * inches. */ - public static double MAX_VEL = 80; - public static double MAX_ACCEL = 80; + public static double MAX_VEL = 60; + public static double MAX_ACCEL = 50; public static double MAX_ANG_VEL = Math.toRadians(360); public static double MAX_ANG_ACCEL = Math.toRadians(360); 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 7641246..d75f764 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java @@ -60,8 +60,8 @@ import java.util.List; */ @Config public class MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDrive { - public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(3, 0, 2.5); - public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 1.5); + public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(5, 0, 4); + public static PIDCoefficients HEADING_PID = new PIDCoefficients(10, 0, 2.5); public static double LATERAL_MULTIPLIER = 1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java index 4dd3242..1d44706 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 @@ -7,6 +7,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; @Autonomous(name = "autoRed") @@ -18,26 +20,29 @@ public class AutoRed 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, -32, Math.toRadians(360)); + 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, -35, Math.toRadians(270)); + final static Pose2d RIGHT_PRELOAD = new Pose2d(45, -27, Math.toRadians(270)); //Board Scores - final static Pose2d LEFT_BOARD = new Pose2d(75.8, -26.5, Math.toRadians(358)); - final static Pose2d CENTER_BOARD = new Pose2d(80, -29, Math.toRadians(358)); - final static Pose2d RIGHT_BOARD = new Pose2d(75.8, -40, Math.toRadians(358)); + 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 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(60, -6, Math.toRadians(360)); - final static Pose2d PARKLEFT2 = new Pose2d(80, -6, Math.toRadians(360)); + final static Pose2d PARKLEFT = new Pose2d(50, -15, Math.toRadians(360)); + final static Pose2d PARKLEFT2 = new Pose2d(75, -2, Math.toRadians(360)); protected void scorePreloadOne() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "LEFT": - builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(360)); + builder.setReversed(true); + builder.splineToSplineHeading(LEFT_PRELOAD_TWO, Math.toRadians(180), + MecanumDrive.getVelocityConstraint(70, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(70)); + builder.setReversed(false); break; case "CENTER": builder.lineToLinearHeading(CENTER_PRELOAD); @@ -59,11 +64,13 @@ public class AutoRed extends LinearOpMode { builder.lineToLinearHeading(CENTER_BOARD); break; case "RIGHT": - builder.lineToLinearHeading(RIGHT_BOARD); + builder.lineToLinearHeading(RIGHT_BOARD, + MecanumDrive.getVelocityConstraint(50, Math.toRadians(60), DriveConstants.TRACK_WIDTH), + MecanumDrive.getAccelerationConstraint(50)); break; } builder.addTemporalMarker(.2, robot.getArm()::armScore); - builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); +// builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer); builder.addTemporalMarker(.2, robot.getWrist()::wristScore); this.robot.getDrive().followTrajectorySequenceAsync(builder.build()); while (this.robot.getDrive().isBusy()) { @@ -130,12 +137,13 @@ public class AutoRed extends LinearOpMode { this.telemetry.update(); } robot.update(); + this.robot.getClaw().close(); scorePreloadOne(); boardScore(); - sleep(250); +// sleep(250); this.robot.getClaw().open(); sleep(250); -// park(); + park(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java deleted file mode 100644 index c6b21e2..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFaronepluszero.java +++ /dev/null @@ -1,168 +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.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 = "AutoRedFar1+0") -public class AutoRedFaronepluszero extends LinearOpMode { - protected Pose2d initialPosition; - private Robot robot; - private String randomization; - private String parkLocation; - private int delay = 10000; - - //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(230)); - final static Pose2d CENTER_PRELOAD = new Pose2d(-35, -28, Math.toRadians(270)); - final static Pose2d LEFT_PRELOAD = new Pose2d(-47, -35, Math.toRadians(270)); - //Ready Truss - final static Pose2d LEAVE = new Pose2d(-43, -40, Math.toRadians(270)); - final static Pose2d READY_TRUSS = new Pose2d(-43, -57, Math.toRadians(0)); - final static Pose2d TO_BOARD = new Pose2d(30, -57, Math.toRadians(0)); - final static Pose2d READY_SCORE = new Pose2d(37, -34, Math.toRadians(0)); - final static Pose2d SCORE_BOARD_LEFT = new Pose2d(54, -27, Math.toRadians(5)); - final static Pose2d SCORE_BOARD_MID = new Pose2d(54, -32, Math.toRadians(0)); - final static Pose2d SCORE_BOARD_RIGHT = new Pose2d(54, -41, 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 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); - builder.lineToLinearHeading(RIGHT_PRELOAD_GETOUT); - break; - } - builder.addTemporalMarker(.4,robot.getArm()::armScore); -// builder.addTemporalMarker(2,robot.getArm()::armRest); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - protected void goBackToWhereYouCameFrom() { - TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(LEAVE); -// builder.lineToLinearHeading(READY_TRUSS); -// builder.lineToLinearHeading(TO_BOARD); - builder.addTemporalMarker(.3,robot.getArm()::armRest); - 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(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - case "CENTER": - builder.lineToLinearHeading(SCORE_BOARD_MID, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - case "RIGHT": - builder.lineToLinearHeading(SCORE_BOARD_RIGHT, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - break; - } - builder.addTemporalMarker(1.8, robot.getClaw()::open); - this.robot.getDrive().followTrajectorySequence(builder.build()); - } - - 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 runOpMode() throws InterruptedException { - this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); - this.robot = new Robot().init(hardwareMap); -// 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()) { - parkLocation(); - delaySet(); - 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(); - - } -// sleep(delay); - scorePreloadOne(); - goBackToWhereYouCameFrom(); -// scoreBoard(); -// score(); -// sleep(500); -// park(); - } - -} 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 73ebfb4..cc4579d 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 @@ -3,6 +3,7 @@ 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; @@ -15,36 +16,44 @@ import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.Tra public class AutoRedTwoPlusTwo extends LinearOpMode { protected Pose2d initialPosition; private Robot robot; - private String randomization; + private String randomization = "CENTER"; + private String parkLocation = "LEFT"; //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, -32, Math.toRadians(360)); - final static Pose2d CENTER_PRELOAD = new Pose2d(34, -28, Math.toRadians(270)); - final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270)); + 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(76.7, -27, Math.toRadians(360)); - final static Pose2d CENTER_BOARD = new Pose2d(76.5, -32, Math.toRadians(355)); - final static Pose2d RIGHT_BOARD = new Pose2d(76.5, -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(-35, -8, Math.toRadians(360)); - final static Pose2d TO_STACK_SLOW = new Pose2d(-38.5, -8.5, Math.toRadians(360)); + 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, -12, 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 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)); - final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360)); + final static Vector2d SCORE_STACK = new Vector2d(72.5, -35); + final static Vector2d SCORE_STACK_SLOW = new Vector2d(73, -35); //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); + 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); @@ -53,7 +62,6 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { builder.lineToLinearHeading(RIGHT_PRELOAD); break; } - builder.addTemporalMarker(.5, robot.getArm()::armScore); this.robot.getDrive().followTrajectorySequence(builder.build()); } @@ -61,142 +69,234 @@ public class AutoRedTwoPlusTwo extends LinearOpMode { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); switch (randomization) { case "LEFT": - builder.lineToLinearHeading(LEFT_BOARD, - MecanumDrive.getVelocityConstraint(20, 20, DriveConstants.TRACK_WIDTH), - MecanumDrive.getAccelerationConstraint(20)); - ; + builder.lineToLinearHeading(LEFT_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)); + 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.getSlides()::slideFirstLayer); 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(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(.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(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(.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(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 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(360), + 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(75, -31, Math.toRadians(360)), - 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 = .7; + 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)); - sleep(30); + this.robot.getSlides().slideFirstLayer(); + sleep(5); } } protected void park() { TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(BACK_OFF); - builder.lineToLinearHeading(PARK); + 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.getClaw()::close); builder.addTemporalMarker(.1, robot.getWrist()::wristPickup); builder.addTemporalMarker(.1, robot.getSlides()::slideDown); - this.robot.getDrive().followTrajectorySequence(builder.build()); + 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().getStartingPosition()); - randomization = String.valueOf(this.robot.getCamera().getStartingPosition()); +// randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue()); + parkLocation(); + startLocation(); + this.telemetry.addData("Starting Position", randomization); + this.telemetry.addData("Park Position", parkLocation); this.telemetry.update(); } - this.robot.update(); + robot.update(); + this.robot.getClaw().close(); scorePreloadOne(); boardScore(); - - sleep(150); +// sleep(250); this.robot.getClaw().open(); - sleep(150); - - switch (randomization) { - case "LEFT": - toStackNoDrift(); - break; - case "CENTER": - toStack(); - break; - case "RIGHT": - toStack(); - break; - } - sleep(500); - this.robot.getClaw().close(); sleep(250); - this.robot.getArm().armRest(); - scoreStack(); - this.robot.refreshPoseEstimateFromAprilTag(); - this.robot.getClaw().setPos(.83); - scoreTest(); - park(); + toStack(); + this.robot.getClaw().close(); + toBoard(); + clawSlowOpen(); +// sleep(100); + toStackLower(); + this.robot.getClaw().close(); + switch (randomization) { + default: + toBoard(); + clawSlowOpen(); + break; +// case "LEFT": +// toStage(); +// break; + } + while (!isStopRequested()) { + this.robot.getArm().armRest(true); + this.robot.getClaw().openStack(); + robot.update(); + } +// park(); } } +