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.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.math.trajectory.TrapezoidProfile;
010
011/**
012 * A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies
013 * how to use the current state of the motion profile by overriding the `useState` method.
014 *
015 * <p>This class is provided by the NewCommands VendorDep
016 */
017public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
018  private final double m_period;
019  private final TrapezoidProfile.Constraints m_constraints;
020
021  private TrapezoidProfile.State m_state;
022  private TrapezoidProfile.State m_goal;
023
024  private boolean m_enabled = true;
025
026  /**
027   * Creates a new TrapezoidProfileSubsystem.
028   *
029   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
030   * @param initialPosition The initial position of the controlled mechanism when the subsystem is
031   *     constructed.
032   * @param period The period of the main robot loop, in seconds.
033   */
034  public TrapezoidProfileSubsystem(
035      TrapezoidProfile.Constraints constraints, double initialPosition, double period) {
036    m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
037    m_state = new TrapezoidProfile.State(initialPosition, 0);
038    setGoal(initialPosition);
039    m_period = period;
040  }
041
042  /**
043   * Creates a new TrapezoidProfileSubsystem.
044   *
045   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
046   * @param initialPosition The initial position of the controlled mechanism when the subsystem is
047   *     constructed.
048   */
049  public TrapezoidProfileSubsystem(
050      TrapezoidProfile.Constraints constraints, double initialPosition) {
051    this(constraints, initialPosition, 0.02);
052  }
053
054  /**
055   * Creates a new TrapezoidProfileSubsystem.
056   *
057   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
058   */
059  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
060    this(constraints, 0, 0.02);
061  }
062
063  @Override
064  public void periodic() {
065    var profile = new TrapezoidProfile(m_constraints, m_goal, m_state);
066    m_state = profile.calculate(m_period);
067    if (m_enabled) {
068      useState(m_state);
069    }
070  }
071
072  /**
073   * Sets the goal state for the subsystem.
074   *
075   * @param goal The goal state for the subsystem's motion profile.
076   */
077  public void setGoal(TrapezoidProfile.State goal) {
078    m_goal = goal;
079  }
080
081  /**
082   * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
083   *
084   * @param goal The goal position for the subsystem's motion profile.
085   */
086  public void setGoal(double goal) {
087    setGoal(new TrapezoidProfile.State(goal, 0));
088  }
089
090  /** Enable the TrapezoidProfileSubsystem's output. */
091  public void enable() {
092    m_enabled = true;
093  }
094
095  /** Disable the TrapezoidProfileSubsystem's output. */
096  public void disable() {
097    m_enabled = false;
098  }
099
100  /**
101   * Users should override this to consume the current state of the motion profile.
102   *
103   * @param state The current state of the motion profile.
104   */
105  protected abstract void useState(TrapezoidProfile.State state);
106}