Add TrapezoidProfileCommand (#1962)

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java

@@ -80,15 +80,6 @@ public class PIDCommand extends CommandBase {
     useOutput(0);
   }
 
-  /**
-   * Sets the function that uses the output of the PIDController.
-   *
-   * @param useOutput The function that uses the output.
-   */
-  public final void setOutput(DoubleConsumer useOutput) {
-    m_useOutput = useOutput;
-  }
-
   /**
    * Returns the PIDController used by the command.
    *
@@ -98,34 +89,6 @@ public class PIDCommand extends CommandBase {
     return m_controller;
   }
 
-  /**
-   * Sets the setpoint for the controller to track the given source.
-   *
-   * @param setpointSource The setpoint source
-   */
-  public void setSetpoint(DoubleSupplier setpointSource) {
-    m_setpoint = setpointSource;
-  }
-
-  /**
-   * Sets the setpoint for the controller to a constant value.
-   *
-   * @param setpoint The setpoint
-   */
-  public void setSetpoint(double setpoint) {
-    setSetpoint(() -> setpoint);
-  }
-
-  /**
-   * Sets the setpoint for the controller to a constant value relative (i.e. added to) the current
-   * setpoint.
-   *
-   * @param relativeReference The change in setpoint
-   */
-  public void setSetpointRelative(double relativeReference) {
-    setSetpoint(m_controller.getSetpoint() + relativeReference);
-  }
-
   /**
    * Gets the setpoint for the controller.  Wraps the passed-in function for readability.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java

@@ -0,0 +1,164 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.Set;
+import java.util.function.BiConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that controls an output with a {@link ProfiledPIDController}.  Runs forever by
+ * default - to add
+ * exit conditions and/or other behavior, subclass this class.  The controller calculation and
+ * output are performed synchronously in the command's execute() method.
+ */
+public class ProfiledPIDCommand extends CommandBase {
+  protected final ProfiledPIDController m_controller;
+  protected DoubleSupplier m_measurement;
+  protected Supplier<State> m_goal;
+  protected BiConsumer<Double, State> m_useOutput;
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+   * Goal velocity is specified.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource        the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+    m_controller = controller;
+    m_useOutput = useOutput;
+    m_measurement = measurementSource;
+    m_goal = goalSource;
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
+   * Goal velocity is implicitly zero.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goalSource        the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
+    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
+    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
+    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");
+
+    m_controller = controller;
+    m_useOutput = useOutput;
+    m_measurement = measurementSource;
+    m_goal = () -> new State(goalSource.getAsDouble(), 0);
+    m_requirements.addAll(Set.of(requirements));
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is specified.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal              the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            State goal, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    this(controller, measurementSource, () -> goal, useOutput, requirements);
+  }
+
+  /**
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is implicitly zero.
+   *
+   * @param controller        the controller that controls the output.
+   * @param measurementSource the measurement of the process variable
+   * @param goal              the controller's goal
+   * @param useOutput         the controller's output
+   * @param requirements      the subsystems required by this command
+   */
+  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
+                            double goal, BiConsumer<Double, State> useOutput,
+                            Subsystem... requirements) {
+    this(controller, measurementSource, () -> goal, useOutput, requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_controller.reset();
+  }
+
+  @Override
+  public void execute() {
+    useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    useOutput(0, new State());
+  }
+
+  /**
+   * Returns the ProfiledPIDController used by the command.
+   *
+   * @return The ProfiledPIDController
+   */
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Gets the goal for the controller.  Wraps the passed-in function for readability.
+   *
+   * @return The goal for the controller
+   */
+  private State getGoal() {
+    return m_goal.get();
+  }
+
+  /**
+   * Gets the measurement of the process variable. Wraps the passed-in function for readability.
+   *
+   * @return The measurement of the process variable
+   */
+  private double getMeasurement() {
+    return m_measurement.getAsDouble();
+  }
+
+  /**
+   * Uses the output of the controller.  Wraps the passed-in function for readability.
+   *
+   * @param output The output value to use
+   */
+  private void useOutput(double output, State state) {
+    m_useOutput.accept(output, state);
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java

@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A subsystem that uses a {@link ProfiledPIDController} to control an output.  The controller is
+ * run synchronously from the subsystem's periodic() method.
+ */
+public abstract class ProfiledPIDSubsystem extends SubsystemBase {
+  protected final ProfiledPIDController m_controller;
+  protected boolean m_enabled;
+
+  /**
+   * Creates a new ProfiledPIDSubsystem.
+   *
+   * @param controller the ProfiledPIDController to use
+   */
+  public ProfiledPIDSubsystem(ProfiledPIDController controller) {
+    requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
+    m_controller = controller;
+  }
+
+  @Override
+  public void periodic() {
+    if (m_enabled) {
+      useOutput(m_controller.calculate(getMeasurement(), getGoal()), m_controller.getSetpoint());
+    }
+  }
+
+  public ProfiledPIDController getController() {
+    return m_controller;
+  }
+
+  /**
+   * Uses the output from the ProfiledPIDController.
+   *
+   * @param output the output of the ProfiledPIDController
+   * @param goal   the goal state of the ProfiledPIDController, for feedforward
+   */
+  public abstract void useOutput(double output, State goal);
+
+  /**
+   * Returns the goal used by the ProfiledPIDController.
+   *
+   * @return the goal to be used by the controller
+   */
+  public abstract State getGoal();
+
+  /**
+   * Returns the measurement of the process variable used by the ProfiledPIDController.
+   *
+   * @return the measurement of the process variable
+   */
+  public abstract double getMeasurement();
+
+  /**
+   * Enables the PID control.  Resets the controller.
+   */
+  public void enable() {
+    m_enabled = true;
+    m_controller.reset();
+  }
+
+  /**
+   * Disables the PID control.  Sets output to zero.
+   */
+  public void disable() {
+    m_enabled = false;
+    useOutput(0, new State());
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj2.command;
+
+import java.util.function.Consumer;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+
+import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+/**
+ * A command that runs a {@link TrapezoidProfile}.  Useful for smoothly controlling mechanism
+ * motion.
+ */
+public class TrapezoidProfileCommand extends CommandBase {
+  private final TrapezoidProfile m_profile;
+  private final Consumer<State> m_output;
+
+  private final Timer m_timer = new Timer();
+
+  /**
+   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
+   * Output will be piped to the provided consumer function.
+   *
+   * @param profile      The motion profile to execute.
+   * @param output       The consumer for the profile output.
+   * @param requirements The subsystems required by this command.
+   */
+  public TrapezoidProfileCommand(TrapezoidProfile profile,
+                                 Consumer<State> output,
+                                 Subsystem... requirements) {
+    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
+    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
+    addRequirements(requirements);
+  }
+
+  @Override
+  public void initialize() {
+    m_timer.reset();
+    m_timer.start();
+  }
+
+  @Override
+  public void execute() {
+    m_output.accept(m_profile.calculate(m_timer.get()));
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_timer.stop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return m_timer.hasPeriodPassed(m_profile.totalTime());
+  }
+}