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