将运动分析和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++) 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 ``实例：

JavaC++

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

目标与设定点

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

JavaC++

// 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（）方法来完成。

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

JavaC++

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

完整用法示例

ElevatorProfilePID示例项目(Java, C++):

JavaC++

package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;

public class Robot extends TimedRobot {
  private static double kDt = 0.02;

  private final Joystick m_joystick = new Joystick(1);
  private final Encoder m_encoder = new Encoder(1, 2);
  private final MotorController m_motor = new PWMSparkMax(1);

  // Create a PID controller whose setpoint's change is subject to maximum
  // velocity and acceleration constraints.
  private final TrapezoidProfile.Constraints m_constraints =
      new TrapezoidProfile.Constraints(1.75, 0.75);
  private final ProfiledPIDController m_controller =
      new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt);

  @Override
  public void robotInit() {
    m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * Math.PI * 1.5);
  }

  @Override
  public void teleopPeriodic() {
    if (m_joystick.getRawButtonPressed(2)) {
      m_controller.setGoal(5);
    } else if (m_joystick.getRawButtonPressed(3)) {
      m_controller.setGoal(0);
    }

    // Run controller and update motor output
    m_motor.set(m_controller.calculate(m_encoder.getDistance()));
  }
}