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;
010import java.util.function.DoubleConsumer;
011import java.util.function.DoubleSupplier;
012
013/**
014 * A command that controls an output with a {@link PIDController}. Runs forever by default - to add
015 * exit conditions and/or other behavior, subclass this class. The controller calculation and output
016 * are performed synchronously in the command's execute() method.
017 *
018 * <p>This class is provided by the NewCommands VendorDep
019 *
020 * @deprecated Use a PIDController instead
021 */
022@Deprecated(forRemoval = true, since = "2025")
023public class PIDCommand extends Command {
024  /** PID controller. */
025  protected final PIDController m_controller;
026
027  /** Measurement getter. */
028  protected DoubleSupplier m_measurement;
029
030  /** Setpoint getter. */
031  protected DoubleSupplier m_setpoint;
032
033  /** PID controller output consumer. */
034  protected DoubleConsumer m_useOutput;
035
036  /**
037   * Creates a new PIDCommand, which controls the given output with a PIDController.
038   *
039   * @param controller the controller that controls the output.
040   * @param measurementSource the measurement of the process variable
041   * @param setpointSource the controller's setpoint
042   * @param useOutput the controller's output
043   * @param requirements the subsystems required by this command
044   */
045  @SuppressWarnings("this-escape")
046  public PIDCommand(
047      PIDController controller,
048      DoubleSupplier measurementSource,
049      DoubleSupplier setpointSource,
050      DoubleConsumer useOutput,
051      Subsystem... requirements) {
052    requireNonNullParam(controller, "controller", "PIDCommand");
053    requireNonNullParam(measurementSource, "measurementSource", "PIDCommand");
054    requireNonNullParam(setpointSource, "setpointSource", "PIDCommand");
055    requireNonNullParam(useOutput, "useOutput", "PIDCommand");
056
057    m_controller = controller;
058    m_useOutput = useOutput;
059    m_measurement = measurementSource;
060    m_setpoint = setpointSource;
061    addRequirements(requirements);
062  }
063
064  /**
065   * Creates a new PIDCommand, which controls the given output with a PIDController.
066   *
067   * @param controller the controller that controls the output.
068   * @param measurementSource the measurement of the process variable
069   * @param setpoint the controller's setpoint
070   * @param useOutput the controller's output
071   * @param requirements the subsystems required by this command
072   */
073  public PIDCommand(
074      PIDController controller,
075      DoubleSupplier measurementSource,
076      double setpoint,
077      DoubleConsumer useOutput,
078      Subsystem... requirements) {
079    this(controller, measurementSource, () -> setpoint, useOutput, requirements);
080  }
081
082  @Override
083  public void initialize() {
084    m_controller.reset();
085  }
086
087  @Override
088  public void execute() {
089    m_useOutput.accept(
090        m_controller.calculate(m_measurement.getAsDouble(), m_setpoint.getAsDouble()));
091  }
092
093  @Override
094  public void end(boolean interrupted) {
095    m_useOutput.accept(0);
096  }
097
098  /**
099   * Returns the PIDController used by the command.
100   *
101   * @return The PIDController
102   */
103  public PIDController getController() {
104    return m_controller;
105  }
106}