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 CommandBase {
024  protected final ProfiledPIDController m_controller;
025  protected DoubleSupplier m_measurement;
026  protected Supplier<State> m_goal;
027  protected BiConsumer<Double, State> m_useOutput;
028
029  /**
030   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
031   * velocity is specified.
032   *
033   * @param controller the controller that controls the output.
034   * @param measurementSource the measurement of the process variable
035   * @param goalSource the controller's goal
036   * @param useOutput the controller's output
037   * @param requirements the subsystems required by this command
038   */
039  public ProfiledPIDCommand(
040      ProfiledPIDController controller,
041      DoubleSupplier measurementSource,
042      Supplier<State> goalSource,
043      BiConsumer<Double, State> useOutput,
044      Subsystem... requirements) {
045    requireNonNullParam(controller, "controller", "ProfiledPIDCommand");
046    requireNonNullParam(measurementSource, "measurementSource", "ProfiledPIDCommand");
047    requireNonNullParam(goalSource, "goalSource", "ProfiledPIDCommand");
048    requireNonNullParam(useOutput, "useOutput", "ProfiledPIDCommand");
049
050    m_controller = controller;
051    m_useOutput = useOutput;
052    m_measurement = measurementSource;
053    m_goal = goalSource;
054    m_requirements.addAll(Set.of(requirements));
055  }
056
057  /**
058   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
059   * velocity is implicitly zero.
060   *
061   * @param controller the controller that controls the output.
062   * @param measurementSource the measurement of the process variable
063   * @param goalSource the controller's goal
064   * @param useOutput the controller's output
065   * @param requirements the subsystems required by this command
066   */
067  public ProfiledPIDCommand(
068      ProfiledPIDController controller,
069      DoubleSupplier measurementSource,
070      DoubleSupplier goalSource,
071      BiConsumer<Double, State> useOutput,
072      Subsystem... requirements) {
073    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
074    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
075    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
076    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
077
078    m_controller = controller;
079    m_useOutput = useOutput;
080    m_measurement = measurementSource;
081    m_goal = () -> new State(goalSource.getAsDouble(), 0);
082    m_requirements.addAll(Set.of(requirements));
083  }
084
085  /**
086   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
087   * velocity is specified.
088   *
089   * @param controller the controller that controls the output.
090   * @param measurementSource the measurement of the process variable
091   * @param goal the controller's goal
092   * @param useOutput the controller's output
093   * @param requirements the subsystems required by this command
094   */
095  public ProfiledPIDCommand(
096      ProfiledPIDController controller,
097      DoubleSupplier measurementSource,
098      State goal,
099      BiConsumer<Double, State> useOutput,
100      Subsystem... requirements) {
101    this(controller, measurementSource, () -> goal, useOutput, requirements);
102  }
103
104  /**
105   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
106   * velocity is implicitly zero.
107   *
108   * @param controller the controller that controls the output.
109   * @param measurementSource the measurement of the process variable
110   * @param goal the controller's goal
111   * @param useOutput the controller's output
112   * @param requirements the subsystems required by this command
113   */
114  public ProfiledPIDCommand(
115      ProfiledPIDController controller,
116      DoubleSupplier measurementSource,
117      double goal,
118      BiConsumer<Double, State> useOutput,
119      Subsystem... requirements) {
120    this(controller, measurementSource, () -> goal, useOutput, requirements);
121  }
122
123  @Override
124  public void initialize() {
125    m_controller.reset(m_measurement.getAsDouble());
126  }
127
128  @Override
129  public void execute() {
130    m_useOutput.accept(
131        m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
132        m_controller.getSetpoint());
133  }
134
135  @Override
136  public void end(boolean interrupted) {
137    m_useOutput.accept(0.0, new State());
138  }
139
140  /**
141   * Returns the ProfiledPIDController used by the command.
142   *
143   * @return The ProfiledPIDController
144   */
145  public ProfiledPIDController getController() {
146    return m_controller;
147  }
148}