From d993a571ef9aa87e6a8b81ce0c040ed037e58511 Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Sun, 10 Mar 2024 23:11:38 -0700 Subject: [PATCH] Convert PathSequenceAuto from Builder pattern to procedural code --- .../common/procedures/PathSequenceAuto.java | 23 +++--------- .../robot/gatorade/procedures/IntakeAuto.java | 35 +++++++++++-------- .../robot/gatorade/procedures/LoopAuto.java | 7 +++- .../gatorade/procedures/TestPathAuto.java | 9 +++-- 4 files changed, 38 insertions(+), 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 index f3596729d..76559aae9 100644 --- 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 pathItems; private final Drive drive; private final Pose2d initialPosition; private final PPHolonomicDriveController controller; public PathSequenceAuto(Drive drive, Pose2d initialPosition) { - pathItems = new LinkedList(); this.drive = drive; this.controller = createDriveController(drive); this.initialPosition = initialPosition; @@ -65,17 +61,11 @@ 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) { @@ -83,9 +73,6 @@ public final void run(Context context) { 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 index 7b93ed67d..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))); - 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 index df16e9d06..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++) { - 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 index 05bcd18f5..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())); - add("RotationTest"); - add(new SetCross()); + } + + @Override + protected void runSequence(Context context) { + runPath(context, "RotationTest"); + context.runSync(new SetCross()); } }