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.controller.ProfiledPIDController;
011import edu.wpi.first.math.trajectory.TrapezoidProfile;
012
013/**
014 * A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is run
015 * synchronously from the subsystem's periodic() method.
016 *
017 * <p>This class is provided by the NewCommands VendorDep
018 */
019public abstract class ProfiledPIDSubsystem extends SubsystemBase {
020  protected final ProfiledPIDController m_controller;
021  protected boolean m_enabled;
022
023  /**
024   * Creates a new ProfiledPIDSubsystem.
025   *
026   * @param controller the ProfiledPIDController to use
027   * @param initialPosition the initial goal position of the controller
028   */
029  public ProfiledPIDSubsystem(ProfiledPIDController controller, double initialPosition) {
030    m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
031    setGoal(initialPosition);
032  }
033
034  /**
035   * Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
036   *
037   * @param controller the ProfiledPIDController to use
038   */
039  public ProfiledPIDSubsystem(ProfiledPIDController controller) {
040    this(controller, 0);
041  }
042
043  @Override
044  public void periodic() {
045    if (m_enabled) {
046      useOutput(m_controller.calculate(getMeasurement()), m_controller.getSetpoint());
047    }
048  }
049
050  public ProfiledPIDController getController() {
051    return m_controller;
052  }
053
054  /**
055   * Sets the goal state for the subsystem.
056   *
057   * @param goal The goal state for the subsystem's motion profile.
058   */
059  public void setGoal(TrapezoidProfile.State goal) {
060    m_controller.setGoal(goal);
061  }
062
063  /**
064   * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
065   *
066   * @param goal The goal position for the subsystem's motion profile.
067   */
068  public void setGoal(double goal) {
069    setGoal(new TrapezoidProfile.State(goal, 0));
070  }
071
072  /**
073   * Uses the output from the ProfiledPIDController.
074   *
075   * @param output the output of the ProfiledPIDController
076   * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
077   */
078  protected abstract void useOutput(double output, State setpoint);
079
080  /**
081   * Returns the measurement of the process variable used by the ProfiledPIDController.
082   *
083   * @return the measurement of the process variable
084   */
085  protected abstract double getMeasurement();
086
087  /** Enables the PID control. Resets the controller. */
088  public void enable() {
089    m_enabled = true;
090    m_controller.reset(getMeasurement());
091  }
092
093  /** Disables the PID control. Sets output to zero. */
094  public void disable() {
095    m_enabled = false;
096    useOutput(0, new State());
097  }
098
099  /**
100   * Returns whether the controller is enabled.
101   *
102   * @return Whether the controller is enabled.
103   */
104  public boolean isEnabled() {
105    return m_enabled;
106  }
107}