Combining Motion Profiling and PID in Command-Based

הערה

For a description of the WPILib PID control features used by these command-based wrappers, see בקרת PID עם WPILib.

A common FRC® controls solution is to pair a trapezoidal motion profile for setpoint generation with a PID controller for setpoint tracking. To facilitate this, WPILib includes its own ProfiledPIDController class. To further aid teams in integrating this functionality into their robots, the command-based framework contains a convenience wrapper for the ProfiledPIDController class: ProfiledPIDCommand, which integrates the controller into a command.

ProfiledPIDCommand

הערה

In C++, the ProfiledPIDCommand class is templated on the unit type used for distance measurements, which may be angular or linear. The passed-in values must have units consistent with the distance units, or a compile-time error will be thrown. For more information on C++ units, see The C++ Units Library.

The ProfiledPIDCommand class (Java, C++) allows users to easily create commands with a built-in ProfiledPIDController.

Creating a PIDCommand

A ProfiledPIDCommand can be created two ways - by subclassing the ProfiledPIDCommand class, or by defining the command inline. Both methods ultimately extremely similar, and ultimately the choice of which to use comes down to where the user desires that the relevant code be located.

הערה

If subclassing ProfiledPIDCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, control functionality will not work properly.

In either case, a ProfiledPIDCommand is created by passing the necessary parameters to its constructor (if defining a subclass, this can be done with a super() call):

29  /**
30   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
31   * velocity is specified.
32   *
33   * @param controller the controller that controls the output.
34   * @param measurementSource the measurement of the process variable
35   * @param goalSource the controller's goal
36   * @param useOutput the controller's output
37   * @param requirements the subsystems required by this command
38   */
39  public ProfiledPIDCommand(
40      ProfiledPIDController controller,
41      DoubleSupplier measurementSource,
42      Supplier<State> goalSource,
43      BiConsumer<Double, State> useOutput,
44      Subsystem... requirements) {
38  /**
39   * Creates a new PIDCommand, which controls the given output with a
40   * ProfiledPIDController.
41   *
42   * @param controller        the controller that controls the output.
43   * @param measurementSource the measurement of the process variable
44   * @param goalSource   the controller's goal
45   * @param useOutput         the controller's output
46   * @param requirements      the subsystems required by this command
47   */
48  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
49                     std::function<Distance_t()> measurementSource,
50                     std::function<State()> goalSource,
51                     std::function<void(double, State)> useOutput,
52                     Requirements requirements = {})

controller

The controller parameter is the ProfiledPIDController object that will be used by the command. By passing this in, users can specify the PID gains, the motion profile constraints, and the period for the controller (if the user is using a nonstandard main robot loop period).

When subclassing ProfiledPIDCommand, additional modifications (e.g. enabling continuous input) can be made to the controller in the constructor body by calling getController().

measurementSource

The measurementSource parameter is a function (usually passed as a lambda) that returns the measurement of the process variable.

When subclassing ProfiledPIDCommand, advanced users may further modify the measurement supplier by modifying the class’s m_measurement field.

goalSource

The goalSource parameter is a function (usually passed as a lambda) that returns the current goal state for the mechanism. If only a constant goal is needed, an overload exists that takes a constant goal rather than a supplier. Additionally, if goal velocities are desired to be zero, overloads exist that take a constant distance rather than a full profile state.

When subclassing ProfiledPIDCommand, advanced users may further modify the setpoint supplier by modifying the class’s m_goal field.

useOutput

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint state of the control loop.

When subclassing ProfiledPIDCommand, advanced users may further modify the output consumer by modifying the class’s m_useOutput field.

דרישות

Like all inlineable commands, ProfiledPIDCommand allows the user to specify its subsystem requirements as a constructor parameter.

Full ProfiledPIDCommand Example

What does a ProfiledPIDCommand look like when used in practice? The following examples are from the GyroDriveCommands example project (Java, C++):

 5package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
 6
 7import edu.wpi.first.math.controller.ProfiledPIDController;
 8import edu.wpi.first.math.trajectory.TrapezoidProfile;
 9import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
10import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
11import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
12
13/** A command that will turn the robot to the specified angle using a motion profile. */
14public class TurnToAngleProfiled extends ProfiledPIDCommand {
15  /**
16   * Turns to robot to the specified angle using a motion profile.
17   *
18   * @param targetAngleDegrees The angle to turn to
19   * @param drive The drive subsystem to use
20   */
21  public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
22    super(
23        new ProfiledPIDController(
24            DriveConstants.kTurnP,
25            DriveConstants.kTurnI,
26            DriveConstants.kTurnD,
27            new TrapezoidProfile.Constraints(
28                DriveConstants.kMaxTurnRateDegPerS,
29                DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
30        // Close loop on heading
31        drive::getHeading,
32        // Set reference to target
33        targetAngleDegrees,
34        // Pipe output to turn robot
35        (output, setpoint) -> drive.arcadeDrive(0, output),
36        // Require the drive
37        drive);
38
39    // Set the controller to be continuous (because it is an angle controller)
40    getController().enableContinuousInput(-180, 180);
41    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
42    // setpoint before it is considered as having reached the reference
43    getController()
44        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
45  }
46
47  @Override
48  public boolean isFinished() {
49    // End when the controller is at the reference.
50    return getController().atGoal();
51  }
52}
 5#pragma once
 6
 7#include <frc2/command/CommandHelper.h>
 8#include <frc2/command/ProfiledPIDCommand.h>
 9
10#include "subsystems/DriveSubsystem.h"
11
12/**
13 * A command that will turn the robot to the specified angle using a motion
14 * profile.
15 */
16class TurnToAngleProfiled
17    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
18                                 TurnToAngleProfiled> {
19 public:
20  /**
21   * Turns to robot to the specified angle using a motion profile.
22   *
23   * @param targetAngleDegrees The angle to turn to
24   * @param drive              The drive subsystem to use
25   */
26  TurnToAngleProfiled(units::degree_t targetAngleDegrees,
27                      DriveSubsystem* drive);
28
29  bool IsFinished() override;
30};
 5#include "commands/TurnToAngleProfiled.h"
 6
 7#include <frc/controller/ProfiledPIDController.h>
 8
 9using namespace DriveConstants;
10
11TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
12                                         DriveSubsystem* drive)
13    : CommandHelper{
14          frc::ProfiledPIDController<units::radians>{
15              kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}},
16          // Close loop on heading
17          [drive] { return drive->GetHeading(); },
18          // Set reference to target
19          target,
20          // Pipe output to turn robot
21          [drive](double output, auto setpointState) {
22            drive->ArcadeDrive(0, output);
23          },
24          // Require the drive
25          {drive}} {
26  // Set the controller to be continuous (because it is an angle controller)
27  GetController().EnableContinuousInput(-180_deg, 180_deg);
28  // Set the controller tolerance - the delta tolerance ensures the robot is
29  // stationary at the setpoint before it is considered as having reached the
30  // reference
31  GetController().SetTolerance(kTurnTolerance, kTurnRateTolerance);
32
33  AddRequirements(drive);
34}
35
36bool TurnToAngleProfiled::IsFinished() {
37  return GetController().AtGoal();
38}