eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

2+4 auto mostly reliable

This commit is contained in:
ImposterVarunoverlord 2024-01-12 21:54:55 -06:00
parent 099ce699ef
commit 0590708cd0
5 changed files with 128 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -17,7 +17,7 @@ public class Slides {
// public static double d = 0; // public static double d = 0;
// public static double f = 0.01; // public static double f = 0.01;
//p was 0.0014 //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; public static double pTolerance = 20;
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); 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 targetMax = 830;
public static int down = 0; 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 tier1 = 350;
public static int tier2 = 500; public static int tier2 = 500;
public static int tier3 = 720; public static int tier3 = 720;

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java
View File

@ -30,7 +30,7 @@ public abstract class AutoBase extends LinearOpMode {
// initialize robot // initialize robot
robot = new Robot(hardwareMap); robot = new Robot(hardwareMap);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red); //robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
// create trajectories // create trajectories
createTrajectories(); createTrajectories();

129
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous; 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.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; 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.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; 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.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config @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 class BlueBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(12.3, 32.5, Math.toRadians(0)); public static final Pose2d DROP_3 = new Pose2d(16, 32, 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, 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 DROP_1 = new Pose2d(25, 41.6, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, 27.5, Math.toRadians(1)); 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(52, 32.5, Math.toRadians(1)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(54, 32.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, 39.5, Math.toRadians(1)); 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 @Override
public void createTrajectories() { public void createTrajectories() {
// set start position // set start position
Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90)); Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start); robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
} }
@Override @Override
@ -73,12 +78,13 @@ public class BlueBackStageAuto extends AutoBase {
// RUN INTAKE // RUN INTAKE
case 2: case 2:
// intake // intake
if (getRuntime() < macroTime + 0.28) { if (getRuntime() < macroTime + 0.2) {
robot.intake.setDcMotor(-0.2); robot.intake.setDcMotor(-0.22);
} }
// if intake is done move on // if intake is done move on
else { else {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime()); robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel //Scores yellow pixel
@ -107,13 +113,16 @@ public class BlueBackStageAuto extends AutoBase {
if (robot.macroState == 0 && !robot.drive.isBusy()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
// STACK RUN 1 -------------------------
case 4: case 4:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); 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()); builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
@ -122,6 +131,8 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 5: case 5:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
} }
@ -135,28 +146,30 @@ public class BlueBackStageAuto extends AutoBase {
break; break;
//goes back to the easel //goes back to the easel
case 7: case 7:
if (getRuntime() > macroTime + 1.6) { if (getRuntime() > macroTime + 0.3) {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); 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) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break; break;
case 2: case 2:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break; break;
case 3: case 3:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
case 8: case 8:
// if drive is done move on // if drive is done move on
if (!robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
@ -171,105 +184,89 @@ public class BlueBackStageAuto extends AutoBase {
if (robot.macroState == 0 && !robot.drive.isBusy()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
//scores pixel and goes back to the stack // STACK RUN 2
case 10: case 10:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
this.robot.drive.followTrajectorySequenceAsync(builder.build()); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
macroState++;
}
break;
/* builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(STACK_LOCATION.vec()); builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
} }
break; break;
//Ensures that the robot has scored the pixel and moves on //waits for the robot to fin the trajectory
case 11: case 11:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
} }
break; break;
//Getting the 2nd and 3rd pixel //First 2 pixels off the stack are intaken by this
case 12: case 12:
robot.intake.setDcMotor(0.49); robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK3); robot.intake.setpos(STACK3);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//Goes back to the easel //goes back to the easel
case 13: case 13:
if (getRuntime() > macroTime + 1.2) { if (getRuntime() > macroTime + 0.3) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); 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) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break; break;
case 2: case 2:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break; break;
case 3: case 3:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
case 14: case 14:
// if drive is done move on // if drive is done move on
if (!robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); robot.extendMacro(Slides.mini_tier1 + 140, getRuntime());
macroState++; macroState++;
} }
break; break;
//Scoring the pixels //extending the macro and about to score
case 15: case 15:
if (robot.macroState != 0) { 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()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
} }
break; break;
// PARK ROBOT
case 16: case 16:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (robot.macroState == 0) {
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) {
macroState = -1; macroState = -1;
} }
// PARK ROBOT // 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;
} }
} }
} }

119
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous; 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.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5; 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.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous; 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.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder; import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config @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 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 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 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_1 = new Pose2d(52, -27.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, -32.5, Math.toRadians(181)); 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(181)); 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 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)); 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 // set start position
Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90)); Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start); robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
} }
@Override @Override
@ -74,11 +79,12 @@ public class RedBackStageAuto extends AutoBase {
case 2: case 2:
// intake // intake
if (getRuntime() < macroTime + 0.28) { if (getRuntime() < macroTime + 0.28) {
robot.intake.setDcMotor(-0.2); robot.intake.setDcMotor(-0.22);
} }
// if intake is done move on // if intake is done move on
else { else {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime()); robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel //Scores yellow pixel
@ -107,13 +113,16 @@ public class RedBackStageAuto extends AutoBase {
if (robot.macroState == 0 && !robot.drive.isBusy()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
// STACK RUN 1 -------------------------
case 4: case 4:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); 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()); builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
@ -122,6 +131,8 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 5: case 5:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
} }
@ -135,28 +146,30 @@ public class RedBackStageAuto extends AutoBase {
break; break;
//goes back to the easel //goes back to the easel
case 7: case 7:
if (getRuntime() > macroTime + 1.6) { if (getRuntime() > macroTime + 0.5) {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); 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) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break; break;
case 2: case 2:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break; break;
case 3: case 3:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
case 8: case 8:
// if drive is done move on // if drive is done move on
if (!robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
@ -171,105 +184,89 @@ public class RedBackStageAuto extends AutoBase {
if (robot.macroState == 0 && !robot.drive.isBusy()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
//scores pixel and goes back to the stack // STACK RUN 2
case 10: case 10:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
this.robot.drive.followTrajectorySequenceAsync(builder.build()); builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
macroState++;
}
break;
/* builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(STACK_LOCATION.vec()); builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
} }
break; break;
//Ensures that the robot has scored the pixel and moves on //waits for the robot to fin the trajectory
case 11: case 11:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
} }
break; break;
//Getting the 2nd and 3rd pixel //First 2 pixels off the stack are intaken by this
case 12: case 12:
robot.intake.setDcMotor(0.49); robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK3); robot.intake.setpos(STACK3);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//Goes back to the easel //goes back to the easel
case 13: case 13:
if (getRuntime() > macroTime + 1.2) { if (getRuntime() > macroTime + 0.5) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); 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) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break; break;
case 2: case 2:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break; break;
case 3: case 3:
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec()); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
macroTime = getRuntime();
} }
break; break;
case 14: case 14:
// if drive is done move on // if drive is done move on
if (!robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
robot.macroState = 0; robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 +20 , getRuntime()); robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
macroState++; macroState++;
} }
break; break;
//Scoring the pixels //extending the macro and about to score
case 15: case 15:
if (robot.macroState != 0) { 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()) { if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
macroState++; macroState++;
} }
break; break;
// PARK ROBOT
case 16: case 16:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) { if (robot.macroState == 0) {
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) {
macroState = -1; macroState = -1;
} }
// PARK ROBOT // 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;
} }
} }
} }

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedLeft.java Normal file
View File

@ -0,0 +1,4 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
public class RedLeft {
}