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 d75ac97..1badbc5 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 @@ -25,7 +25,7 @@ public class Slides { public static int targetMax = 830; public static int down = 0; - public static int mini_tier1 = 205; + public static int mini_tier1 = 200; 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/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index 80fab1b..f9c9344 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 @@ -2,6 +2,7 @@ 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.STACK4; import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; import com.acmerobotics.dashboard.config.Config; @@ -18,26 +19,28 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon; @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.7, -32.5, Math.toRadians(90)); + public static final Pose2d DROP_2 = new Pose2d(13.7, -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 DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(53.5, -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_PRELOAD_2 = new Pose2d(51, -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_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(180));//187 + 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_2 = new Pose2d(52, -29, Math.toRadians(180));//187 + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -30, Math.toRadians(188));//187 - public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(180));//817 + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 //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.5, Math.toRadians(180));//-36 + public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-56.8, -12.5, Math.toRadians(180)); + public static final Pose2d STACK_LOCATION = new Pose2d(-57.9, -12.4, Math.toRadians(190)); + + public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-58.5, -12.4, Math.toRadians(190)); @Override public void createTrajectories() { @@ -131,7 +134,7 @@ 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.setDcMotor(0.54); robot.intake.setpos(STACK5); if (!robot.drive.isBusy()) { macroState++; @@ -139,8 +142,8 @@ public class RedBackStageAuto extends AutoBase { break; //First 2 pixels off the stack are intaken by this case 6: - robot.intake.setDcMotor(0.44); - robot.intake.setpos(STACK5); + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK4); macroTime = getRuntime(); macroState++; break; @@ -192,9 +195,8 @@ public class RedBackStageAuto extends AutoBase { robot.resetMacro(0, getRuntime()); if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { 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()); + builder.lineToConstantHeading(STACK_LOCATION_TWO.vec()); this.robot.drive.followTrajectorySequenceAsync(builder.build()); macroState++; } @@ -202,22 +204,22 @@ public class RedBackStageAuto extends AutoBase { //waits for the robot to fin the trajectory case 11: robot.resetMacro(0, getRuntime()); - robot.intake.setDcMotor(0.44); + robot.intake.setDcMotor(0.54); robot.intake.setpos(STACK3); if (!robot.drive.isBusy()) { macroState++; } break; - //First 2 pixels off the stack are intaken by this + //3rd and 4th pixels off the stack are intaken by this case 12: - robot.intake.setDcMotor(0.44); - robot.intake.setpos(STACK3); + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK2); macroTime = getRuntime(); macroState++; break; //goes back to the easel case 13: - if (getRuntime() > macroTime + 0.5) { + if (getRuntime() > macroTime + 0.6) { robot.intake.setDcMotor(0); builder = this.robot.getTrajectorySequenceBuilder(); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); 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 new file mode 100644 index 0000000..58b3d29 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java @@ -0,0 +1,322 @@ +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.STACK4; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; + +import com.acmerobotics.dashboard.config.Config; +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 FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp") +public class RedFrontStageAuto extends AutoBase { + public static final Pose2d DROP_1 = new Pose2d(12, -32.5, Math.toRadians(0)); + public static final Pose2d DROP_2 = new Pose2d(13.7, -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 DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51, -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_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -30, Math.toRadians(188));//187 + + public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817 + + + //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 STACK_LOCATION = new Pose2d(-57.9, -36.4, Math.toRadians(190)); + + public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-58.5, -36.4, Math.toRadians(190)); + + public static final Pose2d STACK_LOCATION_THREE = new Pose2d(-59.5, -36.4, Math.toRadians(190)); + + public static final Pose2d POST_DROP_POS = new Pose2d(51, -51.5, Math.toRadians(180)); + + public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(51, -51.5, 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.Red); + } + + @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.28) { + robot.intake.setDcMotor(-0.22); + } + else{ + builder = this.robot.getTrajectorySequenceBuilder(); + robot.intake.setDcMotor(0); + builder.lineToLinearHeading(POST_DROP_POS); + builder.lineToLinearHeading(STACK_LOCATION); + 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++; + } + + case 4: + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK5); + macroTime = getRuntime(); + macroState++; + break; + + case 5: + if (getRuntime() > macroTime + 0.6) { + robot.intake.setDcMotor(0); + builder = this.robot.getTrajectorySequenceBuilder(); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_DROP_POS.vec()); + + 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 6: + // if drive is done move on + if (getRuntime() > macroTime + 1.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 7: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + + //Stack run 2 + case 8: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(POST_DROP_POS); + builder.lineToLinearHeading(STACK_LOCATION_TWO); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + break; + + } + + //waits for the robot to fin the trajectory + case 9: + robot.resetMacro(0, getRuntime()); + 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 10: + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK3); + macroTime = getRuntime(); + macroState++; + break; + + case 11: + if (getRuntime() > macroTime + 0.6) { + robot.intake.setDcMotor(0); + builder = this.robot.getTrajectorySequenceBuilder(); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_DROP_POS.vec()); + + 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 12: + // if drive is done move on + if (getRuntime() > macroTime + 1.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 13: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + + //stack run 3 + case 14: + robot.resetMacro(0, getRuntime()); + if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToLinearHeading(POST_DROP_POS); + builder.lineToLinearHeading(STACK_LOCATION_THREE); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + break; + + } + + //waits for the robot to fin the trajectory + case 15: + robot.resetMacro(0, getRuntime()); + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK4); + if (!robot.drive.isBusy()) { + macroState++; + } + break; + //3rd and 4th pixels off the stack are intaken by this + case 16: + robot.intake.setDcMotor(0.54); + robot.intake.setpos(STACK3); + macroTime = getRuntime(); + macroState++; + break; + + case 17: + if (getRuntime() > macroTime + 0.6) { + robot.intake.setDcMotor(0); + builder = this.robot.getTrajectorySequenceBuilder(); + //builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + builder.lineToConstantHeading(POST_DROP_POS.vec()); + + 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 18: + // if drive is done move on + if (getRuntime() > macroTime + 1.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 19: + 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 20: + robot.resetMacro(0, getRuntime()); + if (robot.macroState == 0) { + macroState = -1; + } + } + } +} 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 deleted file mode 100644 index 67060e8..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedLeft.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmode.autonomous; - -public class RedLeft { -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java deleted file mode 100644 index 6752599..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java +++ /dev/null @@ -1,64 +0,0 @@ -package org.firstinspires.ftc.teamcode.opmode.autonomous; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.hardware.Robot; - -@Autonomous (name= "varun_is_Dumb") -public class vannobAuto_RightRed extends LinearOpMode { - public Robot robot; - int teamElementLocation = 2; - int macrostate = 0; - - @Override - public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); - - // make trajectories - - while (!(isStarted() || isStopRequested())) { - double currentRuntime = getRuntime(); - robot.update(currentRuntime); - -// int newLocation = robot.camera.getMarkerId(); -// if (newLocation != -1) { -// teamElementLocation = newLocation; -// } - - telemetry.addLine("Initialized"); - - telemetry.addLine("Randomization of T.O.M: "+teamElementLocation); - telemetry.addLine(robot.getTelemetry()); - telemetry.update(); - } - - while(macrostate < 6 || !isStopRequested()){ - //update everything - robot.update(getRuntime()); - - switch (macrostate){ - case (0): - //follow trajectory - macrostate++; - break; - case (1): - // run intake - macrostate++; - break; - case (2): - macrostate++; - break; - case (3): - macrostate++; - break; - case (4): - macrostate++; - break; - case (5): - macrostate++; - break; - } - } - } -}