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

mirror mirror on the wall

This commit is contained in:
ImposterVarunoverlord 2024-01-12 16:56:05 -06:00
parent bdff6587b0
commit 099ce699ef
4 changed files with 150 additions and 53 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/Arm.java
View File

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

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

@ -16,7 +16,8 @@ public class Slides {
// public static double i = 0.02;
// public static double d = 0;
// public static double f = 0.01;
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0014,0.02,0,0.01);
//p was 0.0014
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.002,0.02,0,0.01);
public static double pTolerance = 20;
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
@ -24,7 +25,7 @@ public class Slides {
public static int targetMax = 830;
public static int down = 0;
public static int mini_tier1 = 165;
public static int mini_tier1 = 170;
public static int tier1 = 350;
public static int tier2 = 500;
public static int tier3 = 720;

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

@ -1,7 +1,6 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
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;
@ -13,19 +12,28 @@ import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "Blue Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Blue Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueBackStageAuto 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, 34.5, Math.toRadians(-90));
public static final Pose2d DROP_1 = new Pose2d(12.3, 32.5, Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(13.7, 32.5, Math.toRadians(-90));
public static final Pose2d ALINE = 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, 29, Math.toRadians(0));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.5, 32, Math.toRadians(0));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.5, 35, Math.toRadians(0));
public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, 9.5);//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-58, 12, Math.toRadians(7));
public static final Pose2d DROP_3 = new Pose2d(25, 41.3, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, 27.5, Math.toRadians(1));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, 32.5, Math.toRadians(1));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, 39.5, Math.toRadians(1));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.3, 35.3, Math.toRadians(7));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.3, 29, Math.toRadians(7));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, 32, Math.toRadians(7));
public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, 8.5);//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, 10.6, Math.toRadians(0));
@Override
public void createTrajectories() {
@ -65,14 +73,15 @@ public class BlueBackStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.5) {
robot.intake.setDcMotor(-0.1);
if (getRuntime() < macroTime + 0.28) {
robot.intake.setDcMotor(-0.2);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
@ -119,26 +128,26 @@ public class BlueBackStageAuto extends AutoBase {
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.46);
robot.intake.setpos(STACK4);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
//gose back to the esile
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 1.5) {
if (getRuntime() > macroTime + 1.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec());
break;
case 2:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec());
break;
case 3:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec());
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -150,55 +159,65 @@ public class BlueBackStageAuto extends AutoBase {
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 9:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//scores pixel and goes back to the stack
case 10:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
/* builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//Ensures that the robot has scored the pixel and moves on
case 11:
robot.resetMacro(0, getRuntime());
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//Geting the 2nd and 3rd pixel
//Getting the 2nd and 3rd pixel
case 12:
robot.intake.setDcMotor(0.46);
robot.intake.setDcMotor(0.49);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
//Goes back to the easel
case 13:
if (getRuntime() > macroTime + 1.5) {
if (getRuntime() > macroTime + 1.2) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_1.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec());
break;
case 2:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_2.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec());
break;
case 3:
builder.lineToConstantHeading(DEPOSIT_PRELOAD_3.vec());
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec());
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -206,7 +225,40 @@ public class BlueBackStageAuto extends AutoBase {
}
break;
case 14:
macroState = -1;
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 +20 , getRuntime());
macroState++;
}
break;
//Scoring the pixels
case 15:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 16:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;*/
case 11:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
macroState = -1;
}
// PARK ROBOT
// case 6:
// // reset macro'

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

@ -12,28 +12,28 @@ import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
@Autonomous(name = "Red Backstage Auto", group = "Competition", preselectTeleOp = "Main TeleOp")
@Autonomous(name = "Red Backstage Auto(2+2)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(12, -33.3, Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(12, -33.2, Math.toRadians(90));
public static final Pose2d DROP_1 = new Pose2d(12.3, -32.5, Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(13.7, -32.5, Math.toRadians(90));
public static final Pose2d ALINE = new Pose2d(12,-37.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(12, -33.2, Math.toRadians(0));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52.2, -27.5, Math.toRadians(181));
public static final Pose2d DROP_3 = new Pose2d(25, -41.3, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -27.5, Math.toRadians(181));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52, -32.5, Math.toRadians(181));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.2, -35.5, Math.toRadians(181));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51.3, -39.5, Math.toRadians(181));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.2, -35, Math.toRadians(187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(51.3, -35.3, Math.toRadians(187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.2, -29, Math.toRadians(187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.3, -29, Math.toRadians(187));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.2, -29, Math.toRadians(187));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(50.6, -32, Math.toRadians(187));
public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(12, -8.5);//-36
public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d STACK_LOCATION = new Pose2d(-56.6, -12, Math.toRadians(180));
public static final Pose2d STACK_LOCATION = new Pose2d(-57.4, -10.6, Math.toRadians(180));
@Override
public void createTrajectories() {
@ -73,14 +73,15 @@ public class RedBackStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.3) {
robot.intake.setDcMotor(-0.21);
if (getRuntime() < macroTime + 0.28) {
robot.intake.setDcMotor(-0.2);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
@ -127,14 +128,14 @@ public class RedBackStageAuto extends AutoBase {
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.49);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
//gose back to the esile
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 1.2) {
if (getRuntime() > macroTime + 1.6) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
@ -158,42 +159,52 @@ public class RedBackStageAuto extends AutoBase {
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 9:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//scores pixel and goes back to the stack
case 10:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
/* builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(STACK_LOCATION.vec());
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//Ensures that the robot has scored the pixel and moves on
case 11:
robot.resetMacro(0, getRuntime());
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//Geting the 2nd and 3rd pixel
//Getting the 2nd and 3rd pixel
case 12:
robot.intake.setDcMotor(0.49);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
//Goes back to the easel
case 13:
if (getRuntime() > macroTime + 1.2) {
builder = this.robot.getTrajectorySequenceBuilder();
@ -214,7 +225,40 @@ public class RedBackStageAuto extends AutoBase {
}
break;
case 14:
macroState = -1;
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 +20 , getRuntime());
macroState++;
}
break;
//Scoring the pixels
case 15:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 20 , getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 16:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;*/
case 11:
robot.resetMacro(0, getRuntime());
if (robot.macroState >= 4) {
macroState = -1;
}
// PARK ROBOT
// case 6:
// // reset macro'