通过TrapezoidProfileSubsystems和TrapezoidProfileCommands进行运动分析

备注

有关这些基于指令的包装程序使用的WPILib运动分析功能的描述，请参阅:ref:docs/software/advanced-controls/controllers/trapezoidal-profiles:Trapezoidal Motion Profiles in WPILib。

备注

“TrapezoidProfile”指令包装通常用于自定义或外部控制器的组合。要将梯形运动轮廓与WPILib的“PIDController”结合使用，请参阅:doc:profilepid-subsystems-commands。

When controlling a mechanism, is often desirable to move it smoothly between two positions, rather than to abruptly change its setpoint. This is called “motion-profiling,” and is supported in WPILib through the TrapezoidProfile class (Java, C++).

为了进一步帮助团队将运动分析集成到基于指令的机器人项目中，WPILib为“TrapezoidProfile”类提供了两个便捷包装器：“TrapezoidProfileSubsystem”，该包装器自动在其“periodic()”中生成并执行运动配置文件。方法和“TrapezoidProfileCommand”，后者执行一个用户提供的“TrapezoidProfile”。

TrapezoidProfileSubsystem

备注

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

The TrapezoidProfileSubsystem class (Java, C++) will automatically create and execute trapezoidal motion profiles to reach the user-provided goal state. To use the TrapezoidProfileSubsystem class, users must create a subclass of it.

创建一个TrapezoidProfileSubsystem

备注

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

当子类化“TrapezoidProfileSubsystem”时，用户必须重写单个抽象方法，以提供该类将在其常规操作中使用的功能：

useState()

  protected abstract void useState(TrapezoidProfile.State state);

  virtual void UseState(State state) = 0;

“useState()”方法消耗运动配置文件的当前状态。 “TrapezoidProfileSubsystem”将自动从其“periodic()”块中调用此方法，并将与子系统当前进度相对应的运动配置文件状态通过运动配置文件传递给该方法。

用户可以在这种状态下做任何想做的事情。典型的用例（如`Full TrapezoidProfileSubsystem Example`_中所示）是使用状态获取外部“智能”电动机控制器的设定值和前馈。

构造器参数

用户必须通过子类的超类构造函数调用将一组“TrapezoidProfile.Constraints”传递给“TrapezoidProfileSubsystem”基类。这用于将自动生成的轮廓约束到给定的最大速度和加速度。

用户还必须通过机制的初始位置。

如果正在使用非标准的主循环周期，则高级用户可以在循环周期中输入备用值。

使用TrapezoidProfileSubsystem

一旦创建了“TrapezoidProfileSubsystem”子类的实例，命令就可以通过以下方法使用它：

setGoal()

备注

如果希望将目标设置为隐式目标速度为零的简单距离，则存在“setGoal()”重载，该重载采用单个距离值而不是完整的运动轮廓状态。

“setGoal()”方法可用于设置“TrapezoidProfileSubsystem”的目标状态。子系统将自动执行针对目标的配置文件，将每次迭代时的当前状态传递给提供的`useState()`_方法。

