Added routine.trajectory() method

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");
@@ -177,7 +175,7 @@ public AutoRoutine newRoutine(String name) {
       trajectoryCache.clear();
     }
 
-    return new AutoRoutine(name, this::allianceKnownOrIgnored);
+    return new AutoRoutine(this, name, this::allianceKnownOrIgnored);
   }
 
   private boolean allianceKnownOrIgnored() {
@@ -196,13 +194,12 @@ public AutoRoutine voidRoutine() {
   }
 
   /**
-   * 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 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<? extends Trajectory<?>> optTrajectory =
         trajectoryCache.loadTrajectory(trajectoryName);
     Trajectory<?> trajectory;
@@ -216,15 +213,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<? extends Trajectory<?>> optTrajectory =
         trajectoryCache.loadTrajectory(trajectoryName, splitIndex);
     Trajectory<?> trajectory;
@@ -238,21 +232,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 <SampleType> 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 <SampleType extends TrajectorySample<SampleType>> AutoTrajectory trajectory(
-      Trajectory<SampleType> trajectory, AutoRoutine routine) {
+  <ST extends TrajectorySample<ST>> AutoTrajectory trajectory(
+      Trajectory<ST> trajectory, AutoRoutine routine) {
     // type solidify everything
-    final Trajectory<SampleType> solidTrajectory = trajectory;
-    final Consumer<SampleType> solidController = (Consumer<SampleType>) this.controller;
-    final Optional<TrajectoryLogger<SampleType>> solidLogger =
-        this.trajectoryLogger.map(logger -> (TrajectoryLogger<SampleType>) logger);
+    final Trajectory<ST> solidTrajectory = trajectory;
+    final Consumer<ST> solidController = (Consumer<ST>) this.controller;
+    final Optional<TrajectoryLogger<ST>> solidLogger =
+        this.trajectoryLogger.map(logger -> (TrajectoryLogger<ST>) logger);
     return new AutoTrajectory(
         trajectory.name(),
         solidTrajectory,

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 inhereting 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 <SampleType> The type of the trajectory samples.
+   * @param trajectory The trajectory to use.
+   * @return A new {@link AutoTrajectory}.
+   */
+  public <SampleType extends TrajectorySample<SampleType>> AutoTrajectory trajectory(
+      Trajectory<SampleType> trajectory) {
+    return factory.trajectory(trajectory, this);
+  }
+
   /**
    * Creates a command that will poll this event loop and reset it when it is cancelled.
    *

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(

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,7 @@ 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 AutoTrajectory ampToC1 = routine.trajectory("ampToC1", factory.voidRoutine());
   final Command c1ToM1 = factory.trajectoryCommand("c1ToM1");
   final Command m1ToS1 = factory.trajectoryCommand("m1ToS1");
   final Command m1ToM2 = factory.trajectoryCommand("m1ToM2");