通过PID子系统和PID指令进行PID控制

备注

有关这些基于指令的程序包使用的WPILib PID控制功能的描述,请参阅WPILib中的:ref:docs/software/advanced-controls/controllers/pidcontroller:PID Control in WPILib

One of the most common control algorithms used in FRC® is the PID controller. WPILib offers its own PIDController class to help teams implement this functionality on their robots. To further help teams integrate PID control into a command-based robot project, the command-based library includes two convenience wrappers for the PIDController class: PIDSubsystem, which integrates the PID controller into a subsystem, and PIDCommand, which integrates the PID controller into a command.

PID子系统

The PIDSubsystem class (Java, C++) allows users to conveniently create a subsystem with a built-in PIDController. In order to use the PIDSubsystem class, users must create a subclass of it.

创建一个PID子系统

备注

If periodic is overridden when inheriting from PIDSubsystem, make sure to call super.periodic()! Otherwise, PID functionality will not work properly.

创建“PIDSubsystem”的子类时,用户必须覆盖其父类的以下两个抽象方法以提供该类将在其常规运算中使用的功能:

getMeasurement()

  protected abstract double getMeasurement();
  virtual double GetMeasurement() = 0;

“getMeasurement”方法返回过程变量的当前测量值。 PIDSubsystem将自动从其“periodic()”块中调用此方法,并将它的值传递给控制循环。

用户应覆盖此方法,以返回他们希望的用作过程变量测量的任何传感器读数。

useOutput()

  protected abstract void useOutput(double output, double setpoint);
  virtual void UseOutput(double output, double setpoint) = 0;

useOutput()方法使用PID控制器的输出和当前设定值(这通常对于计算前馈很有用)。“PIDSubsystem”将自动从其“periodic()”块中调用此方法,并将控制环的计算输出传递给该方法。

用户应覆盖此方法,以将最终计算出的控制输出传递给其子系统的电机。

控制器中的传递

用户还必须通过“PIDController”和“PIDSubsystem”的子类的超类构造函数调用将“PIDController”传递给“PIDSubsystem”基类。这一步骤用于指定PID增益以及周期(如果用户使用的是非标准主机器人循环周期)。

用户还可以通过调用“getController()”在构造函数主体中对控制器进行其他修改(例如启用连续输入)。

使用PID子系统

一旦创建了“PIDSubsystem”子类的实例,就可以在指令中通过以下方法使用它:

setSetpoint()

“setSetpoint()”方法可用于设置“PIDSubsystem”的设定值。子系统将使用已经定义的输出自动跟踪到设定点:

// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.setSetpoint(5);
// The subsystem will track to a setpoint of 5.
examplePIDSubsystem.SetSetpoint(5);

enable()和disable()

“enable()”和“disable()”方法能够启用和禁用“PIDSubsystem”的PID控制。启用子系统后,它将自动运行控制回路并跟踪设定值。禁用时,子系统不执行任何控制。

此外,“enable()”方法能够重置内部的“PIDController”,并且“disable()”方法能够调用用户定义的‘useOutput()’_方法,并将输出值和设定值都设置为“0”。

完整的PID子系统示例

