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