在基于指令中将运动分析和PID结合

备注

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

常见的FRC | reg |控制解决方案是将用于设定值生成的梯形运动曲线与用于设定值跟踪的PID控制器配对。为此，WPILib包含了自己的:ref:ProfiledPIDController <docs/software/advanced-controls/controllers/profiled-pidcontroller:Combining Motion Profiling and PID Control with ProfiledPIDController> 类。为了进一步帮助团队将该功能集成到他们的机器人中，基于指令的框架为“ProfiledPIDController”类提供了两个便捷包装：“ProfiledPIDSubsystem”将控制器集成到子系统中以及“ProfiledPIDCommand”，它将控制器集成到指令中。

ProfiledPIDCommand

备注

在C ++中，“ProfiledPIDCommand”类以用于距离测量的单位类型为模板，该单位类型可以是角度或线性的。传入的值*必须*具有与距离单位一致的单位，否则将引发编译时错误。有关C ++单元的更多信息，请参阅:ref:docs/software/basic-programming/cpp-units:The C++ Units Library。

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

创建一个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.

无论哪种情况，都通过向构造函数传递必要的参数来创建“ProfileDPIDCommand”（如果定义了子类，则可以通过调用“super()”来完成）：

Java

  /**
   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
   * velocity is specified.
   *
   * @param controller the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param goalSource the controller's goal
   * @param useOutput the controller's output
   * @param requirements the subsystems required by this command
   */
  public ProfiledPIDCommand(
      ProfiledPIDController controller,
      DoubleSupplier measurementSource,
      Supplier<State> goalSource,
      BiConsumer<Double, State> useOutput,
      Subsystem... requirements) {

C++

  /**
   * Creates a new PIDCommand, which controls the given output with a
   * ProfiledPIDController.
   *
   * @param controller        the controller that controls the output.
   * @param measurementSource the measurement of the process variable
   * @param goalSource   the controller's goal
   * @param useOutput         the controller's output
   * @param requirements      the subsystems required by this command
   */
  ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
                     std::function<Distance_t()> measurementSource,
                     std::function<State()> goalSource,
                     std::function<void(double, State)> useOutput,
                     Requirements requirements = {})

controller

“controller”参数是在指令中将使用的“ProfiledPIDController”对象。通过传递此参数，用户可以指定PID增益，运动曲线约束和控制器的周期（如果用户使用的是非标准的主机器人循环周期）。

当将“ProfiledPIDCommand”子类化时，可以通过调用“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 ProfiledPIDCommand is functionally analogous to overriding the getMeasurement() function in ProfiledPIDSubsystem.

在子类化“ProfiledPIDCommand”时，高级用户可以通过修改类的“m_measurement”字段来进一步修改度量提供者。

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.

当子类化“ProfiledPIDCommand”时，高级用户可以通过修改类的“m_goal”字段来进一步修改设定值供应商。

useOutput

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

在子类化“ProfiledPIDCommand”时，高级用户可以通过修改类的“m_useOutput”字段来进一步修改输出使用者。

requirements

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

完整ProfiledPIDCommand示例

在实际使用中，“ ProfiledPIDCommand”是什么样的？以下示例来自GyroDriveCommands示例项目（Java，C ++）：

Java

package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;

/** A command that will turn the robot to the specified angle using a motion profile. */
public class TurnToAngleProfiled extends ProfiledPIDCommand {
  /**
   * Turns to robot to the specified angle using a motion profile.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive The drive subsystem to use
   */
  public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
    super(
        new ProfiledPIDController(
            DriveConstants.kTurnP,
            DriveConstants.kTurnI,
            DriveConstants.kTurnD,
            new TrapezoidProfile.Constraints(
                DriveConstants.kMaxTurnRateDegPerS,
                DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
        // Close loop on heading
        drive::getHeading,
        // Set reference to target
        targetAngleDegrees,
        // Pipe output to turn robot
        (output, setpoint) -> 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().atGoal();
  }
}

C++ (Header)

#pragma once

#include <frc2/command/CommandHelper.h>
#include <frc2/command/ProfiledPIDCommand.h>

#include "subsystems/DriveSubsystem.h"

/**
 * A command that will turn the robot to the specified angle using a motion
 * profile.
 */
class TurnToAngleProfiled
    : public frc2::CommandHelper<frc2::ProfiledPIDCommand<units::radians>,
                                 TurnToAngleProfiled> {
 public:
  /**
   * Turns to robot to the specified angle using a motion profile.
   *
   * @param targetAngleDegrees The angle to turn to
   * @param drive              The drive subsystem to use
   */
  TurnToAngleProfiled(units::degree_t targetAngleDegrees,
                      DriveSubsystem* drive);

  bool IsFinished() override;
};

C++ (Source)

#include "commands/TurnToAngleProfiled.h"

#include <frc/controller/ProfiledPIDController.h>

using namespace DriveConstants;

TurnToAngleProfiled::TurnToAngleProfiled(units::degree_t target,
                                         DriveSubsystem* drive)
    : CommandHelper{
          frc::ProfiledPIDController<units::radians>{
              kTurnP, kTurnI, kTurnD, {kMaxTurnRate, kMaxTurnAcceleration}},
          // Close loop on heading
          [drive] { return drive->GetHeading(); },
          // Set reference to target
          target,
          // Pipe output to turn robot
          [drive](double output, auto setpointState) {
            drive->ArcadeDrive(0, output);
          },
          // Require the drive
          {drive}} {
  // Set the controller to be continuous (because it is an angle controller)
  GetController().EnableContinuousInput(-180_deg, 180_deg);
  // 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(kTurnTolerance, kTurnRateTolerance);

  AddRequirements(drive);
}

bool TurnToAngleProfiled::IsFinished() {
  return GetController().AtGoal();
}

在基于指令中将运动分析和PID结合

ProfiledPIDSubsystem

创建一个ProfiledPIDSubsystem

getMeasurement()

useOutput()

传递至控制器

使用ProfiledPIDSubsystem

setGoal()

enable()和disable()

完整ProfiledPIDSubsystem示例