将运动分析和PID控制与ProfiledPIDController相结合

备注

有关在基于命令的框架:ref:command-based framework <docs/software/commandbased/what-is-command-based:What Is “Command-Based” Programming?> framework，请参阅 在基于指令中将运动分析和PID结合.

在上一篇文章中，我们看到了如何使用TrapezoidProfile类来创建和使用梯形运动轮廓。该文章中的示例代码演示了如何使用“智能”电机控制器的外部PID控制功能手动组合“ TrapezoidProfile”类。

This combination of functionality (a motion profile for generating setpoints combined with a PID controller for following them) is extremely common. To facilitate this, WPILib comes with a ProfiledPIDController class (Java, C++, Python) that does most of the work of combining these two functionalities. The API of the ProfiledPIDController is very similar to that of the PIDController, allowing users to add motion profiling to a PID-controlled mechanism with very few changes to their code.

使用ProfiledPIDController类

备注

在C ++中，“ ProfiledPIDController”类在用于距离测量的单位类型（可能是角度的或线性的）上模板化。传入的值*必须*具有与距离单位一致的单位，否则将引发编译时错误。有关C ++单元的更多信息，请参阅：docs / software / basic-programming / cpp-units：C ++单元库。

备注

ProfiledPIDController的许多功能实际上与PIDController的功能相同。因此，本文将仅涵盖经过实质性更改以适应运动分析功能的功能。有关标准``PIDController’’功能的信息，请参阅 WPILib中的PID控制.

构造一个ProfiledPIDController

备注

C ++通常能够推断内部类的类型，因此可以将简单的初始化列表（没有类名）作为参数发送。为了清楚起见，完整的类名包含在下面的示例中。

