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

2+5 broken tho

This commit is contained in:
Varun 2024-01-19 07:51:17 -06:00
parent 185992deae
commit 79332f3266
1 changed files with 32 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -7,10 +7,9 @@ import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d; import com.acmerobotics.roadrunner.geometry.Pose2d;
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; import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@ -18,15 +17,9 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config @Config
@Autonomous(name = "Red FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp") @Autonomous(name = "Red FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontStageAuto extends AutoBase { public class RedFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(12, -32.5, Math.toRadians(0)); public static final Pose2d DROP_1 = new Pose2d(-12, -32.5, Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(13.7, -33.5, Math.toRadians(90)); public static final Pose2d DROP_2 = new Pose2d(-13.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-25, -45.5, Math.toRadians(90));
public static final Pose2d ALINE = new Pose2d(51, -32.5, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(25, -45.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51, -31.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -42, Math.toRadians(180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(50.3, -35.3, Math.toRadians(188));//187
@ -34,24 +27,18 @@ 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 DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(188));//817
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-57.9, -36.4, Math.toRadians(190)); public static final Pose2d STACK_LOCATION = new Pose2d(-57.9, -36.4, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-58.5, -36.4, Math.toRadians(190)); public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d STACK_LOCATION_THREE = new Pose2d(-59.5, -36.4, Math.toRadians(190)); public static final Pose2d POST_DROP_POS = new Pose2d(-51, -51.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS = new Pose2d(51, -51.5, Math.toRadians(180)); public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(-51, -51.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(51, -51.5, 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(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start); robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red); robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
} }
@ -94,8 +81,10 @@ public class RedFrontStageAuto extends AutoBase {
else{ else{
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
builder.lineToLinearHeading(POST_DROP_POS); builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(0)));
builder.lineToLinearHeading(STACK_LOCATION); robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
} }
@ -108,19 +97,14 @@ public class RedFrontStageAuto extends AutoBase {
macroState++; macroState++;
} }
case 4:
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
case 5:
case 4:
if (getRuntime() > macroTime + 0.6) { if (getRuntime() > macroTime + 0.6) {
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_DROP_POS.vec()); builder.splineToSplineHeading(POST_DROP_POS.vec());
switch (teamPropLocation) { switch (teamPropLocation) {
case 1: case 1:
@ -141,7 +125,7 @@ public class RedFrontStageAuto extends AutoBase {
macroTime = getRuntime(); macroTime = getRuntime();
} }
break; break;
case 6: case 5:
// if drive is done move on // if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
@ -151,7 +135,7 @@ public class RedFrontStageAuto extends AutoBase {
} }
break; break;
//extending the macro and about to score //extending the macro and about to score
case 7: case 6:
if (robot.macroState != 0) { if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
} }
@ -162,12 +146,12 @@ public class RedFrontStageAuto extends AutoBase {
break; break;
//Stack run 2 //Stack run 2
case 8: case 7:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(POST_DROP_POS); builder.lineToLinearHeading(POST_DROP_POS);
builder.lineToLinearHeading(STACK_LOCATION_TWO); builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-1.5)));
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
break; break;
@ -175,7 +159,7 @@ public class RedFrontStageAuto extends AutoBase {
} }
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 9: case 8:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.54); robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK4); robot.intake.setpos(STACK4);
@ -184,14 +168,14 @@ public class RedFrontStageAuto extends AutoBase {
} }
break; break;
//3rd and 4th pixels off the stack are in-taken by this //3rd and 4th pixels off the stack are in-taken by this
case 10: case 9:
robot.intake.setDcMotor(0.54); robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK3); robot.intake.setpos(STACK3);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
case 11: case 10:
if (getRuntime() > macroTime + 0.6) { if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
@ -217,7 +201,7 @@ public class RedFrontStageAuto extends AutoBase {
macroTime = getRuntime(); macroTime = getRuntime();
} }
break; break;
case 12: case 11:
// if drive is done move on // if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
@ -227,7 +211,7 @@ public class RedFrontStageAuto extends AutoBase {
} }
break; break;
//extending the macro and about to score //extending the macro and about to score
case 13: case 12:
if (robot.macroState != 0) { if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
} }
@ -238,12 +222,12 @@ public class RedFrontStageAuto extends AutoBase {
break; break;
//stack run 3 //stack run 3
case 14: case 13:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) { if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(POST_DROP_POS); builder.lineToLinearHeading(POST_DROP_POS);
builder.lineToLinearHeading(STACK_LOCATION_THREE); builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-1.5)));
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++; macroState++;
break; break;
@ -251,7 +235,7 @@ public class RedFrontStageAuto extends AutoBase {
} }
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 15: case 14:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.54); robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK4); robot.intake.setpos(STACK4);
@ -260,14 +244,14 @@ public class RedFrontStageAuto extends AutoBase {
} }
break; break;
//3rd and 4th pixels off the stack are intaken by this //3rd and 4th pixels off the stack are intaken by this
case 16: case 15:
robot.intake.setDcMotor(0.54); robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK3); robot.intake.setpos(STACK3);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
case 17: case 16:
if (getRuntime() > macroTime + 0.6) { if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0); robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
@ -293,7 +277,7 @@ public class RedFrontStageAuto extends AutoBase {
macroTime = getRuntime(); macroTime = getRuntime();
} }
break; break;
case 18: case 17:
// if drive is done move on // if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) { if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime(); macroTime = getRuntime();
@ -303,7 +287,7 @@ public class RedFrontStageAuto extends AutoBase {
} }
break; break;
//extending the macro and about to score //extending the macro and about to score
case 19: case 18:
if (robot.macroState != 0) { if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime()); robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
} }
@ -312,7 +296,7 @@ public class RedFrontStageAuto extends AutoBase {
macroState++; macroState++;
} }
break; break;
case 20: case 19:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) { if (robot.macroState == 0) {
macroState = -1; macroState = -1;