[choreolib] Added more triggers

choreolib/src/main/java/choreo/auto/AutoRoutine.java

@@ -43,6 +43,9 @@ public class AutoRoutine {
   /** A boolean utilized in {@link #active()} to resolve trueness */
   private boolean isActive = false;
 
+  /** A boolean indicating if a trajectory is running on the routine right now */
+  private boolean isIdle = true;
+
   /** A boolean that is true when the loop is killed */
   boolean isKilled = false;
 
@@ -114,6 +117,15 @@ int pollCount() {
     return pollCount;
   }
 
+  /**
+   * Updates the idle state of the routine.
+   *
+   * @param isIdle The new idle state of the routine.
+   */
+  void updateIdle(boolean isIdle) {
+    this.isIdle = isIdle;
+  }
+
   /**
    * Resets the routine. This can either be called on auto init or auto end to reset the routine
    * incase you run it again. If this is called on a routine that doesn't need to be reset it will
@@ -135,6 +147,17 @@ public void kill() {
     isKilled = true;
   }
 
+  /**
+   * Creates a trigger that is true when the routine is idle.
+   *
+   * <p>Idle is defined as no trajectories made by the routine are running.
+   *
+   * @return A trigger that is true when the routine is idle.
+   */
+  public Trigger idle() {
+    return new Trigger(loop, () -> isIdle);
+  }
+
   /**
    * Creates a new {@link AutoTrajectory} to be used in an auto routine.
    *
@@ -188,16 +211,34 @@ public Trigger anyDone(AutoTrajectory trajectory, AutoTrajectory... trajectories
    * @param trajectory The first trajectory to watch.
    * @param trajectories The other trajectories to watch
    * @return a trigger that determines if any of the trajectories are finished
+   * @see AutoTrajectory#doneDelayed(int)
    */
-  public Trigger anyDone(
+  public Trigger anyDoneDelayed(
       int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) {
-    var trigger = trajectory.done(cyclesToDelay);
+    var trigger = trajectory.doneDelayed(cyclesToDelay);
     for (int i = 0; i < trajectories.length; i++) {
-      trigger = trigger.or(trajectories[i].done(cyclesToDelay));
+      trigger = trigger.or(trajectories[i].doneDelayed(cyclesToDelay));
     }
     return trigger.and(this.active());
   }
 
+  /**
+   * Creates a trigger that produces a rising edge when any of the trajectories are finished.
+   *
+   * @param cyclesToDelay The number of cycles to delay.
+   * @param trajectory The first trajectory to watch.
+   * @param trajectories The other trajectories to watch
+   * @return a trigger that determines if any of the trajectories are finished
+   * @see AutoTrajectory#doneDelayed(int)
+   * @see AutoRoutine#anyDoneDelayed
+   * @deprecated This method is deprecated and will be removed in 2025. Use {@link #anyDoneDelayed}
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public Trigger anyDone(
+      int cyclesToDelay, AutoTrajectory trajectory, AutoTrajectory... trajectories) {
+    return anyDoneDelayed(cyclesToDelay, trajectory, trajectories);
+  }
+
   /**
    * Creates a trigger that returns true when any of the trajectories given are active.
    *
@@ -213,6 +254,21 @@ public Trigger anyActive(AutoTrajectory trajectory, AutoTrajectory... trajectori
     return trigger.and(this.active());
   }
 
+  /**
+   * Creates a trigger that returns true when any of the trajectories given are inactive.
+   *
+   * @param trajectory The first trajectory to watch.
+   * @param trajectories The other trajectories to watch
+   * @return a trigger that determines if any of the trajectories are inactive
+   */
+  public Trigger anyInactive(AutoTrajectory trajectory, AutoTrajectory... trajectories) {
+    var trigger = trajectory.inactive();
+    for (int i = 0; i < trajectories.length; i++) {
+      trigger = trigger.or(trajectories[i].inactive());
+    }
+    return trigger.and(this.active());
+  }
+
   /**
    * Creates a command that will poll this event loop and reset it when it is cancelled.
    *

choreolib/src/main/java/choreo/auto/AutoTrajectory.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import java.util.Optional;
@@ -145,6 +146,7 @@ private void cmdInitialize() {
     isActive = true;
     isCompleted = false;
     logTrajectory(true);
+    routine.updateIdle(false);
   }
 
   @SuppressWarnings("unchecked")
@@ -173,6 +175,7 @@ private void cmdEnd(boolean interrupted) {
     isCompleted = !interrupted;
     cmdExecute(); // will force the controller to be fed the final sample
     logTrajectory(false);
+    routine.updateIdle(true);
   }
 
   private boolean cmdIsFinished() {
@@ -199,6 +202,18 @@ public Command cmd() {
         .withName("Trajectory_" + name);
   }
 
+  /**
+   * Creates a command that will schedule <b>another</b> command that will follow the trajectory.
+   *
+   * <p>This can be useful when putting {@link AutoTrajectory} commands in sequences that require
+   * subsystems that the AutoTrajectory triggers require.
+   *
+   * @return The command that will schedule the trajectory following command.
+   */
+  public Command spawnCmd() {
+    return new ScheduleCommand(cmd()).withName("Trajectory_" + name + "_Spawner");
+  }
+
   /**
    * Creates a command that resets the robot's odometry to the start of this trajectory.
    *
@@ -293,7 +308,22 @@ public Trigger inactive() {
   }
 
   /**
-   * Returns a trigger that rises to true when the trajectory ends and falls after one pulse.
+   * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls
+   * after one pulse.
+   *
+   * @param cycles The number of cycles to delay the trigger from rising to true.
+   * @return A trigger that is true when the trajectory is finished.
+   * @see #doneDelayed(int)
+   * @deprecated Use {@link #doneDelayed(int)} instead.
+   */
+  @Deprecated(forRemoval = true, since = "2025")
+  public Trigger done(int cycles) {
+    return doneDelayed(cycles);
+  }
+
+  /**
+   * Returns a trigger that rises to true a number of cycles after the trajectory ends and falls
+   * after one pulse.
    *
    * <p>This is different from inactive() in a few ways.
    *
@@ -316,18 +346,18 @@ public Trigger inactive() {
    *
    * routine.enabled().onTrue(rushMidTraj.cmd());
    *
-   * rushMidTraj.done(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd());
-   * rushMidTraj.done(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd());
+   * rushMidTraj.doneDelayed(10).and(noGamepiece).onTrue(pickupAnotherGamepiece.cmd());
+   * rushMidTraj.doneDelayed(10).and(hasGamepiece).onTrue(goShootGamepiece.cmd());
    *
    * // If done never falls when a new trajectory is scheduled
    * // then these triggers leak into the next trajectory, causing the next note pickup
    * // to trigger goShootGamepiece.cmd() even if we no longer care about these checks
    * </code></pre>
    *
-   * @param cyclesToDelay The number of cycles to delay the trigger from rising to true.
+   * @param cycles The number of cycles to delay the trigger from rising to true.
    * @return A trigger that is true when the trajectory is finished.
    */
-  public Trigger done(int cyclesToDelay) {
+  public Trigger doneDelayed(int cycles) {
     BooleanSupplier checker =
         new BooleanSupplier() {
           /** The last used value for trajectory completeness */
@@ -347,7 +377,7 @@ public boolean getAsBoolean() {
               // if just flipped to completed update last seen value
               // and store the cycleTarget based of the current cycle
               lastCompleteness = true;
-              cycleTarget = routine.pollCount() + cyclesToDelay;
+              cycleTarget = routine.pollCount() + cycles;
             }
             // finally if check the cycle matches the target
             return routine.pollCount() == cycleTarget;
@@ -391,7 +421,69 @@ public boolean getAsBoolean() {
    * @return A trigger that is true when the trajectory is finished.
    */
   public Trigger done() {
-    return done(0);
+    return doneDelayed(0);
+  }
+
+  /**
+   * Returns a trigger that stays true for a number of cycles after the trajectory ends.
+   *
+   * @param cycles The number of cycles to stay true after the trajectory ends.
+   * @return A trigger that stays true for a number of cycles after the trajectory ends.
+   */
+  public Trigger doneFor(int cycles) {
+    BooleanSupplier enterPulse = doneDelayed(0);
+    BooleanSupplier exitPulse = doneDelayed(cycles + 1);
+    BooleanSupplier checker =
+        new BooleanSupplier() {
+          boolean output = false;
+
+          @Override
+          public boolean getAsBoolean() {
+            if (enterPulse.getAsBoolean()) {
+              output = true;
+            }
+            if (exitPulse.getAsBoolean()) {
+              output = false;
+            }
+            return output;
+          }
+        };
+    return inactive().and(new Trigger(routine.loop(), checker));
+  }
+
+  /**
+   * Returns a trigger that is true when the trajectory was the last one active and is done.
+   *
+   * @return A trigger that is true when the trajectory was the last one active and is done.
+   */
+  public Trigger recentlyDone() {
+    BooleanSupplier enterPulse = doneDelayed(0);
+    BooleanSupplier exitPulse = routine.idle().negate();
+    BooleanSupplier checker =
+        new BooleanSupplier() {
+          boolean output = false;
+
+          @Override
+          public boolean getAsBoolean() {
+            if (enterPulse.getAsBoolean()) {
+              output = true;
+            }
+            if (exitPulse.getAsBoolean()) {
+              output = false;
+            }
+            return output;
+          }
+        };
+    return inactive().and(new Trigger(routine.loop(), checker));
+  }
+
+  /**
+   * A shorthand for `.done().onTrue(otherTrajectory.cmd())`
+   *
+   * @param otherTrajectory The other trajectory to run when this one is done.
+   */
+  public void chain(AutoTrajectory otherTrajectory) {
+    done().onTrue(otherTrajectory.cmd());
   }
 
   /**

choreolib/src/test/java/choreo/auto/DoneTest.java

@@ -40,7 +40,9 @@ public void testExecution() {
     AutoTrajectory traj = factory.trajectory(trajectory, routine);
 
     BooleanSupplier done = traj.done();
-    BooleanSupplier doneDelayed = traj.done(2);
+    BooleanSupplier doneDelayed = traj.doneDelayed(2);
+    BooleanSupplier doneFor = traj.doneFor(2);
+    BooleanSupplier recentlyDone = traj.recentlyDone();
 
     SimHooks.pauseTiming();
 
@@ -58,6 +60,11 @@ public void testExecution() {
     assertTrue(routine.active().getAsBoolean());
     assertTrue(traj.active());
 
+    assertFalse(done);
+    assertFalse(doneDelayed);
+    assertFalse(doneFor);
+    assertFalse(recentlyDone);
+
     SimHooks.stepTiming(1.0);
     scheduler.run();
     SimHooks.stepTiming(1.0);
@@ -69,13 +76,26 @@ public void testExecution() {
     assertTrue(done);
     assertTrue(done);
     assertFalse(doneDelayed);
+    assertTrue(doneFor);
+    assertTrue(recentlyDone);
+
     scheduler.run();
     assertFalse(done);
     assertFalse(doneDelayed);
+    assertTrue(doneFor);
+    assertTrue(recentlyDone);
+
     scheduler.run();
     assertFalse(done);
     assertTrue(doneDelayed);
     assertTrue(doneDelayed);
+    assertTrue(doneFor);
+    assertTrue(recentlyDone);
+
+    routine.updateIdle(false); // simulating to starting a new trajectory
+    scheduler.run();
+    assertFalse(doneFor);
+    assertFalse(recentlyDone);
 
     SimHooks.resumeTiming();
   }

docs/choreolib/auto-factory.md

@@ -99,6 +99,36 @@ routine.active().onTrue(Commands.print("Started the routine!"));
 
 Trajectories can be loaded using `AutoRoutine.trajectory()`, which will return an `AutoTrajectory` ([Java](/api/choreolib/java/choreo/auto/AutoTrajectory.html)). The `AutoTrajectory` class exposes multiple triggers for you to attach reactive logic to, as well as `AutoTrajectory.cmd()` for scheduling the trajectory.
 
+```java
+public AutoRoutine pickupAndScoreAuto() {
+    AutoRoutine routine = autoFactory.newRoutine("taxi");
+
+    // Load the routine's trajectories
+    AutoTrajectory driveToMiddle = routine.trajectory("driveToMiddle");
+
+    // When the routine begins, reset odometry and start the first trajectory (1)
+    routine.active().onTrue(
+        Commands.sequence(
+            driveToMiddle.resetOdometry(),
+            driveToMiddle.cmd()
+        )
+    );
+
+    return routine;
+}
+```
+
+AuoTrajectories have a variety of triggers that can be used to attach logic to the trajectory, these include:
+
+- `active()` - Triggered when the trajectory is active
+- `inactive()` - Triggered when the trajectory is inactive, equivalent to `active().negate()`
+- `atTime(String)` / `atTime(double)` - Triggered when the trajectory reaches a specific time based on a value or event marker
+- `atPose(String, double, double)` / `atPose(Pose2d, double, double)` - Triggered when the robot reaches a specific pose based on a value or event marker
+- `done()` - Triggered for 1 cycle after the trajectory is finished
+- `doneDelayed(int)` - Triggered for 1 cycle `n` cycles after the trajectory is finished
+- `doneFor(int)` - Triggered for `n` cycles after the trajectory is finished
+- `recentlyDone()` - Triggered after the trajectory is finished until another trajectory is started
+
 ```java
 public AutoRoutine pickupAndScoreAuto() {
     AutoRoutine routine = autoFactory.newRoutine("pickupAndScore");