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

Red Work, blue 2 almost dose

This commit is contained in:
ImposterVarunoverlord 2024-02-24 02:07:02 -06:00
parent 87c5a5df51
commit 3b68b39ea9
3 changed files with 44 additions and 36 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/Intake.java
View File

@ -31,7 +31,7 @@ public class Intake {
//Position
public static double stack1 = 0.015;
public static double stack2 = 0.026;
public static double stack2 = 0.029;
public static double stack3 = 0.065;
public static double stack4 = 0.09;
public static double stack5 = 0.1;

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

@ -5,6 +5,7 @@ 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 static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
@ -43,7 +44,7 @@ public class BlueBackStageAuto extends AutoBase {
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-55, 12.1, Math.toRadians(-187));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 12.1, Math.toRadians(-187));
@ -88,7 +89,7 @@ public class BlueBackStageAuto extends AutoBase {
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
if (getRuntime() < macroTime + 0.25) {
robot.intake.setDcMotor(-0.24);
}
// if intake is done move on
@ -151,23 +152,23 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
//robot.intake.setDcMotor(0.5);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
robot.intake.setDcMotor(0.5);
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK6);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
//robot.intake.setDcMotor(0.5);
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.5);
robot.intake.setpos(STACK4);
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.5) {
if (getRuntime() > macroTime + 0.6) {
//robot.intake.setDcMotor(-0.0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
@ -236,22 +237,23 @@ public class BlueBackStageAuto extends AutoBase {
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
robot.intake.setDcMotor(0.68);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
//robot.intake.setDcMotor(0.68);
macroState++;
}
break;
//Third and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.68);
robot.intake.setpos(STACK1);
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.5) {
if (getRuntime() > macroTime + 0.7) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();

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

@ -5,6 +5,7 @@ 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 static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
@ -30,12 +31,12 @@ public class RedBackStageAuto extends AutoBase {
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_2 = new Pose2d(52.3, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52, -40, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.3, -39, 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(52, -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, -30.5, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(52.4, -34.5, Math.toRadians(180));//187
@ -142,7 +143,7 @@ public class RedBackStageAuto extends AutoBase {
robot.intake.setpos(STACK5);
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-1)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break;
case 3:
@ -152,22 +153,24 @@ public class RedBackStageAuto extends AutoBase {
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
if (!robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK6);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.38);
robot.intake.setpos(STACK4);
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
@ -224,34 +227,37 @@ public class RedBackStageAuto extends AutoBase {
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.75)));
robot.intake.setpos(STACK3);
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3)));
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.3)));
robot.intake.setpos(STACK3);
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-1.8)));
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8)));
robot.intake.setpos(STACK3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.38);
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
@ -285,14 +291,14 @@ public class RedBackStageAuto extends AutoBase {
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 15:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());