diff --git a/choreolib/src/main/java/choreo/auto/AutoFactory.java b/choreolib/src/main/java/choreo/auto/AutoFactory.java index 651b1e28c6..358b6bbb74 100644 --- a/choreolib/src/main/java/choreo/auto/AutoFactory.java +++ b/choreolib/src/main/java/choreo/auto/AutoFactory.java @@ -33,9 +33,7 @@ */ public class AutoFactory { static final AutoRoutine VOID_ROUTINE = - new AutoRoutine("VOID-ROUTINE") { - private final EventLoop loop = new EventLoop(); - + new AutoRoutine(null, "VOID-ROUTINE", new EventLoop()) { @Override public Command cmd() { return Commands.none().withName("VoidAutoRoutine"); @@ -169,7 +167,6 @@ public > AutoFactory( * * @param name The name of the {@link AutoRoutine}. * @return A new {@link AutoRoutine}. - * @see #voidRoutine */ public AutoRoutine newRoutine(String name) { // Clear cache in simulation to allow a form of "hot-reloading" trajectories @@ -177,7 +174,7 @@ public AutoRoutine newRoutine(String name) { trajectoryCache.clear(); } - return new AutoRoutine(name, this::allianceKnownOrIgnored); + return new AutoRoutine(this, name, this::allianceKnownOrIgnored); } private boolean allianceKnownOrIgnored() { @@ -185,24 +182,12 @@ private boolean allianceKnownOrIgnored() { } /** - * An {@link AutoRoutine} that cannot have any side-effects, it stores no state and does nothing - * when polled. + * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link + * AutoRoutine}. * - * @return A void {@link AutoRoutine}. - * @see #newRoutine - */ - public AutoRoutine voidRoutine() { - return VOID_ROUTINE; - } - - /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. - * - * @param trajectoryName The name of the trajectory to use. - * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new {@link AutoTrajectory}. + * @see AutoRoutine#trajectory(String) */ - public AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { + AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName); Trajectory trajectory; @@ -216,15 +201,12 @@ public AutoTrajectory trajectory(String trajectoryName, AutoRoutine routine) { } /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. + * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link + * AutoRoutine}. * - * @param trajectoryName The name of the trajectory to use. - * @param splitIndex The index of the split trajectory to use. - * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new {@link AutoTrajectory}. + * @see AutoRoutine#trajectory(String, int) */ - public AutoTrajectory trajectory( - String trajectoryName, final int splitIndex, AutoRoutine routine) { + AutoTrajectory trajectory(String trajectoryName, final int splitIndex, AutoRoutine routine) { Optional> optTrajectory = trajectoryCache.loadTrajectory(trajectoryName, splitIndex); Trajectory trajectory; @@ -238,21 +220,19 @@ public AutoTrajectory trajectory( } /** - * Creates a new {@link AutoTrajectory} to be used in an auto routine. + * A package protected method to create a new {@link AutoTrajectory} to be used in an {@link + * AutoRoutine}. * - * @param The type of the trajectory samples. - * @param trajectory The trajectory to use. - * @param routine The {@link AutoRoutine} to register this trajectory under. - * @return A new {@link AutoTrajectory}. + * @see AutoRoutine#trajectory(Trajectory) */ @SuppressWarnings("unchecked") - public > AutoTrajectory trajectory( - Trajectory trajectory, AutoRoutine routine) { + > AutoTrajectory trajectory( + Trajectory trajectory, AutoRoutine routine) { // type solidify everything - final Trajectory solidTrajectory = trajectory; - final Consumer solidController = (Consumer) this.controller; - final Optional> solidLogger = - this.trajectoryLogger.map(logger -> (TrajectoryLogger) logger); + final Trajectory solidTrajectory = trajectory; + final Consumer solidController = (Consumer) this.controller; + final Optional> solidLogger = + this.trajectoryLogger.map(logger -> (TrajectoryLogger) logger); return new AutoTrajectory( trajectory.name(), solidTrajectory, diff --git a/choreolib/src/main/java/choreo/auto/AutoRoutine.java b/choreolib/src/main/java/choreo/auto/AutoRoutine.java index ce2f73e67a..0f0e875bb2 100644 --- a/choreolib/src/main/java/choreo/auto/AutoRoutine.java +++ b/choreolib/src/main/java/choreo/auto/AutoRoutine.java @@ -2,6 +2,8 @@ package choreo.auto; +import choreo.trajectory.Trajectory; +import choreo.trajectory.TrajectorySample; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj2.command.Command; @@ -19,6 +21,12 @@ * @see AutoFactory#newRoutine Creating a routine from a AutoFactory */ public class AutoRoutine { + /** + * The factory that created this loop. This is used to create commands that are associated with + * this loop. + */ + protected final AutoFactory factory; + /** The underlying {@link EventLoop} that triggers are bound to and polled */ protected final EventLoop loop; @@ -38,39 +46,41 @@ public class AutoRoutine { protected BooleanSupplier allianceKnownOrIgnored = () -> true; /** - * Creates a new loop with a specific name + * A constructor to be used when inhereting this class to instantiate a custom inner loop * + * @param factory The factory that created this loop * @param name The name of the loop - * @see AutoFactory#newRoutine Creating a loop from a AutoFactory + * @param loop The inner {@link EventLoop} */ - public AutoRoutine(String name) { - this.loop = new EventLoop(); + protected AutoRoutine(AutoFactory factory, String name, EventLoop loop) { + this.factory = factory; + this.loop = loop; this.name = name; } /** - * Creates a new loop with a specific name and a custom alliance supplier. + * Creates a new loop with a specific name * + * @param factory The factory that created this loop * @param name The name of the loop - * @param allianceKnownOrIgnored Returns true if the alliance is known or is irrelevant (i.e. - * flipping is not being done). * @see AutoFactory#newRoutine Creating a loop from a AutoFactory */ - public AutoRoutine(String name, BooleanSupplier allianceKnownOrIgnored) { - this.loop = new EventLoop(); - this.name = name; - this.allianceKnownOrIgnored = allianceKnownOrIgnored; + protected AutoRoutine(AutoFactory factory, String name) { + this(factory, name, new EventLoop()); } /** - * A constructor to be used when inheriting this class to instantiate a custom inner loop + * Creates a new loop with a specific name and a custom alliance supplier. * + * @param factory The factory that created this loop * @param name The name of the loop - * @param loop The inner {@link EventLoop} + * @param allianceKnownOrIgnored Returns true if the alliance is known or is irrelevant (i.e. + * flipping is not being done). + * @see AutoFactory#newRoutine Creating a loop from a AutoFactory */ - protected AutoRoutine(String name, EventLoop loop) { - this.loop = loop; - this.name = name; + protected AutoRoutine(AutoFactory factory, String name, BooleanSupplier allianceKnownOrIgnored) { + this(factory, name); + this.allianceKnownOrIgnored = allianceKnownOrIgnored; } /** @@ -137,6 +147,39 @@ public void kill() { isKilled = true; } + /** + * Creates a new {@link AutoTrajectory} to be used in an auto routine. + * + * @param trajectoryName The name of the trajectory to use. + * @return A new {@link AutoTrajectory}. + */ + public AutoTrajectory trajectory(String trajectoryName) { + return factory.trajectory(trajectoryName, this); + } + + /** + * Creates a new {@link AutoTrajectory} to be used in an auto routine. + * + * @param trajectoryName The name of the trajectory to use. + * @param splitIndex The index of the split trajectory to use. + * @return A new {@link AutoTrajectory}. + */ + public AutoTrajectory trajectory(String trajectoryName, final int splitIndex) { + return factory.trajectory(trajectoryName, splitIndex, this); + } + + /** + * Creates a new {@link AutoTrajectory} to be used in an auto routine. + * + * @param The type of the trajectory samples. + * @param trajectory The trajectory to use. + * @return A new {@link AutoTrajectory}. + */ + public > AutoTrajectory trajectory( + Trajectory trajectory) { + return factory.trajectory(trajectory, this); + } + /** * Creates a command that will poll this event loop and reset it when it is cancelled. * diff --git a/docs/choreolib/auto-factory.md b/docs/choreolib/auto-factory.md index ae499a8b08..56ca80aaa5 100644 --- a/docs/choreolib/auto-factory.md +++ b/docs/choreolib/auto-factory.md @@ -67,7 +67,7 @@ public Robot extends TimedRobot { private AutoRoutine twoPieceAuto(AutoFactory factory) { final AutoRoutine routine = factory.newRoutine("twoPieceAuto"); - final AutoTrajectory trajectory = factory.trajectory("twoPieceAuto", routine); + final AutoTrajectory trajectory = routine.trajectory("twoPieceAuto"); routine.running() .onTrue( diff --git a/docs/choreolib/auto-routines.md b/docs/choreolib/auto-routines.md index f970472652..83c4cdf9af 100644 --- a/docs/choreolib/auto-routines.md +++ b/docs/choreolib/auto-routines.md @@ -82,13 +82,13 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { // AMP, SUB, SRC: The 3 starting positions // Try to load all the trajectories we need - final AutoTrajectory ampToC1 = factory.trajectory("ampToC1", routine); - final AutoTrajectory c1ToM1 = factory.trajectory("c1ToM1", routine); - final AutoTrajectory m1ToS1 = factory.trajectory("m1ToS1", routine); - final AutoTrajectory m1ToM2 = factory.trajectory("m1ToM2", routine); - final AutoTrajectory m2ToS1 = factory.trajectory("m2ToS2", routine); - final AutoTrajectory s1ToC2 = factory.trajectory("s1ToC2", routine); - final AutoTrajectory c2ToC3 = factory.trajectory("c2ToC3", routine); + final AutoTrajectory ampToC1 = routine.trajectory("ampToC1"); + final AutoTrajectory c1ToM1 = routine.trajectory("c1ToM1"); + final AutoTrajectory m1ToS1 = routine.trajectory("m1ToS1"); + final AutoTrajectory m1ToM2 = routine.trajectory("m1ToM2"); + final AutoTrajectory m2ToS1 = routine.trajectory("m2ToS2"); + final AutoTrajectory s1ToC2 = routine.trajectory("s1ToC2"); + final AutoTrajectory c2ToC3 = routine.trajectory("c2ToC3"); // entry point for the auto // resets the odometry to the starting position, @@ -168,7 +168,7 @@ public AutoRoutine fivePieceAutoTriggerSeg(AutoFactory factory) { public Command fivePieceAutoTriggerMono(AutoFactory factory) { final AutoRoutine routine = factory.newRoutine("fivePieceAuto"); final Alert noStartingPoseErr = new Alert("Error: 5 piece auto has no starting pose", AlertType.kError); - final AutoTrajectory trajectory = factory.trajectory("fivePieceAuto", routine); + final AutoTrajectory trajectory = routine.trajectory("fivePieceAuto"); // entry point for the auto // resets the odometry to the starting position, @@ -213,7 +213,8 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) { // value // AMP, SUB, SRC: The 3 starting positions // Try to load all the trajectories we need - final AutoTrajectory ampToC1 = factory.trajectory("ampToC1", factory.voidLoop()); + final Trajectory rawAmpToC1 = factory.cache().loadTrajectory("ampToC1"); + final Command ampToC1 = factory.trajectoryCommand(rawAmpToC1); final Command c1ToM1 = factory.trajectoryCommand("c1ToM1"); final Command m1ToS1 = factory.trajectoryCommand("m1ToS1"); final Command m1ToM2 = factory.trajectoryCommand("m1ToM2"); @@ -222,14 +223,17 @@ public AutoRoutine fivePieceAutoCompositionSeg(AutoFactory factory) { final Command c2ToC3 = factory.trajectoryCommand("c2ToC3"); final Alert noStartingPoseErr = new Alert("Error: 5 piece auto has no starting pose", AlertType.kError); + AtomicBoolean hasInitialPose = new AtomicBoolean(true); + Command ret = sequence( resetOdometry(() -> { - final Optional initialPose = ampToC1.getInitialPose(); + final Optional initialPose = rawAmpToC1.getInitialPose(); if (initialPose.isPresent()) return initialPose.get(); noStartingPoseErr.set(true); - routine.kill(); + hasInitialPose.set(false); return new Pose2d(); }), + waitUntil(hasInitialPose::get), autoAimAndShoot(), deadline( ampToC1.cmd(), intake(), aimFor(()->ampToC1.getFinalPose().orElseGet(Pose2d::new))),