diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java index 4fcc4b9..1c06d82 100644 --- a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java +++ b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java @@ -23,10 +23,80 @@ public class MeepMeepTesting { .build() ); + RoadRunnerBotEntity BlueFrontStage = new DefaultBotBuilder(meepMeep) + .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90))) + //Score then pick up 1 white + .lineToLinearHeading(new Pose2d(-39.7, 33.5, Math.toRadians(-90))) + .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01) + + //Spline to board + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0)) + + //Go back to white stack + .splineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180)) + .lineToConstantHeading(new Pose2d(-45, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec()) + .splineToConstantHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180)) + + //Go back to board + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0)) + .build() + ); + + RoadRunnerBotEntity BlueFrontStage3 = new DefaultBotBuilder(meepMeep) + .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90))) + //Score then pick up 1 white + .lineToLinearHeading(new Pose2d(-46.7, 39.5, Math.toRadians(-90))) + .lineToLinearHeading(new Pose2d(-46.7,50.5,Math.toRadians(-90))) + .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01) + + //Spline to board + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0)) + + //Park + .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180))) + .build() + ); + + RoadRunnerBotEntity BlueFrontStage1 = new DefaultBotBuilder(meepMeep) + .setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15) + .followTrajectorySequence(drive -> + drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90))) + //Score then pick up 1 white + .lineToLinearHeading(new Pose2d(-36, 45.5, Math.toRadians(-90))) + .lineToLinearHeading(new Pose2d(-36,33.5,Math.toRadians(0))) + .lineToLinearHeading(new Pose2d(-32,33.5,Math.toRadians(0))) + .lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01) + + //Spline to board + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0)) + .lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec()) + .splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0)) + + //Park + .lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180))) + .build() + ); meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK) .setDarkMode(true) .setBackgroundAlpha(0.95f) - .addEntity(myBot) + //.addEntity(myBot) + //.addEntity(BlueFrontStage) + //.addEntity(BlueFrontStage3) + .addEntity(BlueFrontStage1) .start(); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java new file mode 100644 index 0000000..f1dec5a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java @@ -0,0 +1,217 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; +import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + + +import org.firstinspires.ftc.teamcode.hardware.Slides; +import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; +import org.firstinspires.ftc.teamcode.util.CenterStageCommon; + +@Config +@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") +public class BlueFrontPreload extends AutoBase { + public static final Pose2d DROP_1 = new Pose2d(-35, 32.5, Math.toRadians(0)); //THIS ANGLE NEEDS TO BE CHANGED + public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); + public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-49, 33.5, Math.toRadians(-90)); + public static final Pose2d DROP_1M = new Pose2d(-48.5, 30, Math.toRadians(-90)); + + public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); + + public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817 + + public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180)); + + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36 + + public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180)); + + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 59.5, Math.toRadians(180)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180)); + + public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180)); + + public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180)); + + @Override + public void createTrajectories() { + // set start position + Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90)); + robot.drive.setPoseEstimate(start); + robot.camera.setAlliance(CenterStageCommon.Alliance.Blue); + } + + @Override + public void followTrajectories() { + TrajectorySequenceBuilder builder = null; + switch (macroState) { + case 0: + builder = this.robot.getTrajectorySequenceBuilder(); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1M); + builder.lineToLinearHeading(DROP_1); + break; + case 2: + builder.lineToLinearHeading(DROP_2); + break; + case 3: + builder.lineToLinearHeading(DROP_3); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + break; + // DRIVE TO TAPE + case 1: + + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + break; + // RUN INTAKE + case 2: + // intake + if (getRuntime() < macroTime + 0.32) { + robot.intake.setDcMotor(-0.18); + } + else{ + builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_2_PART_2); + break; + case 2: + builder.lineToLinearHeading(DROP_2_PART_2); + break; + case 3: + builder.lineToLinearHeading(DROP_2_PART_2); + break; + } + robot.arm.setDoor(OPEN); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01); + + + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK6); + macroTime = getRuntime(); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + // if intake is done move on + break; + case 3: + // if drive is done move on + if (!robot.drive.isBusy()) { + macroTime = getRuntime(); + macroState++; + } + break; + + + case 4: + if (getRuntime() > macroTime + 0.06) { + robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.2); + builder = this.robot.getTrajectorySequenceBuilder(); + + switch (teamPropLocation) { + case 1: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); + break; + case 2: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); + break; + case 3: + builder.setTangent(Math.toRadians(90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + macroTime = getRuntime(); + } + break; +case 5: + // if drive is done move on + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1-70, getRuntime()); + macroState++; + } + break; + //extending the macro and about to score + case 6: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + + //Park + + case 7: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(PARK_1); + builder.lineToLinearHeading(PARK_2); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + + case 8: + // if drive is done move on + if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + macroState++; + } + break; + + case 9: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } + + case 19: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java index 0a81e60..ec25fff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java @@ -62,6 +62,7 @@ public class BlueFrontStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); switch (teamPropLocation) { case 1: + builder.lineToLinearHeading(DROP_1M); builder.lineToLinearHeading(DROP_1); break; case 2: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index f414679..97a74e4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -29,7 +29,7 @@ public class RedBackStageAuto extends AutoBase { public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180)); public static final Pose2d DROP_3 = new Pose2d(25, -45.5, Math.toRadians(90)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -28.3, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -34.5, Math.toRadians(190)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(190)); @@ -41,13 +41,13 @@ public class RedBackStageAuto extends AutoBase { //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 - public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11.2, Math.toRadians(185));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -10.2, Math.toRadians(190));//-36 - public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11.2, Math.toRadians(185)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190)); - public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11.2, Math.toRadians(185)); + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190)); - public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11.2, Math.toRadians(185)); + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185)); @Override public void createTrajectories() { @@ -101,7 +101,7 @@ public class RedBackStageAuto extends AutoBase { //Scores yellow pixel switch (teamPropLocation) { case 1: - builder.lineToLinearHeading(DEPOSIT_PRELOAD_1 ); + builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); break; case 2: robot.extendMacro(Slides.mini_tier1 -20, getRuntime()); @@ -140,7 +140,7 @@ public class RedBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-5))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3))); diff --git a/settings.gradle b/settings.gradle index a7f210f..86e57eb 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1,3 +1,4 @@ include ':FtcRobotController' include ':TeamCode' include ':MeepMeepTesting' +include ':MeepMeepTesting'