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

Dey juicy

This commit is contained in:
ImposterVarunoverlord 2024-02-04 14:16:26 -06:00
parent 924e33e76a
commit b3352113a3
7 changed files with 546 additions and 154 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/Arm.java
View File

@ -22,9 +22,9 @@ public class Arm {
private double armTempPos;
private double doorPos;
private double wristPos;
public static double ARM_KP = 0.11;
public static double ARM_KP = 0.12;
public static double doorOpenPos = 0.3;
public static double doorOpenPos = 0.36;
public static double doorClosePos = 0.61;
public static double armStart = 0.24;

10
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java
View File

@ -16,7 +16,7 @@ public class Intake {
private Position pos = Position.UP;
public enum Position {
STACK1, STACK2, STACK3, STACK4, STACK5, UP;
STACK1, STACK2, STACK3, STACK4, STACK5, STACK6, UP;
public Position nextPosition() {
int nextOne = (this.ordinal() + 1) % Position.values().length;
@ -35,6 +35,8 @@ public class Intake {
public static double stack3 = 0.065;
public static double stack4 = 0.09;
public static double stack5 = 0.1;
public static double stack6 = 0.13;
public static double up = 0.30;
public static double motorPower = 0;
@ -62,7 +64,11 @@ public class Intake {
} else if (stack == Position.STACK5) {
lServo.setPosition(stack5);
rServo.setPosition(stack5);
} else if (stack == Position.UP) {
} else if (stack == Position.STACK6) {
lServo.setPosition(stack6);
rServo.setPosition(stack6);
}
else if (stack == Position.UP) {
lServo.setPosition(up);
rServo.setPosition(up);
}

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 f = 0.01;
//p was 0.0014
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0018,0.02,0,0.01);
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,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 = 200;
public static int mini_tier1 = 250;
public static int tier1 = 350;
public static int tier2 = 500;
public static int tier3 = 720;

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

@ -20,29 +20,32 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
public class BlueBackStageAuto extends AutoBase {
public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
public static final Pose2d DROP_3M = new Pose2d(14.7, 30, Math.toRadians(-180));
public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(25, 43, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(54.8, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.6, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.2, 39.3, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.4, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.5, 30.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.7, 32, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-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(26, 11.1 , Math.toRadians(-180));//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-54.8, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-55.3, 11.1, Math.toRadians(-180));
@Override
public void createTrajectories() {
@ -85,7 +88,7 @@ public class BlueBackStageAuto extends AutoBase {
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
robot.intake.setDcMotor(-0.18);
robot.intake.setDcMotor(-0.24);
}
// if intake is done move on
else {
@ -129,7 +132,17 @@ public class BlueBackStageAuto extends AutoBase {
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().plus(new Vector2d(0,0)));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5)));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -137,7 +150,7 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.38);
robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
macroState++;
@ -145,7 +158,7 @@ public class BlueBackStageAuto extends AutoBase {
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.38);
robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
@ -154,7 +167,7 @@ public class BlueBackStageAuto extends AutoBase {
case 7:
if (getRuntime() > macroTime + 0.03) {
//robot.intake.setDcMotor(-0.0);
robot.intake.setDcMotor(-0.65);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
@ -201,7 +214,18 @@ public class BlueBackStageAuto extends AutoBase {
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5
)));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -209,23 +233,23 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.38);
robot.intake.setpos(STACK4);
robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//Third and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.38);
robot.intake.setpos(STACK3);
robot.intake.setDcMotor(0.6 8);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.03) {
robot.intake.setDcMotor(0.65);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());

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

@ -0,0 +1,328 @@
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 = "Blue FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-50, 45.5, Math.toRadians(-90));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-33.5, 40.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, 35.3, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.5, 36, Math.toRadians(8));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(8));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, 33.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, 58.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 60.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, 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.Blue);
}
@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.22) {
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
// robot.arm.setDoor(OPEN);
// builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-3.6,1.5)));
//
// robot.intake.setDcMotor(0.44);
// robot.intake.setpos(STACK6);
// macroTime = getRuntime();
builder.lineToLinearHeading(POST_DROP_POS);
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();
// //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:
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 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, 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;
//Stack run 2
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180));
builder.lineToConstantHeading(POST_DROP_POS.vec());
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.7, -1.5)).vec(), Math.toRadians(180));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 8:
robot.resetMacro(0, getRuntime());
robot.arm.setDoor(OPEN);
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 9:
robot.intake.setDcMotor(0.54);
robot.arm.setDoor(OPEN);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 10:
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.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 11:
// if drive is done move on
if (getRuntime() > macroTime + 6.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 12:
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 (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 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

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

@ -19,29 +19,35 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@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.3, -33.5, Math.toRadians(90));
public static final Pose2d DROP_1 = new Pose2d(16, -32.5, Math.toRadians(180));
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 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(50, -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_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_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50, -30, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53.6, -32.5, Math.toRadians(190));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(192));//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(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -10.2, Math.toRadians(190));//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-53, -12.9, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-54, -12.9, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -10.2, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -10.2, Math.toRadians(185));
@Override
public void createTrajectories() {
@ -60,6 +66,7 @@ public class RedBackStageAuto extends AutoBase {
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
builder.lineToLinearHeading(DROP_1M);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
@ -82,8 +89,8 @@ public class RedBackStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.23) {
robot.intake.setDcMotor(-0.18);
if (getRuntime() < macroTime + 0.26) {
robot.intake.setDcMotor(-0.35);
}
// if intake is done move on
else {
@ -97,6 +104,7 @@ public class RedBackStageAuto extends AutoBase {
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
robot.extendMacro(Slides.mini_tier1 -20, getRuntime());
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
@ -111,7 +119,7 @@ public class RedBackStageAuto extends AutoBase {
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1 - 20, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
@ -127,7 +135,17 @@ public class RedBackStageAuto extends AutoBase {
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());
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3)));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -135,7 +153,7 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.54);
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
macroState++;
@ -151,7 +169,8 @@ public class RedBackStageAuto extends AutoBase {
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.5) {
robot.intake.setDcMotor(0.65);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
@ -176,14 +195,14 @@ public class RedBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
robot.extendMacro(Slides.mini_tier1 , getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 9:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
robot.extendMacro(Slides.mini_tier1 , getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
@ -197,7 +216,17 @@ public class RedBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
builder.lineToConstantHeading(STACK_LOCATION_TWO.vec());
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8)));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -205,23 +234,24 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK2);
robot.intake.setDcMotor(0.74);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK1);
robot.intake.setDcMotor(0.84);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0.65);
if (getRuntime() > macroTime + 0.37) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
@ -246,7 +276,7 @@ public class RedBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
macroState++;
}
break;