创建``ProfiledPIDController’’几乎与创建PIDController creating a PIDController. 完全相同。唯一的区别是需要提供一组梯形轮廓约束<docs/software/advanced-controls/controllers/trapezoidal-profiles:Constraints>`,这些约束将自动转发到内部生成的``TrapezoidProfile ``实例：

// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
ProfiledPIDController controller = new ProfiledPIDController(
  kP, kI, kD,
  new TrapezoidProfile.Constraints(5, 10));

// Creates a ProfiledPIDController
// Max velocity is 5 meters per second
// Max acceleration is 10 meters per second
frc::ProfiledPIDController<units::meters> controller(
  kP, kI, kD,
  frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq});

from wpimath.controller import ProfiledPIDController
from wpimath.trajectory import TrapezoidProfile

# Creates a ProfiledPIDController
# Max velocity is 5 meters per second
# Max acceleration is 10 meters per second
controller = ProfiledPIDController(
  kP, kI, kD,
  TrapezoidProfile.Constraints(5, 10))

目标与设定点

标准``PIDController’’和``ProfiledPIDController’’之间的主要区别在于控制环的实际*设置点*不是由用户直接指定的。而是由用户指定目标位置或状态，并根据当前状态和目标之间生成的运动曲线自动计算控制器的设定点。因此，尽管用户端调用看起来大致相同：

// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.set(controller.calculate(encoder.getDistance(), goal));

// Calculates the output of the PID algorithm based on the sensor reading
// and sends it to a motor
motor.Set(controller.Calculate(encoder.GetDistance(), goal));

# Calculates the output of the PID algorithm based on the sensor reading
# and sends it to a motor
motor.set(controller.calculate(encoder.getDistance(), goal))

指定的“目标”值（如果需要非零速度，则可以是位置值或“ TrapezoidProfile.State”）*不一定*一定是回路的*当前*设定值-而是一旦生成的配置文件终止，*最终*设定点。

获取/使用设定值

由于ProfiledPIDController目标不同于设定值，因此通常需要轮询控制器的当前设定值（例如，获取:ref:feedforward <docs/software/advanced-controls/controllers/combining-feedforward-feedback:Using Feedforward Components with PID>). 这可以通过getSetpoint（）方法来完成。

然后，可以将返回的设定值用于以下示例：

double lastSpeed = 0;
double lastTime = Timer.getFPGATimestamp();

// Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
public void goToPosition(double goalPosition) {
  double pidVal = controller.calculate(encoder.getDistance(), goalPosition);
  double acceleration = (controller.getSetpoint().velocity - lastSpeed) / (Timer.getFPGATimestamp() - lastTime);
  motor.setVoltage(
      pidVal
      + feedforward.calculate(controller.getSetpoint().velocity, acceleration));
  lastSpeed = controller.getSetpoint().velocity;
  lastTime = Timer.getFPGATimestamp();
}

units::meters_per_second_t lastSpeed = 0_mps;
units::second_t lastTime = frc2::Timer::GetFPGATimestamp();

// Controls a simple motor's position using a SimpleMotorFeedforward
// and a ProfiledPIDController
void GoToPosition(units::meter_t goalPosition) {
  auto pidVal = controller.Calculate(units::meter_t{encoder.GetDistance()}, goalPosition);
  auto acceleration = (controller.GetSetpoint().velocity - lastSpeed) /
      (frc2::Timer::GetFPGATimestamp() - lastTime);
  motor.SetVoltage(
       pidVal +
      feedforward.Calculate(controller.GetSetpoint().velocity, acceleration));
  lastSpeed = controller.GetSetpoint().velocity;
  lastTime = frc2::Timer::GetFPGATimestamp();
}

from wpilib import Timer
from wpilib.controller import ProfiledPIDController
from wpilib.controller import SimpleMotorFeedforward


def __init__(self):

    # Assuming encoder, motor, controller are already defined
    self.lastSpeed = 0
    self.lastTime = Timer.getFPGATimestamp()

    # Assuming feedforward is a SimpleMotorFeedforward object
    self.feedforward = SimpleMotorFeedforward(ks=0.0, kv=0.0, ka=0.0)

def goToPosition(self, goalPosition: float):

    pidVal = self.controller.calculate(self.encoder.getDistance(), goalPosition)
    acceleration = (self.controller.getSetpoint().velocity - self.lastSpeed) / (Timer.getFPGATimestamp() - self.lastTime)

    self.motor.setVoltage(
        pidVal
        + self.feedforward.calculate(self.controller.getSetpoint().velocity, acceleration))

    self.lastSpeed = controller.getSetpoint().velocity
    self.lastTime = Timer.getFPGATimestamp()

完整用法示例

A more complete example of ProfiledPIDController usage is provided in the ElevatorProfilePID example project (Java, C++, Python):

 5package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
 6
 7import edu.wpi.first.math.controller.ElevatorFeedforward;
 8import edu.wpi.first.math.controller.ProfiledPIDController;
 9import edu.wpi.first.math.trajectory.TrapezoidProfile;
10import edu.wpi.first.wpilibj.Encoder;
11import edu.wpi.first.wpilibj.Joystick;
12import edu.wpi.first.wpilibj.TimedRobot;
13import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
14
15@SuppressWarnings("PMD.RedundantFieldInitializer")
16public class Robot extends TimedRobot {
17  private static double kDt = 0.02;
18  private static double kMaxVelocity = 1.75;
19  private static double kMaxAcceleration = 0.75;
20  private static double kP = 1.3;
21  private static double kI = 0.0;
22  private static double kD = 0.7;
23  private static double kS = 1.1;
24  private static double kG = 1.2;
25  private static double kV = 1.3;
26
27  private final Joystick m_joystick = new Joystick(1);
28  private final Encoder m_encoder = new Encoder(1, 2);
29  private final PWMSparkMax m_motor = new PWMSparkMax(1);
30
31  // Create a PID controller whose setpoint's change is subject to maximum
32  // velocity and acceleration constraints.
33  private final TrapezoidProfile.Constraints m_constraints =
34      new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration);
35  private final ProfiledPIDController m_controller =
36      new ProfiledPIDController(kP, kI, kD, m_constraints, kDt);
37  private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV);
38
39  @Override
40  public void robotInit() {
41    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
42  }
43
44  @Override
45  public void teleopPeriodic() {
46    if (m_joystick.getRawButtonPressed(2)) {
47      m_controller.setGoal(5);
48    } else if (m_joystick.getRawButtonPressed(3)) {
49      m_controller.setGoal(0);
50    }
51
52    // Run controller and update motor output
53    m_motor.setVoltage(
54        m_controller.calculate(m_encoder.getDistance())
55            + m_feedforward.calculate(m_controller.getSetpoint().velocity));
56  }
57}

 5#include <numbers>
 6
 7#include <frc/Encoder.h>
 8#include <frc/Joystick.h>
 9#include <frc/TimedRobot.h>
10#include <frc/controller/ElevatorFeedforward.h>
11#include <frc/controller/ProfiledPIDController.h>
12#include <frc/motorcontrol/PWMSparkMax.h>
13#include <frc/trajectory/TrapezoidProfile.h>
14#include <units/acceleration.h>
15#include <units/length.h>
16#include <units/time.h>
17#include <units/velocity.h>
18#include <units/voltage.h>
19
20class Robot : public frc::TimedRobot {
21 public:
22  static constexpr units::second_t kDt = 20_ms;
23
24  Robot() {
25    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
26  }
27
28  void TeleopPeriodic() override {
29    if (m_joystick.GetRawButtonPressed(2)) {
30      m_controller.SetGoal(5_m);
31    } else if (m_joystick.GetRawButtonPressed(3)) {
32      m_controller.SetGoal(0_m);
33    }
34
35    // Run controller and update motor output
36    m_motor.SetVoltage(
37        units::volt_t{
38            m_controller.Calculate(units::meter_t{m_encoder.GetDistance()})} +
39        m_feedforward.Calculate(m_controller.GetSetpoint().velocity));
40  }
41
42 private:
43  static constexpr units::meters_per_second_t kMaxVelocity = 1.75_mps;
44  static constexpr units::meters_per_second_squared_t kMaxAcceleration =
45      0.75_mps_sq;
46  static constexpr double kP = 1.3;
47  static constexpr double kI = 0.0;
48  static constexpr double kD = 0.7;
49  static constexpr units::volt_t kS = 1.1_V;
50  static constexpr units::volt_t kG = 1.2_V;
51  static constexpr auto kV = 1.3_V / 1_mps;
52
53  frc::Joystick m_joystick{1};
54  frc::Encoder m_encoder{1, 2};
55  frc::PWMSparkMax m_motor{1};
56
57  // Create a PID controller whose setpoint's change is subject to maximum
58  // velocity and acceleration constraints.
59  frc::TrapezoidProfile<units::meters>::Constraints m_constraints{
60      kMaxVelocity, kMaxAcceleration};
61  frc::ProfiledPIDController<units::meters> m_controller{kP, kI, kD,
62                                                         m_constraints, kDt};
63  frc::ElevatorFeedforward m_feedforward{kS, kG, kV};
64};
65
66#ifndef RUNNING_FRC_TESTS
67int main() {
68  return frc::StartRobot<Robot>();
69}
70#endif

 8import wpilib
 9import wpimath.controller
10import wpimath.trajectory
11import math
12
13
14class MyRobot(wpilib.TimedRobot):
15    kDt = 0.02
16
17    def robotInit(self) -> None:
18        self.joystick = wpilib.Joystick(1)
19        self.encoder = wpilib.Encoder(1, 2)
20        self.motor = wpilib.PWMSparkMax(1)
21
22        # Create a PID controller whose setpoint's change is subject to maximum
23        # velocity and acceleration constraints.
24        self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
25        self.controller = wpimath.controller.ProfiledPIDController(
26            1.3, 0, 0.7, self.constraints, self.kDt
27        )
28
29        self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5)
30
31    def teleopPeriodic(self) -> None:
32        if self.joystick.getRawButtonPressed(2):
33            self.controller.setGoal(5)
34        elif self.joystick.getRawButtonPressed(3):
35            self.controller.setGoal(0)
36
37        # Run controller and update motor output
38        self.motor.set(self.controller.calculate(self.encoder.getDistance()))