001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj2.command;
006
007import static edu.wpi.first.math.trajectory.TrapezoidProfile.State;
008import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
009
010import edu.wpi.first.math.trajectory.TrapezoidProfile;
011import edu.wpi.first.wpilibj.Timer;
012import java.util.function.Consumer;
013import java.util.function.Supplier;
014
015/**
016 * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
017 *
018 * <p>This class is provided by the NewCommands VendorDep
019 */
020public class TrapezoidProfileCommand extends Command {
021  private final TrapezoidProfile m_profile;
022  private final Consumer<State> m_output;
023  private final Supplier<State> m_goal;
024  private final Supplier<State> m_currentState;
025  private final boolean m_newAPI; // TODO: Remove
026  private final Timer m_timer = new Timer();
027
028  /**
029   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
030   * Output will be piped to the provided consumer function.
031   *
032   * @param profile The motion profile to execute.
033   * @param output The consumer for the profile output.
034   * @param goal The supplier for the desired state
035   * @param currentState The current state
036   * @param requirements The subsystems required by this command.
037   */
038  @SuppressWarnings("this-escape")
039  public TrapezoidProfileCommand(
040      TrapezoidProfile profile,
041      Consumer<State> output,
042      Supplier<State> goal,
043      Supplier<State> currentState,
044      Subsystem... requirements) {
045    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
046    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
047    m_goal = goal;
048    m_currentState = currentState;
049    m_newAPI = true;
050    addRequirements(requirements);
051  }
052
053  /**
054   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
055   * Output will be piped to the provided consumer function.
056   *
057   * @param profile The motion profile to execute.
058   * @param output The consumer for the profile output.
059   * @param requirements The subsystems required by this command.
060   * @deprecated The new constructor allows you to pass in a supplier for desired and current state.
061   *     This allows you to change goals at runtime.
062   */
063  @Deprecated(since = "2024", forRemoval = true)
064  @SuppressWarnings("this-escape")
065  public TrapezoidProfileCommand(
066      TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
067    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
068    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
069    m_newAPI = false;
070    m_goal = null;
071    m_currentState = null;
072    addRequirements(requirements);
073  }
074
075  @Override
076  public void initialize() {
077    m_timer.restart();
078  }
079
080  @Override
081  @SuppressWarnings("removal")
082  public void execute() {
083    if (m_newAPI) {
084      m_output.accept(m_profile.calculate(m_timer.get(), m_currentState.get(), m_goal.get()));
085    } else {
086      m_output.accept(m_profile.calculate(m_timer.get()));
087    }
088  }
089
090  @Override
091  public void end(boolean interrupted) {
092    m_timer.stop();
093  }
094
095  @Override
096  public boolean isFinished() {
097    return m_timer.hasElapsed(m_profile.totalTime());
098  }
099}