From 0a4f3636b42073a2604a57109ccd60c6edc83b72 Mon Sep 17 00:00:00 2001 From: ImposterVarunoverlord Date: Wed, 10 Jan 2024 14:16:30 -0600 Subject: [PATCH] Vanoob auto --- .../opmode/autonomous/RedBackStageAuto.java | 95 +++++++++++++------ 1 file changed, 64 insertions(+), 31 deletions(-) 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 db990e2..9ab902e 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,12 +1,11 @@ package org.firstinspires.ftc.teamcode.opmode.autonomous; -import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4; +import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3; 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.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.firstinspires.ftc.teamcode.hardware.Slides; @@ -18,30 +17,11 @@ public class RedBackStageAuto extends AutoBase { public static final Pose2d DROP_1 = new Pose2d(12, -37.5, Math.toRadians(90)); public static final Pose2d DROP_2 = new Pose2d(12, -37.5, Math.toRadians(90)); public static final Pose2d DROP_3 = new Pose2d(12, -37.5, Math.toRadians(90)); - public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.5, -32, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.5, -32, Math.toRadians(180)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.5, -32, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(53, -29, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(53, -32, Math.toRadians(180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53, -35, Math.toRadians(180)); public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -10.5);//-36 - public static final Pose2d STACK_LOCATION = new Pose2d(-56, -11, Math.toRadians(180)); - public Trajectory scorePurple1; - public Trajectory scorePurple2; - public Trajectory scorePurple3; - - public Trajectory scoreYellow1; - public Trajectory scoreYellow2; - public Trajectory scoreYellow3; - - public Trajectory parkRobot1; - public Trajectory parkRobot2; - public Trajectory parkRobot3; - - public Trajectory stackrun1b1; - public Trajectory stackrun1b2; - public Trajectory stackrun1b3; - - public Trajectory returnstackrun1b1; - public Trajectory returnstackrun1b2; - public Trajectory returnstackrun1b3; + public static final Pose2d STACK_LOCATION = new Pose2d(-56, -14, Math.toRadians(180)); @Override public void createTrajectories() { @@ -56,7 +36,7 @@ public class RedBackStageAuto extends AutoBase { switch (macroState) { case 0: builder = this.robot.getTrajectorySequenceBuilder(); - switch(teamPropLocation) { + switch (teamPropLocation) { case 1: builder.lineToLinearHeading(DROP_1); break; @@ -90,7 +70,7 @@ public class RedBackStageAuto extends AutoBase { robot.intake.setDcMotor(0); robot.extendMacro(Slides.mini_tier1, getRuntime()); builder = this.robot.getTrajectorySequenceBuilder(); - switch(teamPropLocation) { + switch (teamPropLocation) { case 1: builder.lineToLinearHeading(DEPOSIT_PRELOAD_1); break; @@ -127,24 +107,26 @@ public class RedBackStageAuto extends AutoBase { macroState++; } break; + //waits for the robot to fin the trajectory case 5: robot.resetMacro(0, getRuntime()); - if(!robot.drive.isBusy()){ + if (!robot.drive.isBusy()) { macroState++; } break; + //First 2 pixels off the stack are intaken by this case 6: robot.intake.setDcMotor(0.46); robot.intake.setpos(STACK5); macroTime = getRuntime(); - macroState ++; + macroState++; break; - + //gose back to the esile case 7: if (getRuntime() > macroTime + 1.5) { builder = this.robot.getTrajectorySequenceBuilder(); builder.lineToConstantHeading(POST_SCORING_SPLINE_END); - switch(teamPropLocation) { + switch (teamPropLocation) { case 1: builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0); break; @@ -160,6 +142,57 @@ public class RedBackStageAuto extends AutoBase { } break; case 9: + if (robot.macroState != 0) { + robot.extendMacro(Slides.mini_tier1, getRuntime()); + } + if (robot.macroState == 0 && !robot.drive.isBusy()) { + robot.resetMacro(0, getRuntime()); + macroState++; + } + break; + case 10: + robot.resetMacro(0, getRuntime()); + if (robot.macroState >= 4) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.splineToConstantHeading(POST_SCORING_SPLINE_END, 0); + builder.lineToConstantHeading(STACK_LOCATION.vec()); + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + case 11: + robot.resetMacro(0, getRuntime()); + if (!robot.drive.isBusy()) { + macroState++; + } + break; + //Geting the 2nd and 3rd pixel + case 12: + robot.intake.setDcMotor(0.46); + robot.intake.setpos(STACK3); + macroTime = getRuntime(); + macroState++; + break; + case 13: + if (getRuntime() > macroTime + 1.5) { + builder = this.robot.getTrajectorySequenceBuilder(); + builder.lineToConstantHeading(POST_SCORING_SPLINE_END); + switch (teamPropLocation) { + case 1: + builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0); + break; + case 2: + builder.splineToConstantHeading(DEPOSIT_PRELOAD_2.vec(), 0); + break; + case 3: + builder.splineToConstantHeading(DEPOSIT_PRELOAD_3.vec(), 0); + break; + } + this.robot.drive.followTrajectorySequenceAsync(builder.build()); + macroState++; + } + break; + case 14: macroState = -1; // PARK ROBOT // case 6: