made it so each cycle was its own void
This commit is contained in:
@@ -782,6 +782,15 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// initialize path variables here
|
||||||
|
TrajectoryActionBuilder shoot0 = null;
|
||||||
|
TrajectoryActionBuilder pickup1 = null;
|
||||||
|
TrajectoryActionBuilder shoot1 = null;
|
||||||
|
TrajectoryActionBuilder pickup2 = null;
|
||||||
|
TrajectoryActionBuilder shoot2 = null;
|
||||||
|
TrajectoryActionBuilder pickup3 = null;
|
||||||
|
TrajectoryActionBuilder shoot3 = null;
|
||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void runOpMode() throws InterruptedException {
|
public void runOpMode() throws InterruptedException {
|
||||||
|
|
||||||
@@ -813,15 +822,6 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
|
|
||||||
limelightUsed = false;
|
limelightUsed = false;
|
||||||
|
|
||||||
TrajectoryActionBuilder shoot0 = null;
|
|
||||||
TrajectoryActionBuilder pickup1 = null;
|
|
||||||
TrajectoryActionBuilder shoot1 = null;
|
|
||||||
TrajectoryActionBuilder pickup2 = null;
|
|
||||||
TrajectoryActionBuilder shoot2 = null;
|
|
||||||
TrajectoryActionBuilder pickup3 = null;
|
|
||||||
TrajectoryActionBuilder shoot3 = null;
|
|
||||||
|
|
||||||
|
|
||||||
robot.light.setPosition(1);
|
robot.light.setPosition(1);
|
||||||
|
|
||||||
while (opModeInInit()) {
|
while (opModeInInit()) {
|
||||||
@@ -992,6 +992,37 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
|
|
||||||
robot.transfer.setPower(1);
|
robot.transfer.setPower(1);
|
||||||
|
|
||||||
|
startCycle();
|
||||||
|
|
||||||
|
if (ballCycles > 0){
|
||||||
|
cycleStackClose();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ballCycles > 1){
|
||||||
|
cycleStackMiddle();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ballCycles > 2){
|
||||||
|
cycleStackFar();
|
||||||
|
}
|
||||||
|
|
||||||
|
while (opModeIsActive()) {
|
||||||
|
|
||||||
|
drive.updatePoseEstimate();
|
||||||
|
|
||||||
|
teleStart = drive.localizer.getPose();
|
||||||
|
|
||||||
|
flywheel.manageFlywheel(0);
|
||||||
|
|
||||||
|
TELE.addLine("finished");
|
||||||
|
TELE.update();
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void startCycle() {
|
||||||
assert shoot0 != null;
|
assert shoot0 != null;
|
||||||
|
|
||||||
Actions.runBlocking(
|
Actions.runBlocking(
|
||||||
@@ -1013,8 +1044,9 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
Actions.runBlocking(
|
Actions.runBlocking(
|
||||||
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
|
shootAll((int) shoot0Vel, shoot0Time, shoot0SpinSpeedIncrease)
|
||||||
);
|
);
|
||||||
|
}
|
||||||
|
|
||||||
if (ballCycles > 0){
|
void cycleStackClose(){
|
||||||
Actions.runBlocking(
|
Actions.runBlocking(
|
||||||
new ParallelAction(
|
new ParallelAction(
|
||||||
pickup1.build(),
|
pickup1.build(),
|
||||||
@@ -1077,7 +1109,7 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (ballCycles > 1){
|
void cycleStackMiddle(){
|
||||||
Actions.runBlocking(
|
Actions.runBlocking(
|
||||||
new ParallelAction(
|
new ParallelAction(
|
||||||
pickup2.build(),
|
pickup2.build(),
|
||||||
@@ -1135,7 +1167,7 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (ballCycles > 2){
|
void cycleStackFar(){
|
||||||
Actions.runBlocking(
|
Actions.runBlocking(
|
||||||
new ParallelAction(
|
new ParallelAction(
|
||||||
pickup3.build(),
|
pickup3.build(),
|
||||||
@@ -1192,20 +1224,4 @@ public class Auto_LT_Close extends LinearOpMode {
|
|||||||
|
|
||||||
);
|
);
|
||||||
}
|
}
|
||||||
|
|
||||||
while (opModeIsActive()) {
|
|
||||||
|
|
||||||
drive.updatePoseEstimate();
|
|
||||||
|
|
||||||
teleStart = drive.localizer.getPose();
|
|
||||||
|
|
||||||
flywheel.manageFlywheel(0);
|
|
||||||
|
|
||||||
TELE.addLine("finished");
|
|
||||||
TELE.update();
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
Reference in New Issue
Block a user