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 java.util.Set;
012import java.util.function.BiConsumer;
013import java.util.function.DoubleSupplier;
014import java.util.function.Supplier;
015
016/**
017 * A command that controls an output with a {@link ProfiledPIDController}. Runs forever by default -
018 * to add exit conditions and/or other behavior, subclass this class. The controller calculation and
019 * output are performed synchronously in the command's execute() method.
020 *
021 * <p>This class is provided by the NewCommands VendorDep
022 */
023public class ProfiledPIDCommand extends Command {
024  /** Profiled PID controller. */
025  protected final ProfiledPIDController m_controller;
026
027  /** Measurement getter. */
028  protected DoubleSupplier m_measurement;
029
030  /** Goal getter. */
031  protected Supplier<State> m_goal;
032
033  /** Profiled PID controller output consumer. */
034  protected BiConsumer<Double, State> m_useOutput;
035
036  /**
037   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
038   * velocity is specified.
039   *
040   * @param controller the controller that controls the output.
041   * @param measurementSource the measurement of the process variable
042   * @param goalSource the controller's goal
043   * @param useOutput the controller's output
044   * @param requirements the subsystems required by this command
045   */
046  public ProfiledPIDCommand(
047      ProfiledPIDController controller,
048      DoubleSupplier measurementSource,
049      Supplier<State> goalSource,
050      BiConsumer<Double, State> useOutput,
051      Subsystem... requirements) {
052    requireNonNullParam(controller, "controller", "ProfiledPIDCommand");
053    requireNonNullParam(measurementSource, "measurementSource", "ProfiledPIDCommand");
054    requireNonNullParam(goalSource, "goalSource", "ProfiledPIDCommand");
055    requireNonNullParam(useOutput, "useOutput", "ProfiledPIDCommand");
056
057    m_controller = controller;
058    m_useOutput = useOutput;
059    m_measurement = measurementSource;
060    m_goal = goalSource;
061    m_requirements.addAll(Set.of(requirements));
062  }
063
064  /**
065   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
066   * velocity is implicitly zero.
067   *
068   * @param controller the controller that controls the output.
069   * @param measurementSource the measurement of the process variable
070   * @param goalSource the controller's goal
071   * @param useOutput the controller's output
072   * @param requirements the subsystems required by this command
073   */
074  public ProfiledPIDCommand(
075      ProfiledPIDController controller,
076      DoubleSupplier measurementSource,
077      DoubleSupplier goalSource,
078      BiConsumer<Double, State> useOutput,
079      Subsystem... requirements) {
080    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
081    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
082    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
083    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
084
085    m_controller = controller;
086    m_useOutput = useOutput;
087    m_measurement = measurementSource;
088    m_goal = () -> new State(goalSource.getAsDouble(), 0);
089    m_requirements.addAll(Set.of(requirements));
090  }
091
092  /**
093   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
094   * velocity is specified.
095   *
096   * @param controller the controller that controls the output.
097   * @param measurementSource the measurement of the process variable
098   * @param goal the controller's goal
099   * @param useOutput the controller's output
100   * @param requirements the subsystems required by this command
101   */
102  public ProfiledPIDCommand(
103      ProfiledPIDController controller,
104      DoubleSupplier measurementSource,
105      State goal,
106      BiConsumer<Double, State> useOutput,
107      Subsystem... requirements) {
108    this(controller, measurementSource, () -> goal, useOutput, requirements);
109  }
110
111  /**
112   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
113   * velocity is implicitly zero.
114   *
115   * @param controller the controller that controls the output.
116   * @param measurementSource the measurement of the process variable
117   * @param goal the controller's goal
118   * @param useOutput the controller's output
119   * @param requirements the subsystems required by this command
120   */
121  public ProfiledPIDCommand(
122      ProfiledPIDController controller,
123      DoubleSupplier measurementSource,
124      double goal,
125      BiConsumer<Double, State> useOutput,
126      Subsystem... requirements) {
127    this(controller, measurementSource, () -> goal, useOutput, requirements);
128  }
129
130  @Override
131  public void initialize() {
132    m_controller.reset(m_measurement.getAsDouble());
133  }
134
135  @Override
136  public void execute() {
137    m_useOutput.accept(
138        m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
139        m_controller.getSetpoint());
140  }
141
142  @Override
143  public void end(boolean interrupted) {
144    m_useOutput.accept(0.0, new State());
145  }
146
147  /**
148   * Returns the ProfiledPIDController used by the command.
149   *
150   * @return The ProfiledPIDController
151   */
152  public ProfiledPIDController getController() {
153    return m_controller;
154  }
155}