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;
013
014/**
015 * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
016 *
017 * <p>This class is provided by the NewCommands VendorDep
018 */
019public class TrapezoidProfileCommand extends CommandBase {
020  private final TrapezoidProfile m_profile;
021  private final Consumer<State> m_output;
022
023  private final Timer m_timer = new Timer();
024
025  /**
026   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
027   * Output will be piped to the provided consumer function.
028   *
029   * @param profile The motion profile to execute.
030   * @param output The consumer for the profile output.
031   * @param requirements The subsystems required by this command.
032   */
033  public TrapezoidProfileCommand(
034      TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
035    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
036    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
037    addRequirements(requirements);
038  }
039
040  @Override
041  public void initialize() {
042    m_timer.reset();
043    m_timer.start();
044  }
045
046  @Override
047  public void execute() {
048    m_output.accept(m_profile.calculate(m_timer.get()));
049  }
050
051  @Override
052  public void end(boolean interrupted) {
053    m_timer.stop();
054  }
055
056  @Override
057  public boolean isFinished() {
058    return m_timer.hasElapsed(m_profile.totalTime());
059  }
060}