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

注解

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

注解

与早期版本的“PIDController”不同,2020年的“PIDController”类将会*同步运行*,并且不在其自己的线程中完成处理。因此,更改其“period”参数将*不会*更改在任何这些程序包类中运行的实际频率。除非用户确定自己在做什么,否则切勿修改“period”参数。

FRC | reg |中最常用的控制算法之一是`PID controller <https://en.wikipedia.org/wiki/PID_controller>`__。 WPILib提供了自己的:ref:`PIDController <docs/software/advanced-controls/controllers/pidcontroller:PID Control in WPILib>`类,以帮助团队在其机器人上实现此功能。为了进一步帮助团队将PID控制集成到基于指令的机器人项目中,基于指令的库包括两个方便包装器,用于“PIDController”类:“PIDSubsystem”,它将PID控制器集成到子系统中, “PIDCommand”,它将PID控制器集成到指令中。

PID子系统

PIDSubsystem类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.html>`__,C ++)允许用户方便地创建带有内置PIDController的子系统。为了使用“ PIDSubsystem”类,用户必须创建它的子类。

创建一个PID子系统

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

getMeasurement()

protected abstract double getMeasurement();

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

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

useOutput()

protected abstract void useOutput(double output, double setpoint);

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);

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 ++):

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj2.command.PIDSubsystem;

import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;

public class ShooterSubsystem extends PIDSubsystem {
  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
  private final Encoder m_shooterEncoder =
      new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
                  ShooterConstants.kEncoderReversed);
  private final SimpleMotorFeedforward m_shooterFeedforward =
      new SimpleMotorFeedforward(ShooterConstants.kSVolts,
                                 ShooterConstants.kVVoltSecondsPerRotation);

  /**
   * The shooter subsystem for the robot.
   */
  public ShooterSubsystem() {
    super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
    getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
    m_shooterEncoder.setDistancePerPulse(ShooterConstants.kEncoderDistancePerPulse);
    setSetpoint(ShooterConstants.kShooterTargetRPS);
  }

  @Override
  public void useOutput(double output, double setpoint) {
    m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
  }

  @Override
  public double getMeasurement() {
    return m_shooterEncoder.getRate();
  }

  public boolean atSetpoint() {
    return m_controller.atSetpoint();
  }

  public void runFeeder() {
    m_feederMotor.set(ShooterConstants.kFeederSpeed);
  }

  public void stopFeeder() {
    m_feederMotor.set(0);
  }
}

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

85
86
87
88
89
90
91
    // Spin up the shooter when the 'A' button is pressed
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(new InstantCommand(m_shooter::enable, m_shooter));

    // Turn off the shooter when the 'B' button is pressed
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));

PID指令

“PIDCommand”类允许用户使用内置的PID控制器轻松创建指令。与PID子系统一样,用户可以通过子类化“PIDCommand”类来创建PID指令。但是,与基于指令的库中的许多其他指令类一样,用户可能希望通过定义一个“PIDCommand”来保存代码:ref:inline <docs/software/commandbased/convenience-features:Inline Command Definitions>

创建一个PID指令

可以通过两种方式创建“PIDCommand”:-通过对PIDCommand类进行子类化,或通过定义指令:ref:inline <docs/software/commandbased/convenience-features:Inline Command Definitions>。两种方法最终都极为相似,最终选择使用哪种方法取决于用户希望找到相关代码位于哪里。

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

29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
  /**
   * Creates a new PIDCommand, which controls the given output with a PIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param setpointSource    the controller's setpoint
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
                    Subsystem... requirements) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_setpoint = setpointSource;
    m_requirements.addAll(Set.of(requirements));
  }

controller

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

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

measurementSource

“measurementSource”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),该函数返回过程变量的度量值。在“PIDCommand”中传递“measurementSource”函数在功能上类似于覆盖“PIDSubsystem”中的“getMeasurement()”_函数。

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

setpointSource

“setpointSource”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),该函数返回控制回路的当前设定值。如果仅需要一个恒定的设定值,则存在一个需要恒定的设定值而不是提供器的过载。

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

useOutput

“useOutput”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),该函数使用控制循环的输出值和设定值。在“PIDCommand”中传递“useOutput”函数在功能上类似于覆盖“PIDSubsystem”中的“useOutput()”_函数。

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

要求

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

完整的PID指令示例

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

 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;

import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;

/**
 * A command that will turn the robot to the specified angle.
 */
public class TurnToAngle extends PIDCommand {
  /**
   * Turns to robot to the specified angle.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new PIDController(DriveConstants.kTurnP, DriveConstants.kTurnI, DriveConstants.kTurnD),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        output -> drive.arcadeDrive(0, output),
        // Require the drive
        drive);

    // Set the controller to be continuous (because it is an angle controller)
    getController().enableContinuousInput(-180, 180);
    // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the
    // setpoint before it is considered as having reached the reference
    getController()
        .setTolerance(DriveConstants.kTurnToleranceDeg, DriveConstants.kTurnRateToleranceDegPerS);
  }

  @Override
  public boolean isFinished() {
    // End when the controller is at the reference.
    return getController().atSetpoint();
  }
}

以及对于:ref:inlined <docs/software/commandbased/convenience-features:Inline Command Definitions> 的示例:

70
71
72
73
74
75
76
77
78
79
80
81
    // Stabilize robot to drive straight with gyro when left bumper is held
    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
        new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
                          DriveConstants.kStabilizationD),
        // Close the loop on the turn rate
        m_robotDrive::getTurnRate,
        // Setpoint is 0
        0,
        // Pipe the output to the turning controls
        output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
        // Require the robot drive
        m_robotDrive));