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

Merge remote-tracking branch 'origin/optimus_dev' into optimus_dev 

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
This commit is contained in:
Justin 2024-01-10 18:23:44 -06:00
parent a3800ee616 0a4f3636b4
commit 67319031db
1 changed files with 55 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,12 +1,11 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
@ -18,30 +17,11 @@ public class RedBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(12, -37.5, Math.toRadians(90));
public static final Pose2d DROP_2 = new Pose2d(12, -37.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(12, -37.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.5, -32, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.5, -32, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.5, -32, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(53, -29, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(53, -32, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53, -35, Math.toRadians(180));
public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -10.5);//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-56, -11, Math.toRadians(180));
public Trajectory scorePurple1;
public Trajectory scorePurple2;
public Trajectory scorePurple3;
public Trajectory scoreYellow1;
public Trajectory scoreYellow2;
public Trajectory scoreYellow3;
public Trajectory parkRobot1;
public Trajectory parkRobot2;
public Trajectory parkRobot3;
public Trajectory stackrun1b1;
public Trajectory stackrun1b2;
public Trajectory stackrun1b3;
public Trajectory returnstackrun1b1;
public Trajectory returnstackrun1b2;
public Trajectory returnstackrun1b3;
public static final Pose2d STACK_LOCATION = new Pose2d(-56, -14, Math.toRadians(180));
@Override
public void createTrajectories() {
@ -56,7 +36,7 @@ public class RedBackStageAuto extends AutoBase {
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch(teamPropLocation) {
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
break;
@ -90,7 +70,7 @@ public class RedBackStageAuto extends AutoBase {
robot.intake.setDcMotor(0);
robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder();
switch(teamPropLocation) {
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
@ -107,7 +87,6 @@ public class RedBackStageAuto extends AutoBase {
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
@ -128,24 +107,26 @@ public class RedBackStageAuto extends AutoBase {
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
if(!robot.drive.isBusy()){
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.46);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState ++;
macroState++;
break;
//gose back to the esile
case 7:
if (getRuntime() > macroTime + 1.5) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
switch(teamPropLocation) {
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0);
break;
@ -161,17 +142,57 @@ public class RedBackStageAuto extends AutoBase {
}
break;
case 9:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 10:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(POST_SCORING_SPLINE_END, 0);
builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 11:
robot.resetMacro(0, getRuntime());
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//Geting the 2nd and 3rd pixel
case 12:
robot.intake.setDcMotor(0.46);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 13:
if (getRuntime() > macroTime + 1.5) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_PRELOAD_1.vec(), 0);
break;
case 2:
builder.splineToConstantHeading(DEPOSIT_PRELOAD_2.vec(), 0);
break;
case 3:
builder.splineToConstantHeading(DEPOSIT_PRELOAD_3.vec(), 0);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 14:
macroState = -1;
// PARK ROBOT
// case 6: