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

注解

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

注解

与早期版本的PIDController不同,2020年的“ProfiledPIDController”类*同步运行*,并且不在其自己的线程中处理。因此,更改其“ period”参数将*不会*更改在任何这些包装类中运行的实际频率。用户除非确定自己在做什么,否则切勿修改“period”参数。

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

ProfiledPIDSubsystem

注解

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

ProfiledPIDSubsystem类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.html>`__,C ++)允许用户方便地使用内置PIDController创建子系统。为了使用ProfileDPIDSubsystem类,用户必须创建它的子类。

创建一个ProfiledPIDSubsystem

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

getMeasurement()

protected abstract double getMeasurement();

“getMeasurement”方法返回过程变量的当前测量值。 “PIDSubsystem”将自动从其“periodic()”块中调用此方法,并将其值传递给控制循环。

用户应重写此方法,以返回他们希望用作过程变量测量的任何传感器读数。

useOutput()

protected abstract void useOutput(double output, TrapezoidProfile.State setpoint);

“useOutput()”方法消耗PID控制器的输出和当前设定值状态(这通常对于计算前馈很有用)。 “PIDSubsystem”将自动从其“periodic()”块中调用此方法,并将控制环的计算输出传递给该方法。

用户应重写此方法,以将最终计算出的控制输出传递给其子系统的电机。

传递至控制器

用户还必须通过子类的超类构造函数调用将“ProfiledPIDController”传递给“ProfiledPIDSubsystem”基类。这用于指定PID增益,运动曲线约束和周期(如果用户使用的是非标准主机器人循环周期)。

可以通过调用“getController()”在构造函数主体中对控制器进行其他修改(例如启用连续输入)。

使用ProfiledPIDSubsystem

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

setGoal()

注解

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

“setGoal()”方法可用于设置“PIDSubsystem”的设定值。子系统将使用定义的输出自动跟踪到设定点:

// The subsystem will track to a goal of 5 meters and velocity of 3 meters per second.
examplePIDSubsystem.setGoal(5, 3);

enable()和disable()

“enable()”和“disable()”方法启用和禁用“ProfiledPIDSubsystem’”的自动控制。启用子系统后,它将自动运行运动曲线和控制循环并跟踪目标。禁用时,不执行任何控制。

另外,“enable()”方法重置内部ProfileDPIDController,“disable()”方法调用用户定义的`useOutput()`_方法,将output和setpoint都设置为“0”。

完整ProfiledPIDSubsystem示例

在实际使用中,PIDSubsystem是什么样的?以下示例摘自ArmBot示例项目(Java <https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot>`__,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
42
43
44
45
46
47
48
49
50
51
52
53
package edu.wpi.first.wpilibj.examples.armbot.subsystems;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.controller.ArmFeedforward;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;

import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;

/**
 * A robot arm subsystem that moves with a motion profile.
 */
public class ArmSubsystem extends ProfiledPIDSubsystem {
  private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
  private final Encoder m_encoder =
      new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
  private final ArmFeedforward m_feedforward =
      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);

  /**
   * Create a new ArmSubsystem.
   */
  public ArmSubsystem() {
    super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
        ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
    m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
    // Start arm at rest in neutral position
    setGoal(ArmConstants.kArmOffsetRads);
  }

  @Override
  public void useOutput(double output, 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.setVoltage(output + feedforward);
  }

  @Override
  public double getMeasurement() {
    return m_encoder.getDistance() + ArmConstants.kArmOffsetRads;
  }
}

通过命令使用“ProfiledPIDSubsystem”可以非常简单:

63
64
65
66
67
68
    // 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.enable();
        }, m_robotArm);

ProfiledPIDCommand

注解

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

ProfiledPIDCommand类(Java <https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.html>`__,C ++)允许用户使用内置ProfiledPIDController轻松创建命令。与ProfiledPIDSubsystem一样,用户可以通过对ProfiledPIDCommand类进行子类化来创建ProfiledPIDCommmand。但是,与基于命令的库中的许多其他命令类一样,用户可能希望通过定义inline <docs/software/commandbased/convenience-features:Inline Command Definitions>来保存代码。

创建一个PIDCommand

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

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

32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
  /**
   * 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) {
    requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
    requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
    requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
    requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand");

    m_controller = controller;
    m_useOutput = useOutput;
    m_measurement = measurementSource;
    m_goal = goalSource;
    m_requirements.addAll(Set.of(requirements));
  }

controller

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

当将“ProfiledPIDCommand”子类化时,可以通过调用“getController()”对构造函数主体中的控制器进行其他修改(例如,启用连续输入)。

measurementSource

“measurementSource”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),该函数返回过程变量的度量值。在“ProfiledPIDCommand”中传递“measurementSource”函数在功能上类似于重写“ProfiledPIDSubsystem”中的`getMeasurement()`_函数。

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

goalSource

“goalSource”参数是一个函数(通常作为:ref:`lambda <docs/software/commandbased/convenience-features:Lambda Expressions (Java)>`传递),返回该机制的当前目标状态。如果仅需要一个不变的目标,那么就存在一个需要一个不变的目标而不是供应商的过载。另外,如果希望目标速度为零,则存在以恒定距离而不是完整轮廓状态存在的过载。

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

useOutput

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

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

requirements

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

完整ProfiledPIDCommand示例

在实际使用中,“ ProfiledPIDCommand”是什么样的?以下示例来自GyroDriveCommands示例项目(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
46
47
48
49
50
51
52
53
54
55
package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;

import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;

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

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