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