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

2+5 BROKEN AUTO!!!!!!!!

This commit is contained in:
ImposterVarunoverlord 2024-01-18 20:21:41 -06:00
parent 3e623df8aa
commit 185992deae
5 changed files with 343 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,7 +25,7 @@ public class Slides {
public static int targetMax = 830;
public static int down = 0;
public static int mini_tier1 = 205;
public static int mini_tier1 = 200;
public static int tier1 = 350;
public static int tier2 = 500;
public static int tier3 = 720;

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

@ -2,6 +2,7 @@ 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.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import com.acmerobotics.dashboard.config.Config;
@ -18,26 +19,28 @@ import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@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.7, -32.5, Math.toRadians(90));
public static final Pose2d DROP_2 = new Pose2d(13.7, -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 DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(53.5, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -39.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(180));//187
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_2 = new Pose2d(52, -29, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(50.5, -30, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(180));//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.5, Math.toRadians(180));//-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(-56.8, -12.5, Math.toRadians(180));
public static final Pose2d STACK_LOCATION = new Pose2d(-57.9, -12.4, Math.toRadians(190));
public static final Pose2d STACK_LOCATION_TWO = new Pose2d(-58.5, -12.4, Math.toRadians(190));
@Override
public void createTrajectories() {
@ -131,7 +134,7 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
macroState++;
@ -139,8 +142,8 @@ public class RedBackStageAuto extends AutoBase {
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK4);
macroTime = getRuntime();
macroState++;
break;
@ -192,9 +195,8 @@ public class RedBackStageAuto extends AutoBase {
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
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());
builder.lineToConstantHeading(STACK_LOCATION_TWO.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
@ -202,22 +204,22 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.44);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
//3rd and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK3);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.5) {
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);

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

@ -0,0 +1,322 @@
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.STACK4;
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.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Arm;
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 FrontStage Auto(2+5)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontStageAuto extends AutoBase {
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 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_2 = new Pose2d(50.5, -30, Math.toRadians(188));//187
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_TWO = new Pose2d(-58.5, -36.4, Math.toRadians(190));
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 PRE_DEPOSIT_POS = new Pose2d(51, -51.5, Math.toRadians(180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, -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_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.28) {
robot.intake.setDcMotor(-0.22);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
builder.lineToLinearHeading(POST_DROP_POS);
builder.lineToLinearHeading(STACK_LOCATION);
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++;
}
case 4:
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
case 5:
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_DROP_POS.vec());
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 6:
// if drive is done move on
if (getRuntime() > macroTime + 1.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 7:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Stack run 2
case 8:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(POST_DROP_POS);
builder.lineToLinearHeading(STACK_LOCATION_TWO);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
}
//waits for the robot to fin the trajectory
case 9:
robot.resetMacro(0, getRuntime());
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 10:
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 11:
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_DROP_POS.vec());
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 12:
// if drive is done move on
if (getRuntime() > macroTime + 1.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 13:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//stack run 3
case 14:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(POST_DROP_POS);
builder.lineToLinearHeading(STACK_LOCATION_THREE);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
}
//waits for the robot to fin the trajectory
case 15:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK4);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 16:
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 17:
if (getRuntime() > macroTime + 0.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_DROP_POS.vec());
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 18:
// if drive is done move on
if (getRuntime() > macroTime + 1.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 19:
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 20:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

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

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

64
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/vannobAuto_RightRed.java
View File

@ -1,64 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@Autonomous (name= "varun_is_Dumb")
public class vannobAuto_RightRed extends LinearOpMode {
public Robot robot;
int teamElementLocation = 2;
int macrostate = 0;
@Override
public void runOpMode() throws InterruptedException {
robot = new Robot(hardwareMap);
// make trajectories
while (!(isStarted() || isStopRequested())) {
double currentRuntime = getRuntime();
robot.update(currentRuntime);
// int newLocation = robot.camera.getMarkerId();
// if (newLocation != -1) {
// teamElementLocation = newLocation;
// }
telemetry.addLine("Initialized");
telemetry.addLine("Randomization of T.O.M: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
while(macrostate < 6 || !isStopRequested()){
//update everything
robot.update(getRuntime());
switch (macrostate){
case (0):
//follow trajectory
macrostate++;
break;
case (1):
// run intake
macrostate++;
break;
case (2):
macrostate++;
break;
case (3):
macrostate++;
break;
case (4):
macrostate++;
break;
case (5):
macrostate++;
break;
}
}
}
}