diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 486f1d38b..c31f0e616 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -6,7 +6,6 @@ import com.team766.config.ConfigFileReader; import com.team766.framework.Context; import com.team766.framework.Procedure; -import com.team766.framework.RunnableWithContext; import com.team766.robot.common.constants.ConfigConstants; import com.team766.robot.common.constants.PathPlannerConstants; import com.team766.robot.common.mechanisms.Drive; @@ -16,12 +15,10 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import java.util.LinkedList; import java.util.Optional; -public class PathSequenceAuto extends Procedure { +public abstract class PathSequenceAuto extends Procedure { - private final LinkedList pathItems; private final Drive drive; private final Pose2d initialPosition; private final PPHolonomicDriveController controller; @@ -33,7 +30,6 @@ public class PathSequenceAuto extends Procedure { * @param initialPosition Starting position on Blue Alliance in meters (gets flipped when on red) */ public PathSequenceAuto(Drive drive, Pose2d initialPosition) { - pathItems = new LinkedList(); this.drive = drive; this.controller = createDriveController(drive); this.initialPosition = initialPosition; @@ -78,17 +74,11 @@ private PPHolonomicDriveController createDriveController(Drive drive) { drive.maxWheelDistToCenter()); } - protected void addPath(String pathName) { - pathItems.add(new FollowPath(pathName, controller, drive)); + protected void runPath(Context context, String pathName) { + context.runSync(new FollowPath(pathName, controller, drive)); } - protected void addProcedure(Procedure procedure) { - pathItems.add(procedure); - } - - protected void addWait(double waitForSeconds) { - pathItems.add((context) -> context.waitForSeconds(waitForSeconds)); - } + protected abstract void runSequence(Context context); @Override public final void run(Context context) { @@ -115,10 +105,7 @@ public final void run(Context context) { ? GeometryUtil.flipFieldRotation(initialPosition.getRotation()) : initialPosition.getRotation()) .getDegrees()); - for (RunnableWithContext pathItem : pathItems) { - context.runSync(pathItem); - context.yield(); - } + runSequence(context); context.takeOwnership(Robot.shooter); Robot.shooter.stop(); diff --git a/src/main/java/com/team766/robot/gatorade/procedures/IntakeAuto.java b/src/main/java/com/team766/robot/gatorade/procedures/IntakeAuto.java index 5f83c7e94..49b2d840e 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/IntakeAuto.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/IntakeAuto.java @@ -1,5 +1,6 @@ package com.team766.robot.gatorade.procedures; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.gatorade.Robot; import edu.wpi.first.math.geometry.Pose2d; @@ -8,20 +9,24 @@ public class IntakeAuto extends PathSequenceAuto { public IntakeAuto() { super(Robot.drive, new Pose2d(2.00, 7.00, new Rotation2d(0))); - addProcedure(new IntakeIn()); - addPath("Intake_Path_1"); - addProcedure(new IntakeIdle()); - addPath("Intake_Path_2"); - addProcedure(new IntakeOut()); - addProcedure(new SetCross()); - addWait(1); - addProcedure(new IntakeStop()); - addPath("Intake_Path_3"); - addProcedure(new IntakeIn()); - addPath("Intake_Path_4"); - addProcedure(new SetCross()); - addProcedure(new IntakeOut()); - addWait(2); - addProcedure(new IntakeStop()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new IntakeIn()); + runPath(context, "Intake_Path_1"); + context.runSync(new IntakeIdle()); + runPath(context, "Intake_Path_2"); + context.runSync(new IntakeOut()); + context.runSync(new SetCross()); + context.waitForSeconds(1); + context.runSync(new IntakeStop()); + runPath(context, "Intake_Path_3"); + context.runSync(new IntakeIn()); + runPath(context, "Intake_Path_4"); + context.runSync(new SetCross()); + context.runSync(new IntakeOut()); + context.waitForSeconds(2); + context.runSync(new IntakeStop()); } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/LoopAuto.java b/src/main/java/com/team766/robot/gatorade/procedures/LoopAuto.java index f4d21f024..f80b64155 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/LoopAuto.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/LoopAuto.java @@ -1,5 +1,6 @@ package com.team766.robot.gatorade.procedures; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.gatorade.Robot; import edu.wpi.first.math.geometry.Pose2d; @@ -8,8 +9,12 @@ public class LoopAuto extends PathSequenceAuto { public LoopAuto() { super(Robot.drive, new Pose2d(2.00, 7.00, new Rotation2d(0))); + } + + @Override + protected void runSequence(Context context) { for (int i = 0; i < 5; i++) { - addPath("Loop Test"); + runPath(context, "Loop Test"); } } } diff --git a/src/main/java/com/team766/robot/gatorade/procedures/TestPathAuto.java b/src/main/java/com/team766/robot/gatorade/procedures/TestPathAuto.java index b0fdba7ce..9b0de3b7b 100644 --- a/src/main/java/com/team766/robot/gatorade/procedures/TestPathAuto.java +++ b/src/main/java/com/team766/robot/gatorade/procedures/TestPathAuto.java @@ -1,5 +1,6 @@ package com.team766.robot.gatorade.procedures; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.gatorade.Robot; import edu.wpi.first.math.geometry.Pose2d; @@ -9,7 +10,11 @@ public class TestPathAuto extends PathSequenceAuto { public TestPathAuto() { super(Robot.drive, new Pose2d(2.00, 7.00, new Rotation2d())); - addPath("RotationTest"); - addProcedure(new SetCross()); + } + + @Override + protected void runSequence(Context context) { + runPath(context, "RotationTest"); + context.runSync(new SetCross()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java b/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java index a353e66be..923e93e1c 100644 --- a/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import edu.wpi.first.math.geometry.Pose2d; @@ -8,10 +9,14 @@ public class MidfieldAutonSourceSide extends PathSequenceAuto { public MidfieldAutonSourceSide() { super(Robot.drive, new Pose2d(0.71, 4.39, Rotation2d.fromDegrees(-60))); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("MidfieldSource 1"); - addPath("MidfieldSource 2"); - addProcedure(new ShootNow()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "MidfieldSource 1"); + runPath(context, "MidfieldSource 2"); + context.runSync(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/auton_routines/FourPieceAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/auton_routines/FourPieceAmpSide.java index 8a76dc8a9..cf955f0cf 100644 --- a/src/main/java/com/team766/robot/reva/procedures/auton_routines/FourPieceAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/auton_routines/FourPieceAmpSide.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures.auton_routines; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import com.team766.robot.reva.procedures.ShootAtSubwoofer; @@ -11,15 +12,19 @@ public class FourPieceAmpSide extends PathSequenceAuto { public FourPieceAmpSide() { super(Robot.drive, new Pose2d(0.71, 6.72, Rotation2d.fromDegrees(60))); - addProcedure(new ShootAtSubwoofer()); - addProcedure(new StartAutoIntake()); - addPath("Amp Side Start to Top Piece"); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("Fast Top Piece to Middle Piece"); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("Middle Piece to Bottom Piece"); - addProcedure(new ShootNow()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootAtSubwoofer()); + context.runSync(new StartAutoIntake()); + runPath(context, "Amp Side Start to Top Piece"); + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "Fast Top Piece to Middle Piece"); + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "Middle Piece to Bottom Piece"); + context.runSync(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceAmpSide.java index e01d272e1..56c8b161a 100644 --- a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceAmpSide.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures.auton_routines; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import com.team766.robot.reva.procedures.ShootAtSubwoofer; @@ -11,12 +12,16 @@ public class ThreePieceAmpSide extends PathSequenceAuto { public ThreePieceAmpSide() { super(Robot.drive, new Pose2d(0.71, 6.72, Rotation2d.fromDegrees(60))); - addProcedure(new ShootAtSubwoofer()); - addProcedure(new StartAutoIntake()); - addPath("Amp Side Start to Top Piece"); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("Top Piece to Middle Piece"); - addProcedure(new ShootNow()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootAtSubwoofer()); + context.runSync(new StartAutoIntake()); + runPath(context, "Amp Side Start to Top Piece"); + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "Top Piece to Middle Piece"); + context.runSync(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceMidfieldAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceMidfieldAmpSide.java index 1b6a87770..7b906459f 100644 --- a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceMidfieldAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceMidfieldAmpSide.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures.auton_routines; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import com.team766.robot.reva.procedures.ShootAtSubwoofer; @@ -11,12 +12,16 @@ public class ThreePieceMidfieldAmpSide extends PathSequenceAuto { public ThreePieceMidfieldAmpSide() { super(Robot.drive, new Pose2d(0.71, 6.72, Rotation2d.fromDegrees(60))); - addProcedure(new ShootAtSubwoofer()); - addProcedure(new StartAutoIntake()); - addPath("Amp Side Start to Top Piece"); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("Retrieve Top Midfield from Top Piece"); - addProcedure(new ShootNow()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootAtSubwoofer()); + context.runSync(new StartAutoIntake()); + runPath(context, "Amp Side Start to Top Piece"); + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "Retrieve Top Midfield from Top Piece"); + context.runSync(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceStartCenterTopAndAmp.java b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceStartCenterTopAndAmp.java index 236a7f34c..2963506a8 100644 --- a/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceStartCenterTopAndAmp.java +++ b/src/main/java/com/team766/robot/reva/procedures/auton_routines/ThreePieceStartCenterTopAndAmp.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures.auton_routines; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import com.team766.robot.reva.procedures.ShootAtSubwoofer; @@ -11,12 +12,16 @@ public class ThreePieceStartCenterTopAndAmp extends PathSequenceAuto { public ThreePieceStartCenterTopAndAmp() { super(Robot.drive, new Pose2d(1.35, 5.55, Rotation2d.fromDegrees(0))); - addProcedure(new ShootAtSubwoofer()); - addProcedure(new StartAutoIntake()); - addPath("Middle Start to Middle Piece"); - addProcedure(new ShootNow()); - addProcedure(new StartAutoIntake()); - addPath("Middle Piece to Top Piece"); - addProcedure(new ShootNow()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootAtSubwoofer()); + context.runSync(new StartAutoIntake()); + runPath(context, "Middle Start to Middle Piece"); + context.runSync(new ShootNow()); + context.runSync(new StartAutoIntake()); + runPath(context, "Middle Piece to Top Piece"); + context.runSync(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/auton_routines/TwoPieceMidfieldSourceSide.java b/src/main/java/com/team766/robot/reva/procedures/auton_routines/TwoPieceMidfieldSourceSide.java index 80765b5d7..6586a9e21 100644 --- a/src/main/java/com/team766/robot/reva/procedures/auton_routines/TwoPieceMidfieldSourceSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/auton_routines/TwoPieceMidfieldSourceSide.java @@ -1,5 +1,6 @@ package com.team766.robot.reva.procedures.auton_routines; +import com.team766.framework.Context; import com.team766.robot.common.procedures.PathSequenceAuto; import com.team766.robot.reva.Robot; import com.team766.robot.reva.procedures.ShootAtSubwoofer; @@ -10,10 +11,14 @@ public class TwoPieceMidfieldSourceSide extends PathSequenceAuto { public TwoPieceMidfieldSourceSide() { super(Robot.drive, new Pose2d(0.71, 4.40, Rotation2d.fromDegrees(-60))); - addProcedure(new ShootAtSubwoofer()); - addProcedure(new StartAutoIntake()); - addPath("Bottom Start to Bottom Midfield"); // moves to midfield position - addPath("Bottom Midfield to Bottom Start"); // moves to subwoofer scoring position - addProcedure(new ShootAtSubwoofer()); + } + + @Override + protected void runSequence(Context context) { + context.runSync(new ShootAtSubwoofer()); + context.runSync(new StartAutoIntake()); + runPath(context, "Bottom Start to Bottom Midfield"); // moves to midfield position + runPath(context, "Bottom Midfield to Bottom Start"); // moves to subwoofer scoring position + context.runSync(new ShootAtSubwoofer()); } }