// The subsystem will execute a profile to a position of 5 and a velocity of 3.
examplePIDSubsystem.setGoal(new TrapezoidProfile.State(5, 3);

// The subsystem will execute a profile to a position of 5 meters and a velocity of 3 mps.
examplePIDSubsyste.SetGoal({5_m, 3_mps});

enable() and disable()

The enable() and disable() methods enable and disable the motion profiling control of the TrapezoidProfileSubsystem. When the subsystem is enabled, it will automatically run the control loop and call useState() periodically. When it is disabled, no control is performed.

完整的TrapezoidProfileSubsystem示例

在实践中使用时，“ TrapezoidProfileSubsystem”是什么样的？以下示例摘自ArmbotOffobard示例项目（Java，C ++）：

 5package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
 6
 7import edu.wpi.first.math.controller.ArmFeedforward;
 8import edu.wpi.first.math.trajectory.TrapezoidProfile;
 9import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
10import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
11import edu.wpi.first.wpilibj2.command.Command;
12import edu.wpi.first.wpilibj2.command.Commands;
13import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
14
15/** A robot arm subsystem that moves with a motion profile. */
16public class ArmSubsystem extends TrapezoidProfileSubsystem {
17  private final ExampleSmartMotorController m_motor =
18      new ExampleSmartMotorController(ArmConstants.kMotorPort);
19  private final ArmFeedforward m_feedforward =
20      new ArmFeedforward(
21          ArmConstants.kSVolts, ArmConstants.kGVolts,
22          ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
23
24  /** Create a new ArmSubsystem. */
25  public ArmSubsystem() {
26    super(
27        new TrapezoidProfile.Constraints(
28            ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
29        ArmConstants.kArmOffsetRads);
30    m_motor.setPID(ArmConstants.kP, 0, 0);
31  }
32
33  @Override
34  public void useState(TrapezoidProfile.State setpoint) {
35    // Calculate the feedforward from the sepoint
36    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
37    // Add the feedforward to the PID output to get the motor output
38    m_motor.setSetpoint(
39        ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
40  }
41
42  public Command setArmGoalCommand(double kArmOffsetRads) {
43    return Commands.runOnce(() -> setGoal(kArmOffsetRads), this);
44  }
45}

 5#pragma once
 6
 7#include <frc/controller/ArmFeedforward.h>
 8#include <frc2/command/Commands.h>
 9#include <frc2/command/TrapezoidProfileSubsystem.h>
10#include <units/angle.h>
11
12#include "ExampleSmartMotorController.h"
13
14/**
15 * A robot arm subsystem that moves with a motion profile.
16 */
17class ArmSubsystem : public frc2::TrapezoidProfileSubsystem<units::radians> {
18  using State = frc::TrapezoidProfile<units::radians>::State;
19
20 public:
21  ArmSubsystem();
22
23  void UseState(State setpoint) override;
24
25  frc2::CommandPtr SetArmGoalCommand(units::radian_t goal);
26
27 private:
28  ExampleSmartMotorController m_motor;
29  frc::ArmFeedforward m_feedforward;
30};

 5#include "subsystems/ArmSubsystem.h"
 6
 7#include "Constants.h"
 8
 9using namespace ArmConstants;
10using State = frc::TrapezoidProfile<units::radians>::State;
11
12ArmSubsystem::ArmSubsystem()
13    : frc2::TrapezoidProfileSubsystem<units::radians>(
14          {kMaxVelocity, kMaxAcceleration}, kArmOffset),
15      m_motor(kMotorPort),
16      m_feedforward(kS, kG, kV, kA) {
17  m_motor.SetPID(kP, 0, 0);
18}
19
20void ArmSubsystem::UseState(State setpoint) {
21  // Calculate the feedforward from the sepoint
22  units::volt_t feedforward =
23      m_feedforward.Calculate(setpoint.position, setpoint.velocity);
24  // Add the feedforward to the PID output to get the motor output
25  m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
26                      setpoint.position.value(), feedforward / 12_V);
27}
28
29frc2::CommandPtr ArmSubsystem::SetArmGoalCommand(units::radian_t goal) {
30  return frc2::cmd::RunOnce([this, goal] { this->SetGoal(goal); }, {this});
31}

通过命令使用“TrapezoidProfileSubsystem”可能非常简单：

52    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
53    m_driverController.a().onTrue(m_robotArm.setArmGoalCommand(2));
54
55    // Move the arm to neutral position when the 'B' button is pressed.
56    m_driverController
57        .b()
58        .onTrue(m_robotArm.setArmGoalCommand(Constants.ArmConstants.kArmOffsetRads));

25  // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
26  m_driverController.A().OnTrue(m_arm.SetArmGoalCommand(2_rad));
27
28  // Move the arm to neutral position when the 'B' button is pressed.
29  m_driverController.B().OnTrue(
30      m_arm.SetArmGoalCommand(ArmConstants::kArmOffset));

TrapezoidProfileCommand

备注

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

The TrapezoidProfileCommand class (Java, C++) allows users to create a command that will execute a single TrapezoidProfile, passing its current state at each iteration to a user-defined function.

创建一个TrapezoidProfileCommand

A TrapezoidProfileCommand can be created two ways - by subclassing the TrapezoidProfileCommand class, or by defining the command inline. Both methods are 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 TrapezoidProfileCommand and overriding any methods, make sure to call the super version of those methods! Otherwise, motion profiling functionality will not work properly.

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

28  /**
29   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
30   * Output will be piped to the provided consumer function.
31   *
32   * @param profile The motion profile to execute.
33   * @param output The consumer for the profile output.
34   * @param goal The supplier for the desired state
35   * @param currentState The current state
36   * @param requirements The subsystems required by this command.
37   */
38  @SuppressWarnings("this-escape")
39  public TrapezoidProfileCommand(
40      TrapezoidProfile profile,
41      Consumer<State> output,
42      Supplier<State> goal,
43      Supplier<State> currentState,

35  /**
36   * Creates a new TrapezoidProfileCommand that will execute the given
37   * TrapezoidalProfile. Output will be piped to the provided consumer function.
38   *
39   * @param profile      The motion profile to execute.
40   * @param output       The consumer for the profile output.
41   * @param goal The supplier for the desired state
42   * @param currentState The current state
43   * @param requirements The list of requirements.
44   */
45  TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
46                          std::function<void(State)> output,
47                          std::function<State()> goal,
48                          std::function<State()> currentState,
49                          Requirements requirements = {})

profile

The profile parameter is the TrapezoidProfile object that will be executed by the command. By passing this in, users specify the motion constraints of the profile that the command will use.

output

The output 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 useState() function in PIDSubsystem.

goal

The goal parameter is a function that supplies the desired state of the motion profile. This can be used to change the goal at runtime if desired.

currentState

The currentState parameter is a function that supplies the starting state of the motion profile. Combined with goal, this can be used to dynamically generate and follow any motion profile at runtime.

requirements

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

完整的TrapezoidProfileCommand示例

在实践中使用时，“ TrapezoidProfileSubsystem”是什么样的？以下示例取自DriveDistanceOffboard示例项目（Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard>`__，C ++）：

 5package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
 6
 7import edu.wpi.first.math.trajectory.TrapezoidProfile;
 8import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 9import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
10import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
11
12/** Drives a set distance using a motion profile. */
13public class DriveDistanceProfiled extends TrapezoidProfileCommand {
14  /**
15   * Creates a new DriveDistanceProfiled command.
16   *
17   * @param meters The distance to drive.
18   * @param drive The drive subsystem to use.
19   */
20  public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
21    super(
22        new TrapezoidProfile(
23            // Limit the max acceleration and velocity
24            new TrapezoidProfile.Constraints(
25                DriveConstants.kMaxSpeedMetersPerSecond,
26                DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
27        // Pipe the profile state to the drive
28        setpointState -> drive.setDriveStates(setpointState, setpointState),
29        // End at desired position in meters; implicitly starts at 0
30        () -> new TrapezoidProfile.State(meters, 0),
31        // Current position
32        TrapezoidProfile.State::new,
33        // Require the drive
34        drive);
35    // Reset drive encoders since we're starting at 0
36    drive.resetEncoders();
37  }
38}

 5#pragma once
 6
 7#include <frc2/command/CommandHelper.h>
 8#include <frc2/command/TrapezoidProfileCommand.h>
 9
10#include "subsystems/DriveSubsystem.h"
11
12class DriveDistanceProfiled
13    : public frc2::CommandHelper<frc2::TrapezoidProfileCommand<units::meters>,
14                                 DriveDistanceProfiled> {
15 public:
16  DriveDistanceProfiled(units::meter_t distance, DriveSubsystem* drive);
17};

 5#include "commands/DriveDistanceProfiled.h"
 6
 7#include "Constants.h"
 8
 9using namespace DriveConstants;
10
11DriveDistanceProfiled::DriveDistanceProfiled(units::meter_t distance,
12                                             DriveSubsystem* drive)
13    : CommandHelper{
14          frc::TrapezoidProfile<units::meters>{
15              // Limit the max acceleration and velocity
16              {kMaxSpeed, kMaxAcceleration}},
17          // Pipe the profile state to the drive
18          [drive](auto setpointState) {
19            drive->SetDriveStates(setpointState, setpointState);
20          },
21          // End at desired position in meters; implicitly starts at 0
22          [distance] {
23            return frc::TrapezoidProfile<units::meters>::State{distance, 0_mps};
24          },
25          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
26          // Require the drive
27          {drive}} {
28  // Reset drive encoders since we're starting at 0
29  drive->ResetEncoders();
30}

And, for an inlined example:

66    // Do the same thing as above when the 'B' button is pressed, but defined inline
67    m_driverController
68        .b()
69        .onTrue(
70            new TrapezoidProfileCommand(
71                    new TrapezoidProfile(
72                        // Limit the max acceleration and velocity
73                        new TrapezoidProfile.Constraints(
74                            DriveConstants.kMaxSpeedMetersPerSecond,
75                            DriveConstants.kMaxAccelerationMetersPerSecondSquared)),
76                    // Pipe the profile state to the drive
77                    setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
78                    // End at desired position in meters; implicitly starts at 0
79                    () -> new TrapezoidProfile.State(3, 0),
80                    // Current position
81                    TrapezoidProfile.State::new,
82                    // Require the drive
83                    m_robotDrive)
84                .beforeStarting(m_robotDrive::resetEncoders)
85                .withTimeout(10));

37      DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
38
39  // Do the same thing as above when the 'B' button is pressed, but defined
40  // inline
41  m_driverController.B().OnTrue(
42      frc2::TrapezoidProfileCommand<units::meters>(
43          frc::TrapezoidProfile<units::meters>(
44              // Limit the max acceleration and velocity
45              {DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}),
46          // Pipe the profile state to the drive
47          [this](auto setpointState) {
48            m_drive.SetDriveStates(setpointState, setpointState);
49          },
50          // End at desired position in meters; implicitly starts at 0
51          [] {
52            return frc::TrapezoidProfile<units::meters>::State{3_m, 0_mps};
53          },
54          // Current position
55          [] { return frc::TrapezoidProfile<units::meters>::State{}; },
56          // Require the drive
57          {&m_drive})
58          // Convert to CommandPtr
59          .ToPtr()
60          .BeforeStarting(