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