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