(Proposal) Convert PathSequenceAuto from Builder pattern to procedural code

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<RunnableWithContext> 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<RunnableWithContext>();
         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();

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());
     }
 }

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");
         }
     }
 }

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());
     }
 }

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());
     }
 }

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());
     }
 }

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());
     }
 }

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());
     }
 }

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());
     }
 }

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());
     }
 }