001// Copyright (c) Choreo contributors
002
003package choreo.auto;
004
005import edu.wpi.first.wpilibj.DriverStation;
006import edu.wpi.first.wpilibj.event.EventLoop;
007import edu.wpi.first.wpilibj2.command.Command;
008import edu.wpi.first.wpilibj2.command.CommandScheduler;
009import edu.wpi.first.wpilibj2.command.Commands;
010import edu.wpi.first.wpilibj2.command.button.Trigger;
011import java.util.function.BooleanSupplier;
012
013/**
014 * An object that represents an autonomous routine.
015 *
016 * <p>This loop is used to handle autonomous trigger logic and schedule commands. This loop should
017 * **not** be shared across multiple autonomous routines.
018 *
019 * @see AutoFactory#newRoutine Creating a routine from a AutoFactory
020 */
021public class AutoRoutine {
022  /** The underlying {@link EventLoop} that triggers are bound to and polled */
023  protected final EventLoop loop;
024
025  /** The name of the auto routine this loop is associated with */
026  protected final String name;
027
028  /** A boolean utilized in {@link #running()} to resolve trueness */
029  protected boolean isActive = false;
030
031  /** A boolean that is true when the loop is killed */
032  protected boolean isKilled = false;
033
034  /** The amount of times the routine has been polled */
035  protected int pollCount = 0;
036
037  /**
038   * Creates a new loop with a specific name
039   *
040   * @param name The name of the loop
041   * @see AutoFactory#newRoutine Creating a loop from a AutoFactory
042   */
043  public AutoRoutine(String name) {
044    this.loop = new EventLoop();
045    this.name = name;
046  }
047
048  /**
049   * A constructor to be used when inhereting this class to instantiate a custom inner loop
050   *
051   * @param name The name of the loop
052   * @param loop The inner {@link EventLoop}
053   */
054  protected AutoRoutine(String name, EventLoop loop) {
055    this.loop = loop;
056    this.name = name;
057  }
058
059  /**
060   * Returns a {@link Trigger} that is true while this autonomous routine is being polled.
061   *
062   * <p>Using a {@link Trigger#onFalse(Command)} will do nothing as when this is false the routine
063   * is not being polled anymore.
064   *
065   * @return A {@link Trigger} that is true while this autonomous routine is being polled.
066   */
067  public Trigger running() {
068    return new Trigger(loop, () -> isActive && DriverStation.isAutonomousEnabled());
069  }
070
071  /** Polls the routine. Should be called in the autonomous periodic method. */
072  public void poll() {
073    if (!DriverStation.isAutonomousEnabled() || isKilled) {
074      isActive = false;
075      return;
076    }
077    pollCount++;
078    loop.poll();
079    isActive = true;
080  }
081
082  /**
083   * Gets the event loop that this routine is using.
084   *
085   * @return The event loop that this routine is using.
086   */
087  public EventLoop loop() {
088    return loop;
089  }
090
091  /**
092   * Gets the poll count of the routine.
093   *
094   * @return The poll count of the routine.
095   */
096  int pollCount() {
097    return pollCount;
098  }
099
100  /**
101   * Resets the routine. This can either be called on auto init or auto end to reset the routine
102   * incase you run it again. If this is called on a routine that doesn't need to be reset it will
103   * do nothing.
104   */
105  public void reset() {
106    pollCount = 0;
107    isActive = false;
108  }
109
110  /** Kills the loop and prevents it from running again. */
111  public void kill() {
112    CommandScheduler.getInstance().cancelAll();
113    if (isKilled) {
114      return;
115    }
116    reset();
117    DriverStation.reportWarning("Killed An Auto Loop", true);
118    isKilled = true;
119  }
120
121  /**
122   * Creates a command that will poll this event loop and reset it when it is cancelled.
123   *
124   * @return A command that will poll this event loop and reset it when it is cancelled.
125   * @see #cmd(BooleanSupplier) A version of this method that takes a condition to finish the loop.
126   */
127  public Command cmd() {
128    return Commands.run(this::poll)
129        .finallyDo(this::reset)
130        .until(() -> !DriverStation.isAutonomousEnabled())
131        .withName(name);
132  }
133
134  /**
135   * Creates a command that will poll this event loop and reset it when it is finished or canceled.
136   *
137   * @param finishCondition A condition that will finish the loop when it is true.
138   * @return A command that will poll this event loop and reset it when it is finished or canceled.
139   * @see #cmd() A version of this method that doesn't take a condition and never finishes.
140   */
141  public Command cmd(BooleanSupplier finishCondition) {
142    return Commands.run(this::poll)
143        .finallyDo(this::reset)
144        .until(() -> !DriverStation.isAutonomousEnabled() || finishCondition.getAsBoolean())
145        .withName(name);
146  }
147}