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