通过TrapezoidProfileSubsystems和TrapezoidProfileCommands进行运动分析

注解

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

注解

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

在控制机构时,通常希望在两个位置之间平稳地移动它,而不是突然更改其设定值。这称为“运动分析”,并且在WPILib中通过“ TrapezoidProfile”类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.html>` __,C ++)得到支持。

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

TrapezoidProfileSubsystem

注解

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

TrapezoidProfileSubsystem类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.html>`__,C ++)将自动创建并执行梯形运动轮廓以达到用户提供的目标状态。要使用“ TrapezoidProfileSubsystem”类,用户必须创建它的子类。

创建一个TrapezoidProfileSubsystem

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

useState()

protected abstract void useState(TrapezoidProfile.State state);

“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.Goal(5, 3);

完整的TrapezoidProfileSubsystem示例

在实践中使用时,“ TrapezoidProfileSubsystem”是什么样的?以下示例摘自ArmbotOffobard示例项目(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
package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;

import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;

import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;

/**
 * A robot arm subsystem that moves with a motion profile.
 */
public class ArmSubsystem extends TrapezoidProfileSubsystem {
  private final ExampleSmartMotorController m_motor =
      new ExampleSmartMotorController(ArmConstants.kMotorPort);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /**
   * Create a new ArmSubsystem.
   */
  public ArmSubsystem() {
    super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond,
                                           ArmConstants.kMaxAccelerationRadPerSecSquared),
          ArmConstants.kArmOffsetRads);
    m_motor.setPID(ArmConstants.kP, 0, 0);
  }

  @Override
  public void useState(TrapezoidProfile.State setpoint) {
    // Calculate the feedforward from the sepoint
    double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
    // Add the feedforward to the PID output to get the motor output
    m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position,
                        feedforward / 12.0);
  }
}

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

63
64
65
66
67
68
69
    // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
    new JoystickButton(m_driverController, Button.kA.value)
        .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);

    // Move the arm to neutral position when the 'B' button is pressed.
    new JoystickButton(m_driverController, Button.kB.value)
        .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);

TrapezoidProfileCommand

注解

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

TrapezoidProfileCommand类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.html>`__,C ++)允许用户创建一个执行单个``TrapezoidProfile’’的命令,将每次迭代的当前状态传递给用户定义的函数。

与“TrapezoidProfileSubsystem”一样,用户可以通过将“TrapezoidProfileCommand”类作为子类来创建“TrapezoidProfileCommand”。但是,与基于指令的库中的许多其他指令类一样,用户可能希望通过定义“TrapezoidProfileCommand”:ref:`inline <docs/software/commandbased/convenience-features:Inline Command Definitions>`来保存代码。

创建一个TrapezoidProfileCommand

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

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

28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
  /**
   * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
   * Output will be piped to the provided consumer function.
   *
   * @param profile      The motion profile to execute.
   * @param output       The consumer for the profile output.
   * @param requirements The subsystems required by this command.
   */
  public TrapezoidProfileCommand(TrapezoidProfile profile,
                                 Consumer<State> output,
                                 Subsystem... requirements) {
    m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
    m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
    addRequirements(requirements);
  }

profile

“profile”参数是将由指令执行的“TrapezoidProfile”对象。通过传递此参数,用户可以指定指令将使用的配置文件的开始状态,结束状态和运动约束。

output

“output”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),它消耗控制回路的输出和设定值。在“PIDCommand”中传递“useOutput”函数在功能上类似于重写“PIDSubsystem”中的`useState()`_函数。

requirements

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

完整的TrapezoidProfileCommand示例

在实践中使用时,“ TrapezoidProfileSubsystem”是什么样的?以下示例取自DriveDistanceOffboard示例项目(Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard>`__,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
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;

import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;

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

/**
 * Drives a set distance using a motion profile.
 */
public class DriveDistanceProfiled extends TrapezoidProfileCommand {
  /**
   * Creates a new DriveDistanceProfiled command.
   *
   * @param meters The distance to drive.
   * @param drive The drive subsystem to use.
   */
  public DriveDistanceProfiled(double meters, DriveSubsystem drive) {
    super(
        new TrapezoidProfile(
            // Limit the max acceleration and velocity
            new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond,
                                             DriveConstants.kMaxAccelerationMetersPerSecondSquared),
            // End at desired position in meters; implicitly starts at 0
            new TrapezoidProfile.State(meters, 0)),
        // Pipe the profile state to the drive
        setpointState -> drive.setDriveStates(setpointState, setpointState),
        // Require the drive
        drive);
    // Reset drive encoders since we're starting at 0
    drive.resetEncoders();
  }
}

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

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