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 8d6b340..97a9057 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.002,0.02,0,0.01); + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0018,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 = 170; + public static int mini_tier1 = 190; 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/AutoBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java index 22937b9..c33448e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java @@ -30,7 +30,7 @@ public abstract class AutoBase extends LinearOpMode { // initialize robot robot = new Robot(hardwareMap); - robot.camera.setAlliance(CenterStageCommon.Alliance.Red); + //robot.camera.setAlliance(CenterStageCommon.Alliance.Red); // create trajectories createTrajectories(); 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 569e1f3..4d3f249 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +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.STACK5; @@ -8,38 +9,42 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.hardware.Arm; 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 Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp") +@Autonomous(name = "Blue Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp") public class BlueBackStageAuto extends AutoBase { - public static final Pose2d DROP_1 = new Pose2d(12.3, 32.5, Math.toRadians(0)); - public static final Pose2d DROP_2 = new Pose2d(13.7, 32.5, Math.toRadians(-90)); + public static final Pose2d DROP_3 = new Pose2d(16, 32, Math.toRadians(-180)); + public static final Pose2d DROP_2 = new Pose2d(13.7, 35, Math.toRadians(-90)); - public static final Pose2d ALINE = new Pose2d(12,37.5, Math.toRadians(-90)); + public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180)); - public static final Pose2d DROP_3 = new Pose2d(25, 41.3, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, 27.5, Math.toRadians(1)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, 32.5, Math.toRadians(1)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, 39.5, Math.toRadians(1)); + public static final Pose2d DROP_1 = new Pose2d(25, 41.6, Math.toRadians(-90)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(54.8, 27.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(54, 32.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(53.3, 39.5, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.3, 35.3, Math.toRadians(7)); + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.4, 35.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.3, 29, Math.toRadians(7)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.6, 30.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(7)); + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.7, 32, Math.toRadians(-187)); - public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, 8.5);//-36 + //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, 11.6, Math.toRadians(-180));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, 10.6, Math.toRadians(0)); + public static final Pose2d STACK_LOCATION = new Pose2d(-55.5, 11.6, Math.toRadians(-180)); @Override public void createTrajectories() { // set start position Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90)); robot.drive.setPoseEstimate(start); + robot.camera.setAlliance(CenterStageCommon.Alliance.Blue); } @Override @@ -73,12 +78,13 @@ public class BlueBackStageAuto extends AutoBase { // RUN INTAKE case 2: // intake - if (getRuntime() < macroTime + 0.28) { - robot.intake.setDcMotor(-0.2); + if (getRuntime() < macroTime + 0.2) { + robot.intake.setDcMotor(-0.22); } // if intake is done move on else { robot.intake.setDcMotor(0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.extendMacro(Slides.mini_tier1, getRuntime()); builder = this.robot.getTrajectorySequenceBuilder(); //Scores yellow pixel @@ -107,13 +113,16 @@ public class BlueBackStageAuto extends AutoBase { if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; + macroTime = getRuntime(); } break; + // STACK RUN 1 ------------------------- case 4: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -122,6 +131,8 @@ public class BlueBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; } @@ -135,28 +146,30 @@ public class BlueBackStageAuto extends AutoBase { break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 1.6) { + if (getRuntime() > macroTime + 0.3) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; case 8: // if drive is done move on - if (!robot.drive.isBusy()) { + if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); @@ -171,105 +184,89 @@ public class BlueBackStageAuto extends AutoBase { if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; + macroTime = getRuntime(); } break; - //scores pixel and goes back to the stack + // STACK RUN 2 case 10: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; - - } - break; - /* builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - //Ensures that the robot has scored the pixel and moves on + //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { macroState++; } break; - //Getting the 2nd and 3rd pixel + //First 2 pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.49); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; - //Goes back to the easel + //goes back to the easel case 13: - if (getRuntime() > macroTime + 1.2) { + if (getRuntime() > macroTime + 0.3) { + robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; case 14: // if drive is done move on - if (!robot.drive.isBusy()) { + if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 140, getRuntime()); macroState++; } break; - //Scoring the pixels + //extending the macro and about to score case 15: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; } break; + + // PARK ROBOT case 16: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; - } - break;*/ - - case 11: - robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (robot.macroState == 0) { macroState = -1; } // PARK ROBOT -// case 6: -// // reset macro' -// if (robot.macroState != 0) { -// robot.resetMacro(0, getRuntime()); -// } -// // if macro and drive are done, end auto -// if (robot.macroState == 0 && !robot.drive.isBusy()) { -// macroState=-1; -// } -// break; +// } } } \ No newline at end of file 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 8aeeba2..95475bd 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; +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.STACK5; @@ -8,30 +9,33 @@ import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.teamcode.hardware.Arm; 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 Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp") +@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.3, -32.5, Math.toRadians(180)); + public static final Pose2d DROP_1 = new Pose2d(12, -32.5, Math.toRadians(180)); public static final Pose2d DROP_2 = new Pose2d(13.7, -32.5, Math.toRadians(90)); - public static final Pose2d ALINE = new Pose2d(12,-37.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, -41.3, Math.toRadians(90)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(181)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, -32.5, Math.toRadians(181)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -39.5, Math.toRadians(181)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, -32.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -39.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.3, -35.3, Math.toRadians(187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.3, -29, Math.toRadians(187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -29, Math.toRadians(187)); public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(187)); - public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 + //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, -10.6, Math.toRadians(180));//-36 public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, -10.6, Math.toRadians(180)); @@ -40,6 +44,7 @@ public class RedBackStageAuto extends AutoBase { // set start position Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90)); robot.drive.setPoseEstimate(start); + robot.camera.setAlliance(CenterStageCommon.Alliance.Red); } @Override @@ -74,11 +79,12 @@ public class RedBackStageAuto extends AutoBase { case 2: // intake if (getRuntime() < macroTime + 0.28) { - robot.intake.setDcMotor(-0.2); + robot.intake.setDcMotor(-0.22); } // if intake is done move on else { robot.intake.setDcMotor(0); + robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.extendMacro(Slides.mini_tier1, getRuntime()); builder = this.robot.getTrajectorySequenceBuilder(); //Scores yellow pixel @@ -107,13 +113,16 @@ public class RedBackStageAuto extends AutoBase { if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; + macroTime = getRuntime(); } break; + // STACK RUN 1 ------------------------- case 4: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; @@ -122,6 +131,8 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; } @@ -135,28 +146,30 @@ public class RedBackStageAuto extends AutoBase { break; //goes back to the easel case 7: - if (getRuntime() > macroTime + 1.6) { + if (getRuntime() > macroTime + 0.5) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; case 8: // if drive is done move on - if (!robot.drive.isBusy()) { + if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); @@ -171,105 +184,89 @@ public class RedBackStageAuto extends AutoBase { if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; + macroTime = getRuntime(); } break; - //scores pixel and goes back to the stack + // STACK RUN 2 case 10: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; - - } - break; - /* builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); builder.lineToConstantHeading(STACK_LOCATION.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } break; - //Ensures that the robot has scored the pixel and moves on + //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.44); + robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { macroState++; } break; - //Getting the 2nd and 3rd pixel + //First 2 pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.49); + robot.intake.setDcMotor(0.44); robot.intake.setpos(STACK3); macroTime = getRuntime(); macroState++; break; - //Goes back to the easel + //goes back to the easel case 13: - if (getRuntime() > macroTime + 1.2) { + if (getRuntime() > macroTime + 0.5) { + robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); break; case 2: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0)); break; case 3: - builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); + builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0)); break; } this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; + macroTime = getRuntime(); } break; case 14: // if drive is done move on - if (!robot.drive.isBusy()) { + if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { macroTime = getRuntime(); robot.macroState = 0; - robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 180, getRuntime()); macroState++; } break; - //Scoring the pixels + //extending the macro and about to score case 15: if (robot.macroState != 0) { - robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime()); + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); } if (robot.macroState == 0 && !robot.drive.isBusy()) { robot.resetMacro(0, getRuntime()); macroState++; } break; + + // PARK ROBOT case 16: robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { - builder = this.robot.getTrajectorySequenceBuilder(); - builder.lineToLinearHeading(DEPOSIT_PRELOAD_2); - this.robot.drive.followTrajectorySequenceAsync(builder.build()); - macroState++; - } - break;*/ - - case 11: - robot.resetMacro(0, getRuntime()); - if (robot.macroState >= 4) { + if (robot.macroState == 0) { macroState = -1; } // PARK ROBOT -// case 6: -// // reset macro' -// if (robot.macroState != 0) { -// robot.resetMacro(0, getRuntime()); -// } -// // if macro and drive are done, end auto -// if (robot.macroState == 0 && !robot.drive.isBusy()) { -// macroState=-1; -// } -// break; +// } } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedLeft.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedLeft.java new file mode 100644 index 0000000..67060e8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedLeft.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.opmode.autonomous; + +public class RedLeft { +}