204
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java
View File

@ -1,11 +1,13 @@
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;
@ -17,7 +19,7 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-50, -45.5, Math.toRadians(90));
public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
@ -29,22 +31,22 @@ public class RedFrontStageAuto extends AutoBase {
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-52, -34.4, Math.toRadians(180));
public static final Pose2d STACK_LOCATION = new Pose2d(-54.5, -33.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, -57.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -57.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -60.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -60.5, Math.toRadians(0));
@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);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
@ -80,15 +82,15 @@ public class RedFrontStageAuto extends AutoBase {
case 2:
// intake
if (getRuntime() < macroTime + 0.22) {
robot.intake.setDcMotor(-0.15);
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(0,-1.5)));
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-2,-1.5)));
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK5);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
@ -96,17 +98,18 @@ public class RedFrontStageAuto extends AutoBase {
// if intake is done move on
break;
case 3:
// if drive is done move on
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
}
break;
case 4:
if (getRuntime() > macroTime + 0.2) {
robot.intake.setDcMotor(0);
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.44);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToLinearHeading(POST_DROP_POS);
@ -136,14 +139,14 @@ public class RedFrontStageAuto extends AutoBase {
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
robot.extendMacro(Slides.mini_tier1, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
@ -156,14 +159,14 @@ public class RedFrontStageAuto extends AutoBase {
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.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(0));
builder.lineToLinearHeading(POST_DROP_POS);
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180));
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-0.5)).vec(), Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
//waits for the robot to fin the trajectory
case 8:
robot.resetMacro(0, getRuntime());
robot.arm.setDoor(OPEN);
@ -184,11 +187,11 @@ public class RedFrontStageAuto extends AutoBase {
case 10:
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0);
robot.intake.setDcMotor(-0.45);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0));
builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
@ -212,7 +215,7 @@ public class RedFrontStageAuto extends AutoBase {
break;
case 11:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
@ -231,82 +234,82 @@ public class RedFrontStageAuto extends AutoBase {
break;
//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.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 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
@ -315,3 +318,4 @@ public class RedFrontStageAuto extends AutoBase {
}
}
}