diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java index 7184037..a4cc4ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java @@ -30,8 +30,8 @@ public class Intake { } //Position - public static double stack1 = 0.03; - public static double stack2 = 0.03; + public static double stack1 = 0.015; + public static double stack2 = 0.026; public static double stack3 = 0.065; public static double stack4 = 0.09; public static double stack5 = 0.1; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index 40945c8..0bdda44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -141,7 +141,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5))); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25))); + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -151,9 +151,10 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.5); + //robot.intake.setDcMotor(0.5); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { + robot.intake.setDcMotor(0.5); macroState++; } break; @@ -214,6 +215,7 @@ public class BlueBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); switch (teamPropLocation) { @@ -224,7 +226,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3))); break; case 3: - builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25))); + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -234,9 +236,9 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.68); robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { + robot.intake.setDcMotor(0.68); macroState++; } break; 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 index 82f7481..1c8bcb0 100644 --- 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 @@ -21,9 +21,9 @@ 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(-36, 33.5, Math.toRadians(0)); + public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0)); - public static final Pose2d DROP_1_PART_2 = new Pose2d(-32,33.5,Math.toRadians(0)); + public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0)); 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(-46.7, 50.5, Math.toRadians(-90)); @@ -31,15 +31,15 @@ public class BlueFrontPreload extends AutoBase { public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90)); - public static final Pose2d DROP_3M = new Pose2d(-46.7, 39.5, Math.toRadians(-90)); + public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, 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 DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36 @@ -95,7 +95,7 @@ public class BlueFrontPreload extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.32) { + if (getRuntime() < macroTime + 0.18) { robot.intake.setDcMotor(-0.23); } else{ @@ -116,7 +116,7 @@ public class BlueFrontPreload extends AutoBase { builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01); - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.74); robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -136,7 +136,7 @@ public class BlueFrontPreload extends AutoBase { case 4: if (getRuntime() > macroTime + 0.06) { robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.2); + robot.intake.setDcMotor(-0.5); builder = this.robot.getTrajectorySequenceBuilder(); switch (teamPropLocation) { 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 ec25fff..8c568c8 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 @@ -19,24 +19,27 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config -@Autonomous(name = "Blue FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Blue FrontStage Auto (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueFrontStageAuto 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_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0)); + + public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0)); public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.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_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90)); + public static final Pose2d DROP_1M = new Pose2d(-36, 45, 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 DROP_3M = new Pose2d(-48.7, 35.9, 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_2 = new Pose2d(53, 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 DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36 @@ -46,6 +49,10 @@ public class BlueFrontStageAuto extends AutoBase { 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 @@ -69,7 +76,8 @@ public class BlueFrontStageAuto extends AutoBase { builder.lineToLinearHeading(DROP_2); break; case 3: - builder.lineToLinearHeading(DROP_3); + builder.lineToLinearHeading(DROP_3M); +// builder.lineToLinearHeading(DROP_3); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -87,17 +95,28 @@ public class BlueFrontStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.32) { - robot.intake.setDcMotor(-0.18); + if (getRuntime() < macroTime + 0.18) { + robot.intake.setDcMotor(-0.23); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1_PART_2); + break; + case 2: + builder.lineToLinearHeading(DROP_2_PART_2); + break; + case 3: + builder.lineToLinearHeading(DROP_3); + break; + } robot.arm.setDoor(OPEN); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01); - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.74); robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); @@ -117,11 +136,8 @@ public class BlueFrontStageAuto extends AutoBase { case 4: if (getRuntime() > macroTime + 0.06) { robot.arm.setDoor(CLOSE); - robot.intake.setDcMotor(-0.8); + robot.intake.setDcMotor(-0.5); builder = this.robot.getTrajectorySequenceBuilder(); -// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); -// builder.lineToConstantHeading(POST_DROP_POS.vec()); -// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0)); switch (teamPropLocation) { case 1: @@ -142,24 +158,6 @@ public class BlueFrontStageAuto extends AutoBase { builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; -// 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.lineToLinearHeading(POST_DROP_POS); -// 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.lineToLinearHeading(POST_DROP_POS); -// 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++; @@ -271,89 +269,93 @@ public class BlueFrontStageAuto extends AutoBase { macroState++; } break; - case 13: - robot.resetMacro(0, getRuntime()); - if (robot.macroState == 0) { - macroState = -1; - } //stack run 3 -// case 13: -// robot.resetMacro(0, getRuntime()); -// if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) { -// builder = this.robot.getTrajectorySequenceBuilder(); -// builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180)); -// builder.lineToLinearHeading(POST_DROP_POS); -// builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180)); -// this.robot.drive.followTrajectorySequenceAsync(builder.build()); -// macroState++; -// } -// break; -// -// //waits for the robot to fin the trajectory -// case 14: -// robot.resetMacro(0, getRuntime()); -// robot.arm.setDoor(OPEN); -// robot.intake.setDcMotor(0.54); -// robot.intake.setpos(STACK2); -// if (!robot.drive.isBusy()) { -// macroState++; -// } -// break; -// //3rd and 4th pixels off the stack are intaken by this -// case 15: -// robot.intake.setDcMotor(0.54); -// robot.arm.setDoor(OPEN); -// robot.intake.setpos(STACK1); -// macroTime = getRuntime(); -// macroState++; -// break; -// -// case 16: -// if (getRuntime() > macroTime + 0.6) { -// robot.intake.setDcMotor(0); -// builder = this.robot.getTrajectorySequenceBuilder(); -// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); -// builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0)); -// -// switch (teamPropLocation) { -// case 1: -// builder.lineToLinearHeading(PRE_DEPOSIT_POS); -// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0)); -// break; -// case 2: -// builder.lineToLinearHeading(PRE_DEPOSIT_POS); -// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0)); -// break; -// case 3: -// builder.lineToLinearHeading(PRE_DEPOSIT_POS); -// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0)); -// break; -// } -// this.robot.drive.followTrajectorySequenceAsync(builder.build()); -// macroState++; -// macroTime = getRuntime(); -// } -// break; -// case 17: -// // if drive is done move on -// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { -// macroTime = getRuntime(); -// robot.macroState = 0; -// robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); -// macroState++; -// } -// break; -// //extending the macro and about to score -// case 18: -// if (robot.macroState != 0) { -// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); -// } -// if (robot.macroState == 0 && !robot.drive.isBusy()) { -// robot.resetMacro(0, getRuntime()); -// macroState++; -// } -// break; + case 13: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180)); + builder.lineToLinearHeading(POST_DROP_POS); + builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180)); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + + //waits for the robot to fin the trajectory + case 14: + robot.resetMacro(0, getRuntime()); + robot.arm.setDoor(OPEN); + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK2); + if (!robot.drive.isBusy()) { + macroState++; + } + break; + //3rd and 4th pixels off the stack are intaken by this + case 15: + robot.intake.setDcMotor(0.54); + robot.arm.setDoor(OPEN); + robot.intake.setpos(STACK1); + macroTime = getRuntime(); + macroState++; + break; + case 16: + if (getRuntime() > macroTime + 0.6) { + robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.45); + builder = this.robot.getTrajectorySequenceBuilder(); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); + //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); + + + 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 17: + // if drive is done move on + if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { + macroTime = getRuntime(); + robot.macroState = 0; + robot.extendMacro(Slides.mini_tier1 + 20, getRuntime()); + macroState++; + } + break; + //extending the macro and about to score + case 18: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + case 19: robot.resetMacro(0, getRuntime()); if (robot.macroState == 0) { 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 97a74e4..3711b93 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 @@ -24,30 +24,30 @@ public class RedBackStageAuto extends AutoBase { public static final Pose2d DROP_1M = new Pose2d(11.5, -32.5, Math.toRadians(180)); - public static final Pose2d DROP_2 = new Pose2d(13.6, -34.5, Math.toRadians(90)); + public static final Pose2d DROP_2 = new Pose2d(15.6, -33.5, Math.toRadians(90)); 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 DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90)); 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)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -32.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(180));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.6, -32.5, Math.toRadians(190));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -29.5, Math.toRadians(180));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(192));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(180));//187 //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, -10.2, Math.toRadians(190));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11, Math.toRadians(180));//-36 - public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11, Math.toRadians(180)); - public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11, Math.toRadians(180)); - public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185)); + public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11, Math.toRadians(185)); @Override public void createTrajectories() { @@ -90,7 +90,7 @@ public class RedBackStageAuto extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.26) { - robot.intake.setDcMotor(-0.35); + robot.intake.setDcMotor(-0.18); } // if intake is done move on else { @@ -104,7 +104,7 @@ public class RedBackStageAuto extends AutoBase { builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); break; case 2: - robot.extendMacro(Slides.mini_tier1 -20, getRuntime()); + //robot.extendMacro(Slides.mini_tier1 -40, getRuntime()); builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); break; case 3: @@ -133,6 +133,7 @@ public class RedBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); switch (teamPropLocation) { @@ -140,7 +141,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(-3))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3))); @@ -153,22 +154,23 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.48); + robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { + macroState++; } break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.54); + robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK4); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 0.5) { + if (getRuntime() > macroTime + 1.2) { robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); @@ -214,6 +216,7 @@ public class RedBackStageAuto extends AutoBase { case 10: robot.resetMacro(0, getRuntime()); if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) { + robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); switch (teamPropLocation) { @@ -221,7 +224,7 @@ public class RedBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.5))); + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8))); @@ -234,22 +237,22 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.74); robot.intake.setpos(STACK3); + robot.intake.setDcMotor(0.58); if (!robot.drive.isBusy()) { macroState++; } break; //3rd and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.84); + robot.intake.setDcMotor(0.58); robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.37) { + if (getRuntime() > macroTime + 1) { robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java new file mode 100644 index 0000000..5df7df8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java @@ -0,0 +1,219 @@ +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 = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp") +public class RedFrontPreload extends AutoBase { + public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, -33.5, Math.toRadians(180)); + + public static final Pose2d DROP_1 = new Pose2d(-32,-33.5,Math.toRadians(180)); + 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(-46.7, -50.5, Math.toRadians(90)); + public static final Pose2d DROP_1M = new Pose2d(-36, -45, 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.7, -35.9, Math.toRadians(90)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -23.5, Math.toRadians(186));//817 + + public static final Pose2d STACK_LOCATION = new Pose2d(-52, -29.6, Math.toRadians(0)); + + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 + + public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0)); + + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0)); + + 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.Red); + } + + @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_3M); +// 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.18) { + robot.intake.setDcMotor(-0.23); + } + else{ + builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); + switch (teamPropLocation) { + case 1: + builder.lineToLinearHeading(DROP_1_PART_2); + break; + case 2: + builder.lineToLinearHeading(DROP_2_PART_2); + break; + case 3: + builder.lineToLinearHeading(DROP_3); + break; + } + robot.arm.setDoor(OPEN); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-2.5))).waitSeconds(.01); + + + robot.intake.setDcMotor(0.74); + 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.5); + builder = this.robot.getTrajectorySequenceBuilder(); + + switch (teamPropLocation) { + case 1: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180)); + break; + case 2: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180)); + break; + case 3: + builder.setTangent(Math.toRadians(-90)); + builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180)); + builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180)); + 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); + 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