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

fix red PLSSSSSSSs

This commit is contained in:
ImposterVarunoverlord 2024-02-23 09:33:12 -06:00
parent 1783c2f173
commit 7e0f32149d
6 changed files with 377 additions and 151 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/Intake.java
View File

@ -30,8 +30,8 @@ public class Intake {
}
//Position
public static double stack1 = 0.03;
public static double stack2 = 0.03;
public static double stack1 = 0.015;
public static double stack2 = 0.026;
public static double stack3 = 0.065;
public static double stack4 = 0.09;
public static double stack5 = 0.1;

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

@ -141,7 +141,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25)));
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -151,9 +151,10 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.5);
//robot.intake.setDcMotor(0.5);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
robot.intake.setDcMotor(0.5);
macroState++;
}
break;
@ -214,6 +215,7 @@ public class BlueBackStageAuto extends AutoBase {
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
@ -224,7 +226,7 @@ public class BlueBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(-1.25)));
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -234,9 +236,9 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
robot.intake.setDcMotor(0.68);
macroState++;
}
break;

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java
View File

@ -21,9 +21,9 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontPreload extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-36, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1_PART_2 = new Pose2d(-32,33.5,Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90));
@ -31,15 +31,15 @@ public class BlueFrontPreload extends AutoBase {
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-46.7, 39.5, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
@ -95,7 +95,7 @@ public class BlueFrontPreload extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.32) {
if (getRuntime() < macroTime + 0.18) {
robot.intake.setDcMotor(-0.23);
}
else{
@ -116,7 +116,7 @@ public class BlueFrontPreload extends AutoBase {
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setDcMotor(0.74);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -136,7 +136,7 @@ public class BlueFrontPreload extends AutoBase {
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.2);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {

234
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java
View File

@ -19,24 +19,27 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
@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(-35, 32.5, Math.toRadians(0)); //THIS ANGLE NEEDS TO BE CHANGED
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-32,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-49, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, 35.9, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 32, Math.toRadians(8));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, 23.5, Math.toRadians(6));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
@ -46,6 +49,10 @@ public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 59.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
@ -69,7 +76,8 @@ public class BlueFrontStageAuto extends AutoBase {
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -87,17 +95,28 @@ public class BlueFrontStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.32) {
robot.intake.setDcMotor(-0.18);
if (getRuntime() < macroTime + 0.18) {
robot.intake.setDcMotor(-0.23);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,2.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setDcMotor(0.74);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -117,11 +136,8 @@ public class BlueFrontStageAuto extends AutoBase {
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.8);
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:
@ -142,24 +158,6 @@ public class BlueFrontStageAuto extends AutoBase {
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
// case 1:
// builder.setTangent(Math.toRadians(90))
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
// break;
// case 3:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
// break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
@ -271,89 +269,93 @@ public class BlueFrontStageAuto extends AutoBase {
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 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.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.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
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 + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 20, 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) {

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

@ -24,30 +24,30 @@ public class RedBackStageAuto extends AutoBase {
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 DROP_2 = new Pose2d(15.6, -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 DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90));
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_PRELOAD_2 = new Pose2d(52.3, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.6, -32.5, Math.toRadians(190));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -29.5, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(192));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(180));//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(27, -10.2, Math.toRadians(190));//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11, Math.toRadians(180));//-36
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11, Math.toRadians(185));
@Override
public void createTrajectories() {
@ -90,7 +90,7 @@ public class RedBackStageAuto extends AutoBase {
case 2:
// intake
if (getRuntime() < macroTime + 0.26) {
robot.intake.setDcMotor(-0.35);
robot.intake.setDcMotor(-0.18);
}
// if intake is done move on
else {
@ -104,7 +104,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
robot.extendMacro(Slides.mini_tier1 -20, getRuntime());
//robot.extendMacro(Slides.mini_tier1 -40, getRuntime());
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
@ -133,6 +133,7 @@ public class RedBackStageAuto extends AutoBase {
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
@ -140,7 +141,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3)));
@ -153,22 +154,23 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.48);
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.54);
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK4);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.5) {
if (getRuntime() > macroTime + 1.2) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
@ -214,6 +216,7 @@ public class RedBackStageAuto extends AutoBase {
case 10:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
@ -221,7 +224,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.5)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8)));
@ -234,22 +237,22 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.74);
robot.intake.setpos(STACK3);
robot.intake.setDcMotor(0.58);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.84);
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.37) {
if (getRuntime() > macroTime + 1) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();

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

@ -0,0 +1,219 @@
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 = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-36, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-32,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,-36.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-46.7, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-48.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -23.5, Math.toRadians(186));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-52, -29.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, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
public static final Pose2d PARK_2 = new Pose2d(58,-58,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.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// 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.18) {
robot.intake.setDcMotor(-0.23);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,-2.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.74);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
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();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
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-70, 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;
//Park
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroState++;
}
break;
case 9:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}