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.wpilibj.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 */
019public class PIDCommand extends CommandBase {
020  protected final PIDController m_controller;
021  protected DoubleSupplier m_measurement;
022  protected DoubleSupplier m_setpoint;
023  protected DoubleConsumer m_useOutput;
024
025  /**
026   * Creates a new PIDCommand, which controls the given output with a PIDController.
027   *
028   * @param controller the controller that controls the output.
029   * @param measurementSource the measurement of the process variable
030   * @param setpointSource the controller's setpoint
031   * @param useOutput the controller's output
032   * @param requirements the subsystems required by this command
033   */
034  public PIDCommand(
035      PIDController controller,
036      DoubleSupplier measurementSource,
037      DoubleSupplier setpointSource,
038      DoubleConsumer useOutput,
039      Subsystem... requirements) {
040    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
041    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
042    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
043    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
044
045    m_controller = controller;
046    m_useOutput = useOutput;
047    m_measurement = measurementSource;
048    m_setpoint = setpointSource;
049    m_requirements.addAll(Set.of(requirements));
050  }
051
052  /**
053   * Creates a new PIDCommand, which controls the given output with a PIDController.
054   *
055   * @param controller the controller that controls the output.
056   * @param measurementSource the measurement of the process variable
057   * @param setpoint the controller's setpoint
058   * @param useOutput the controller's output
059   * @param requirements the subsystems required by this command
060   */
061  public PIDCommand(
062      PIDController controller,
063      DoubleSupplier measurementSource,
064      double setpoint,
065      DoubleConsumer useOutput,
066      Subsystem... requirements) {
067    this(controller, measurementSource, () -> setpoint, useOutput, requirements);
068  }
069
070  @Override
071  public void initialize() {
072    m_controller.reset();
073  }
074
075  @Override
076  public void execute() {
077    m_useOutput.accept(
078        m_controller.calculate(m_measurement.getAsDouble(), m_setpoint.getAsDouble()));
079  }
080
081  @Override
082  public void end(boolean interrupted) {
083    m_useOutput.accept(0);
084  }
085
086  /**
087   * Returns the PIDController used by the command.
088   *
089   * @return The PIDController
090   */
091  public PIDController getController() {
092    return m_controller;
093  }
094}