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 m_profile;
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    requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
037    m_profile = new TrapezoidProfile(constraints);
038    m_state = new TrapezoidProfile.State(initialPosition, 0);
039    setGoal(initialPosition);
040    m_period = period;
041  }
042
043  /**
044   * Creates a new TrapezoidProfileSubsystem.
045   *
046   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
047   * @param initialPosition The initial position of the controlled mechanism when the subsystem is
048   *     constructed.
049   */
050  public TrapezoidProfileSubsystem(
051      TrapezoidProfile.Constraints constraints, double initialPosition) {
052    this(constraints, initialPosition, 0.02);
053  }
054
055  /**
056   * Creates a new TrapezoidProfileSubsystem.
057   *
058   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
059   */
060  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
061    this(constraints, 0, 0.02);
062  }
063
064  @Override
065  public void periodic() {
066    m_state = m_profile.calculate(m_period, m_state, m_goal);
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 final 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 final 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}