Convert PathSequenceAuto from Builder pattern to procedural code

Team766 · Mar 11, 2024 · d993a57 · d993a57
1 parent a70d43d
commit d993a57
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 36 deletions.
diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java
@@ -5,23 +5,19 @@
 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;
 import com.team766.robot.gatorade.Robot;
 import edu.wpi.first.math.geometry.Pose2d;
-import java.util.LinkedList;
 
-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;
 
     public PathSequenceAuto(Drive drive, Pose2d initialPosition) {
-        pathItems = new LinkedList<RunnableWithContext>();
         this.drive = drive;
         this.controller = createDriveController(drive);
         this.initialPosition = initialPosition;
@@ -65,27 +61,18 @@ private PPHolonomicDriveController createDriveController(Drive drive) {
                 drive.maxWheelDistToCenter());
     }
 
-    protected void add(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 add(Procedure procedure) {
-        pathItems.add(procedure);
-    }
-
-    protected void add(double waitForSeconds) {
-        pathItems.add((context) -> context.waitForSeconds(waitForSeconds));
-    }
+    protected abstract void runSequence(Context context);
 
     @Override
     public final void run(Context context) {
         context.takeOwnership(Robot.drive);
         Robot.drive.resetGyro();
         Robot.drive.setCurrentPosition(initialPosition);
 
-        for (RunnableWithContext pathItem : pathItems) {
-            context.runSync(pathItem);
-            context.yield();
-        }
+        runSequence(context);
     }
 }
diff --git 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)));
-        add(new IntakeIn());
-        add("Intake_Path_1");
-        add(new IntakeIdle());
-        add("Intake_Path_2");
-        add(new IntakeOut());
-        add(new SetCross());
-        add(1);
-        add(new IntakeStop());
-        add("Intake_Path_3");
-        add(new IntakeIn());
-        add("Intake_Path_4");
-        add(new SetCross());
-        add(new IntakeOut());
-        add(2);
-        add(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
@@ -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++) {
-            add("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
@@ -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()));
-        add("RotationTest");
-        add(new SetCross());
+    }
+
+    @Override
+    protected void runSequence(Context context) {
+        runPath(context, "RotationTest");
+        context.runSync(new SetCross());
     }
 }