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

ok auto

This commit is contained in:
ImposterVarunoverlord 2024-02-23 21:01:23 -06:00
parent 7e0f32149d
commit 87c5a5df51
2 changed files with 20 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

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

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

@ -31,13 +31,13 @@ public class RedBackStageAuto extends AutoBase {
public static final Pose2d DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90)); public static final Pose2d DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, 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, -32.5, Math.toRadians(180)); public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(52.3, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(51, -40.7, Math.toRadians(180)); public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52, -40, Math.toRadians(180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(180));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, -35.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -29.5, Math.toRadians(180));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -29.5, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(51.4, -34.5, Math.toRadians(180));//187 public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(52.4, -34.5, Math.toRadians(180));//187
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36 //public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
@ -139,12 +139,15 @@ public class RedBackStageAuto extends AutoBase {
switch (teamPropLocation) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85))); builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85)));
robot.intake.setpos(STACK5);
break; break;
case 2: case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1))); builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1)));
robot.intake.setpos(STACK5);
break; break;
case 3: case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-3))); builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -154,7 +157,7 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 5: case 5:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58); robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5); robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
@ -163,19 +166,19 @@ public class RedBackStageAuto extends AutoBase {
break; break;
//First 2 pixels off the stack are intaken by this //First 2 pixels off the stack are intaken by this
case 6: case 6:
robot.intake.setDcMotor(0.58); robot.intake.setDcMotor(0.38);
robot.intake.setpos(STACK4); robot.intake.setpos(STACK4);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//goes back to the easel //goes back to the easel
case 7: case 7:
if (getRuntime() > macroTime + 1.2) { if (getRuntime() > macroTime + 0.6) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
robot.intake.setDcMotor(-0.35);
switch (teamPropLocation) { switch (teamPropLocation) {
case 1: case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
@ -222,12 +225,15 @@ public class RedBackStageAuto extends AutoBase {
switch (teamPropLocation) { switch (teamPropLocation) {
case 1: case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2))); builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK3);
break; break;
case 2: case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3))); builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
robot.intake.setpos(STACK3);
break; break;
case 3: case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8))); builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-1.8)));
robot.intake.setpos(STACK3);
break; break;
} }
this.robot.drive.followTrajectorySequenceAsync(builder.build()); this.robot.drive.followTrajectorySequenceAsync(builder.build());
@ -237,27 +243,27 @@ public class RedBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory //waits for the robot to fin the trajectory
case 11: case 11:
robot.resetMacro(0, getRuntime()); robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK3); robot.intake.setpos(STACK3);
robot.intake.setDcMotor(0.58);
if (!robot.drive.isBusy()) { if (!robot.drive.isBusy()) {
macroState++; macroState++;
} }
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 12: case 12:
robot.intake.setDcMotor(0.58); robot.intake.setDcMotor(0.38);
robot.intake.setpos(STACK2); robot.intake.setpos(STACK2);
macroTime = getRuntime(); macroTime = getRuntime();
macroState++; macroState++;
break; break;
//goes back to the easel //goes back to the easel
case 13: case 13:
if (getRuntime() > macroTime + 1) { if (getRuntime() > macroTime + 0.7) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE); robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder(); builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END); //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec()); builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
robot.intake.setDcMotor(-0.35);
switch (teamPropLocation) { switch (teamPropLocation) {
case 1: case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0)); builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));