在实践中使用时,“ PIDSubsystem”是什么样的?以下示例取自FrisbeeBot示例项目(Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot>`__,C ++):

 5package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 6
 7import edu.wpi.first.math.controller.PIDController;
 8import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 9import edu.wpi.first.wpilibj.Encoder;
10import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
11import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
12import edu.wpi.first.wpilibj2.command.PIDSubsystem;
13
14public class ShooterSubsystem extends PIDSubsystem {
15  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
16  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
17  private final Encoder m_shooterEncoder =
18      new Encoder(
19          ShooterConstants.kEncoderPorts[0],
20          ShooterConstants.kEncoderPorts[1],
21          ShooterConstants.kEncoderReversed);
22  private final SimpleMotorFeedforward m_shooterFeedforward =
23      new SimpleMotorFeedforward(
24          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
25
26  /** The shooter subsystem for the robot. */
27  public ShooterSubsystem() {
28    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
29    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
30    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
31    setSetpoint(ShooterConstants.kShooterTargetRPS);
32  }
33
34  @Override
35  public void useOutput(double output, double setpoint) {
36    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
37  }
38
39  @Override
40  public double getMeasurement() {
41    return m_shooterEncoder.getRate();
42  }
43
44  public boolean atSetpoint() {
45    return m_controller.atSetpoint();
46  }
47
48  public void runFeeder() {
49    m_feederMotor.set(ShooterConstants.kFeederSpeed);
50  }
51
52  public void stopFeeder() {
53    m_feederMotor.set(0);
54  }
55}
 5#pragma once
 6
 7#include <frc/Encoder.h>
 8#include <frc/controller/SimpleMotorFeedforward.h>
 9#include <frc/motorcontrol/PWMSparkMax.h>
10#include <frc2/command/PIDSubsystem.h>
11#include <units/angle.h>
12
13class ShooterSubsystem : public frc2::PIDSubsystem {
14 public:
15  ShooterSubsystem();
16
17  void UseOutput(double output, double setpoint) override;
18
19  double GetMeasurement() override;
20
21  bool AtSetpoint();
22
23  void RunFeeder();
24
25  void StopFeeder();
26
27 private:
28  frc::PWMSparkMax m_shooterMotor;
29  frc::PWMSparkMax m_feederMotor;
30  frc::Encoder m_shooterEncoder;
31  frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
32};
 5#include "subsystems/ShooterSubsystem.h"
 6
 7#include <frc/controller/PIDController.h>
 8
 9#include "Constants.h"
10
11using namespace ShooterConstants;
12
13ShooterSubsystem::ShooterSubsystem()
14    : PIDSubsystem{frc::PIDController{kP, kI, kD}},
15      m_shooterMotor(kShooterMotorPort),
16      m_feederMotor(kFeederMotorPort),
17      m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
18      m_shooterFeedforward(kS, kV) {
19  m_controller.SetTolerance(kShooterToleranceRPS.value());
20  m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
21  SetSetpoint(kShooterTargetRPS.value());
22}
23
24void ShooterSubsystem::UseOutput(double output, double setpoint) {
25  m_shooterMotor.SetVoltage(units::volt_t{output} +
26                            m_shooterFeedforward.Calculate(kShooterTargetRPS));
27}
28
29bool ShooterSubsystem::AtSetpoint() {
30  return m_controller.AtSetpoint();
31}
32
33double ShooterSubsystem::GetMeasurement() {
34  return m_shooterEncoder.GetRate();
35}
36
37void ShooterSubsystem::RunFeeder() {
38  m_feederMotor.Set(kFeederSpeed);
39}
40
41void ShooterSubsystem::StopFeeder() {
42  m_feederMotor.Set(0);
43}

通过命令使用“PIDSubsystem”非常简单:

  private final Command m_spinUpShooter = Commands.runOnce(m_shooter::enable, m_shooter);
  private final Command m_stopShooter = Commands.runOnce(m_shooter::disable, m_shooter);

    // We can bind commands while retaining references to them in RobotContainer

    // Spin up the shooter when the 'A' button is pressed
    m_driverController.a().onTrue(m_spinUpShooter);

    // Turn off the shooter when the 'B' button is pressed
    m_driverController.b().onTrue(m_stopShooter);
45  frc2::CommandPtr m_spinUpShooter =
46      frc2::cmd::RunOnce([this] { m_shooter.Enable(); }, {&m_shooter});
47
48  frc2::CommandPtr m_stopShooter =
49      frc2::cmd::RunOnce([this] { m_shooter.Disable(); }, {&m_shooter});
25  // We can bind commands while keeping their ownership in RobotContainer
26
27  // Spin up the shooter when the 'A' button is pressed
28  m_driverController.A().OnTrue(m_spinUpShooter.get());
29
30  // Turn off the shooter when the 'B' button is pressed
31  m_driverController.B().OnTrue(m_stopShooter.get());

PID指令

The PIDCommand class allows users to easily create commands with a built-in PIDController.

创建一个PID指令

A PIDCommand can be created two ways - by subclassing the PIDCommand 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 PIDCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, PID functionality will not work properly.

无论哪种情况,都会通过将必要的参数传递给“PIDCommand”的构造函数来创建“PIDCommand”(如果定义了子类,则可以通过“ super()”调用来完成):

27  /**
28   * Creates a new PIDCommand, which controls the given output with a PIDController.
29   *
30   * @param controller the controller that controls the output.
31   * @param measurementSource the measurement of the process variable
32   * @param setpointSource the controller's setpoint
33   * @param useOutput the controller's output
34   * @param requirements the subsystems required by this command
35   */
36  public PIDCommand(
37      PIDController controller,
38      DoubleSupplier measurementSource,
39      DoubleSupplier setpointSource,
40      DoubleConsumer useOutput,
41      Subsystem... requirements) {
28  /**
29   * Creates a new PIDCommand, which controls the given output with a
30   * PIDController.
31   *
32   * @param controller        the controller that controls the output.
33   * @param measurementSource the measurement of the process variable
34   * @param setpointSource   the controller's reference (aka setpoint)
35   * @param useOutput         the controller's output
36   * @param requirements      the subsystems required by this command
37   */
38  PIDCommand(frc::PIDController controller,
39             std::function<double()> measurementSource,
40             std::function<double()> setpointSource,
41             std::function<void(double)> useOutput,
42             Requirements requirements = {});

controller

“controller”参数是在指令中将使用的“PIDController”对象。通过传递此参数,用户可以详细指定PID增益和控制器的周期(如果用户使用的是非标准主机器人循环周期)。

当子类化“PIDCommand”时,可以通过调用getController()对构造函数主体中的控制器进行额外的修改(例如启用连续输入)。

measurementSource

The measurementSource parameter is a function (usually passed as a lambda) that returns the measurement of the process variable. Passing in the measurementSource function in PIDCommand is functionally analogous to overriding the getMeasurement() function in PIDSubsystem.

当子类化“PIDCommand”时,高级用户可以通过修改类的“m_measurement”字段来进一步修改度量值提供器。

setpointSource

The setpointSource parameter is a function (usually passed as a lambda) that returns the current setpoint for the control loop. If only a constant setpoint is needed, an overload exists that takes a constant setpoint rather than a supplier.

当子类化“PIDCommand”时,高级用户可以通过修改类的“m_setpoint”字段来进一步修改设定值提供器。

useOutput

The useOutput parameter is a function (usually passed as a lambda) that consumes the output and setpoint of the control loop. Passing in the useOutput function in PIDCommand is functionally analogous to overriding the useOutput() function in PIDSubsystem.

当子类化“PIDCommand”时,高级用户可以通过修改类的“m_useOutput”字段来进一步修改输出值使用器。

要求

像所有内联命令一样,“PIDCommand”允许用户将其子系统要求指定为构造函数参数。

完整的PID指令示例

在实际使用中,“ PIDCommand”是什么样的?以下示例来自GyroDriveCommands示例项目(JavaC ++):

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

And, for an inlined example:

64    // Stabilize robot to drive straight with gyro when left bumper is held
65    new JoystickButton(m_driverController, Button.kL1.value)
66        .whileTrue(
67            new PIDCommand(
68                new PIDController(
69                    DriveConstants.kStabilizationP,
70                    DriveConstants.kStabilizationI,
71                    DriveConstants.kStabilizationD),
72                // Close the loop on the turn rate
73                m_robotDrive::getTurnRate,
74                // Setpoint is 0
75                0,
76                // Pipe the output to the turning controls
77                output -> m_robotDrive.arcadeDrive(-m_driverController.getLeftY(), output),
78                // Require the robot drive
79                m_robotDrive));
34  // Stabilize robot to drive straight with gyro when L1 is held
35  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1)
36      .WhileTrue(
37          frc2::PIDCommand(
38              frc::PIDController{dc::kStabilizationP, dc::kStabilizationI,
39                                 dc::kStabilizationD},
40              // Close the loop on the turn rate
41              [this] { return m_drive.GetTurnRate(); },
42              // Setpoint is 0
43              0,
44              // Pipe the output to the turning controls
45              [this](double output) {
46                m_drive.ArcadeDrive(m_driverController.GetLeftY(), output);
47              },
48              // Require the robot drive
49              {&m_drive})