diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index 4b677ee..7e96e6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -22,9 +22,9 @@ public class Arm { private double armTempPos; private double doorPos; private double wristPos; - public static double ARM_KP = 0.11; + public static double ARM_KP = 0.12; - public static double doorOpenPos = 0.3; + public static double doorOpenPos = 0.36; public static double doorClosePos = 0.61; public static double armStart = 0.24; 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 293dc4c..7184037 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 @@ -16,7 +16,7 @@ public class Intake { private Position pos = Position.UP; public enum Position { - STACK1, STACK2, STACK3, STACK4, STACK5, UP; + STACK1, STACK2, STACK3, STACK4, STACK5, STACK6, UP; public Position nextPosition() { int nextOne = (this.ordinal() + 1) % Position.values().length; @@ -35,6 +35,8 @@ public class Intake { public static double stack3 = 0.065; public static double stack4 = 0.09; public static double stack5 = 0.1; + + public static double stack6 = 0.13; public static double up = 0.30; public static double motorPower = 0; @@ -62,7 +64,11 @@ public class Intake { } else if (stack == Position.STACK5) { lServo.setPosition(stack5); rServo.setPosition(stack5); - } else if (stack == Position.UP) { + } else if (stack == Position.STACK6) { + lServo.setPosition(stack6); + rServo.setPosition(stack6); + } + else if (stack == Position.UP) { lServo.setPosition(up); rServo.setPosition(up); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 1badbc5..a850233 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -17,7 +17,7 @@ public class Slides { // public static double d = 0; // public static double f = 0.01; //p was 0.0014 - public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0018,0.02,0,0.01); + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.02,0,0.01); public static double pTolerance = 20; private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); @@ -25,7 +25,7 @@ public class Slides { public static int targetMax = 830; public static int down = 0; - public static int mini_tier1 = 200; + public static int mini_tier1 = 250; public static int tier1 = 350; public static int tier2 = 500; public static int tier3 = 720; 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 5a3037a..64e8527 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 @@ -20,29 +20,32 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; public class BlueBackStageAuto extends AutoBase { public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180)); - public static final Pose2d DROP_3M = new Pose2d(14.7, 30, Math.toRadians(-180)); + public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180)); public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90)); public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180)); - public static final Pose2d DROP_1 = new Pose2d(25, 43, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(54.8, 28.7, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.6, 34.5, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.2, 39.3, Math.toRadians(-180)); + public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.4, 35.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.5, 30.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.7, 32, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-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(26, 11.1 , Math.toRadians(-180));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-54.8, 11.1, Math.toRadians(-180)); + public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180)); + + public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180)); + + public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180)); - public static final Pose2d STACK_LOCATION2 = new Pose2d(-55.3, 11.1, Math.toRadians(-180)); @Override public void createTrajectories() { @@ -85,7 +88,7 @@ public class BlueBackStageAuto extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.1) { - robot.intake.setDcMotor(-0.18); + robot.intake.setDcMotor(-0.24); } // if intake is done move on else { @@ -129,7 +132,17 @@ public class BlueBackStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION.vec().plus(new Vector2d(0,0))); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); + break; + case 2: + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0))); + break; + case 3: + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5))); + break; + } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -137,7 +150,7 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.38); + robot.intake.setDcMotor(0.68); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -145,7 +158,7 @@ public class BlueBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.38); + robot.intake.setDcMotor(0.68); robot.intake.setpos(STACK5); macroTime = getRuntime(); macroState++; @@ -154,7 +167,7 @@ public class BlueBackStageAuto extends AutoBase { case 7: if (getRuntime() > macroTime + 0.03) { //robot.intake.setDcMotor(-0.0); - robot.intake.setDcMotor(-0.65); + robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); @@ -201,7 +214,18 @@ public class BlueBackStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0))); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); + break; + case 2: + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2))); + break; + case 3: + builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5 + ))); + break; + } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -209,23 +233,23 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.38); - robot.intake.setpos(STACK4); + robot.intake.setDcMotor(0.68); + robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { macroState++; } break; //Third and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.38); - robot.intake.setpos(STACK3); + robot.intake.setDcMotor(0.6 8); + robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: if (getRuntime() > macroTime + 0.03) { - robot.intake.setDcMotor(0.65); + robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); 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 new file mode 100644 index 0000000..eb165d6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java @@ -0,0 +1,328 @@ +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 FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") +public class BlueFrontStageAuto extends AutoBase { + public static final Pose2d DROP_1 = new Pose2d(-50, 45.5, Math.toRadians(-90)); + public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(-33.5, 40.5, Math.toRadians(180)); + + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, 35.3, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.5, 36, Math.toRadians(8));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.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, 58.5, Math.toRadians(180)); + + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 60.5, Math.toRadians(180)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, 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_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.22) { + robot.intake.setDcMotor(-0.18); + } + else{ + builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); +// robot.arm.setDoor(OPEN); +// builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-3.6,1.5))); +// +// robot.intake.setDcMotor(0.44); +// robot.intake.setpos(STACK6); +// macroTime = getRuntime(); + builder.lineToLinearHeading(POST_DROP_POS); + 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(); +// //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: + 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 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, 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; + + //Stack run 2 + case 7: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180)); + builder.lineToConstantHeading(POST_DROP_POS.vec()); + builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.7, -1.5)).vec(), Math.toRadians(180)); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + //waits for the robot to fin the trajectory + case 8: + robot.resetMacro(0, getRuntime()); + robot.arm.setDoor(OPEN); + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK4); + if (!robot.drive.isBusy()) { + macroState++; + } + break; + //3rd and 4th pixels off the stack are in-taken by this + case 9: + robot.intake.setDcMotor(0.54); + robot.arm.setDoor(OPEN); + robot.intake.setpos(STACK3); + macroTime = getRuntime(); + macroState++; + break; + + case 10: + 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.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 11: + // if drive is done move on + if (getRuntime() > macroTime + 6.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 12: + 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 (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 19: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } + } + } +} 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 babfc0b..7b8a71e 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 @@ -19,29 +19,35 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config @Autonomous(name = "Red Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedBackStageAuto extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(12, -32.5, Math.toRadians(180)); - public static final Pose2d DROP_2 = new Pose2d(13.3, -33.5, Math.toRadians(90)); + + public static final Pose2d DROP_1 = new Pose2d(16, -32.5, Math.toRadians(180)); + + 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 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, -27.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(50, -31.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -42, 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)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(188));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -30, Math.toRadians(188));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.6, -32.5, Math.toRadians(190));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(192));//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(24, -12.4, Math.toRadians(190));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -10.2, Math.toRadians(190));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-53, -12.9, Math.toRadians(190)); + public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190)); - public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-54, -12.9, Math.toRadians(190)); + 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, -10.2, Math.toRadians(185)); @Override public void createTrajectories() { @@ -60,6 +66,7 @@ public class RedBackStageAuto extends AutoBase { switch (teamPropLocation) { case 1: builder.lineToLinearHeading(DROP_1); + builder.lineToLinearHeading(DROP_1M); break; case 2: builder.lineToLinearHeading(DROP_2); @@ -82,8 +89,8 @@ public class RedBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.23) { - robot.intake.setDcMotor(-0.18); + if (getRuntime() < macroTime + 0.26) { + robot.intake.setDcMotor(-0.35); } // if intake is done move on else { @@ -97,6 +104,7 @@ public class RedBackStageAuto extends AutoBase { builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); break; case 2: + robot.extendMacro(Slides.mini_tier1 -20, getRuntime()); builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); break; case 3: @@ -111,7 +119,7 @@ public class RedBackStageAuto extends AutoBase { case 3: // extend macro if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1, getRuntime()); + robot.extendMacro(Slides.mini_tier1 - 20, getRuntime()); } // if macro and drive are done, move on if (robot.macroState == 0 && !robot.drive.isBusy()) { @@ -127,7 +135,17 @@ public class RedBackStageAuto extends AutoBase { builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION.vec()); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85))); + break; + case 2: + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2))); + break; + case 3: + builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3))); + break; + } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -135,7 +153,7 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.54); + robot.intake.setDcMotor(0.48); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -151,7 +169,8 @@ public class RedBackStageAuto extends AutoBase { //goes back to the easel case 7: if (getRuntime() > macroTime + 0.5) { - robot.intake.setDcMotor(0.65); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); + robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); @@ -176,14 +195,14 @@ public class RedBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 , getRuntime()); macroState++; } break; //extending the macro and about to score case 9: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1 , getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); @@ -197,7 +216,17 @@ public class RedBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); - builder.lineToConstantHeading(STACK_LOCATION_TWO.vec()); + switch (teamPropLocation) { + case 1: + builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2))); + break; + case 2: + builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); + break; + case 3: + builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8))); + break; + } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -205,23 +234,24 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK2); + robot.intake.setDcMotor(0.74); + robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { macroState++; } break; //3rd and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK1); + robot.intake.setDcMotor(0.84); + robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.6) { - robot.intake.setDcMotor(0.65); + if (getRuntime() > macroTime + 0.37) { + robot.arm.setDoor(Arm.DoorPosition.CLOSE); + robot.intake.setDcMotor(-0.35); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); @@ -246,7 +276,7 @@ public class RedBackStageAuto extends AutoBase { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); macroState++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java index c73c565..924242b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java @@ -1,11 +1,13 @@ 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; @@ -17,7 +19,7 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @Config -@Autonomous(name = "Red FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp") public class RedFrontStageAuto extends AutoBase { public static final Pose2d DROP_1 = new Pose2d(-50, -45.5, Math.toRadians(90)); public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90)); @@ -29,22 +31,22 @@ public class RedFrontStageAuto extends AutoBase { public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 - public static final Pose2d STACK_LOCATION = new Pose2d(-52, -34.4, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-54.5, -33.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, -57.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS = new Pose2d(-45, -57.5, Math.toRadians(0)); - public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(180)); + public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(0)); - public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(180)); + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(0)); @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); + robot.camera.setAlliance(CenterStageCommon.Alliance.Blue); } @Override @@ -80,15 +82,15 @@ public class RedFrontStageAuto extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.22) { - robot.intake.setDcMotor(-0.15); + robot.intake.setDcMotor(-0.18); } else{ builder = this.robot.getTrajectorySequenceBuilder(); robot.intake.setDcMotor(0); - builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(0,-1.5))); + builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-2,-1.5))); robot.arm.setDoor(OPEN); - robot.intake.setDcMotor(0.54); - robot.intake.setpos(STACK5); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK6); macroTime = getRuntime(); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -96,17 +98,18 @@ public class RedFrontStageAuto extends AutoBase { // if intake is done move on break; case 3: - // if drive is done move on + // if drive is done move on if (!robot.drive.isBusy()) { macroTime = getRuntime(); macroState++; - } - break; + } + break; case 4: - if (getRuntime() > macroTime + 0.2) { - robot.intake.setDcMotor(0); + if (getRuntime() > macroTime + 0.06) { + robot.arm.setDoor(CLOSE); + robot.intake.setDcMotor(-0.44); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); builder.lineToLinearHeading(POST_DROP_POS); @@ -136,14 +139,14 @@ public class RedFrontStageAuto extends AutoBase { if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); + robot.extendMacro(Slides.mini_tier1, getRuntime()); macroState++; } break; //extending the macro and about to score case 6: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + robot.extendMacro(Slides.mini_tier1, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); @@ -156,14 +159,14 @@ public class RedFrontStageAuto extends AutoBase { 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.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(0)); builder.lineToLinearHeading(POST_DROP_POS); - builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180)); + builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-0.5)).vec(), Math.toRadians(0)); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - //waits for the robot to fin the trajectory + //waits for the robot to fin the trajectory case 8: robot.resetMacro(0, getRuntime()); robot.arm.setDoor(OPEN); @@ -184,11 +187,11 @@ public class RedFrontStageAuto extends AutoBase { case 10: if (getRuntime() > macroTime + 0.6) { - robot.intake.setDcMotor(0); + robot.intake.setDcMotor(-0.45); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0)); - builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0)); + builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0)); + //builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2))); switch (teamPropLocation) { @@ -212,7 +215,7 @@ public class RedFrontStageAuto extends AutoBase { break; case 11: // if drive is done move on - if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) { + if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); @@ -231,82 +234,82 @@ public class RedFrontStageAuto extends AutoBase { break; //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.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 19: robot.resetMacro(0, getRuntime()); if (robot.macroState == 0) { @@ -315,3 +318,4 @@ public class RedFrontStageAuto extends AutoBase { } } } +