子系统

Subsystems are the basic unit of robot organization in the command-based paradigm. A subsystem is an abstraction for a collection of robot hardware that operates together as a unit. Subsystems form an encapsulation for this hardware, “hiding” it from the rest of the robot code and restricting access to it except through the subsystem’s public methods. Restricting the access in this way provides a single convenient place for code that might otherwise be duplicated in multiple places (such as scaling motor outputs or checking limit switches) if the subsystem internals were exposed. It also allows changes to the specific details of how the subsystem works (the “implementation”) to be isolated from the rest of robot code, making it far easier to make substantial changes if/when the design constraints change.

Subsystems also serve as the backbone of the CommandScheduler’s resource management system. Commands may declare resource requirements by specifying which subsystems they interact with; the scheduler will never concurrently schedule more than one command that requires a given subsystem. An attempt to schedule a command that requires a subsystem that is already-in-use will either interrupt the currently-running command or be ignored, based on the running command’s Interruption Behavior.

Subsystems can be associated with “default commands” that will be automatically scheduled when no other command is currently using the subsystem. This is useful for “background” actions such as controlling the robot drive, keeping an arm held at a setpoint, or stopping motors when the subsystem isn’t used. Similar functionality can be achieved in the subsystem’s periodic() method, which is run once per run of the scheduler; teams should try to be consistent within their codebase about which functionality is achieved through either of these methods. Subsystems are represented in the command-based library by the Subsystem interface (Java, C++).

创建子系统

The recommended method to create a subsystem for most users is to subclass the abstract SubsystemBase class (Java, C++), as seen in the command-based template (Java, C++):

 7import edu.wpi.first.wpilibj2.command.CommandBase;
 8import edu.wpi.first.wpilibj2.command.SubsystemBase;
 9
10public class ExampleSubsystem extends SubsystemBase {
11  /** Creates a new ExampleSubsystem. */
12  public ExampleSubsystem() {}
13
14  /**
15   * Example command factory method.
16   *
17   * @return a command
18   */
19  public CommandBase exampleMethodCommand() {
20    // Inline construction of command goes here.
21    // Subsystem::RunOnce implicitly requires `this` subsystem.
22    return runOnce(
23        () -> {
24          /* one-time action goes here */
25        });
26  }
27
28  /**
29   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
30   *
31   * @return value of some boolean subsystem state, such as a digital sensor.
32   */
33  public boolean exampleCondition() {
34    // Query some boolean state, such as a digital sensor.
35    return false;
36  }
37
38  @Override
39  public void periodic() {
40    // This method will be called once per scheduler run
41  }
42
43  @Override
44  public void simulationPeriodic() {
45    // This method will be called once per scheduler run during simulation
46  }
47}

此类在基本的“Subsystem”接口之上包含一些便利功能:它会在其构造函数中自动调用”register()”方法以向调度程序注册子系统(这对于在调度程序运行时调用“periodic()”方法是必须的),并实现“Sendable”接口,以便将其发送到仪表板以显示/记录相关状态信息。

寻求更高灵活性的高级用户可以简单地创建一个实现“Subsystem”接口的类。

简单子系统示例

What might a functional subsystem look like in practice? Below is a simple pneumatically-actuated hatch mechanism from the HatchBotTraditional example project (Java, C++):

 5package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
 6
 7import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
 8import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
 9
10import edu.wpi.first.util.sendable.SendableBuilder;
11import edu.wpi.first.wpilibj.DoubleSolenoid;
12import edu.wpi.first.wpilibj.PneumaticsModuleType;
13import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
14import edu.wpi.first.wpilibj2.command.SubsystemBase;
15
16/** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */
17public class HatchSubsystem extends SubsystemBase {
18  private final DoubleSolenoid m_hatchSolenoid =
19      new DoubleSolenoid(
20          PneumaticsModuleType.CTREPCM,
21          HatchConstants.kHatchSolenoidPorts[0],
22          HatchConstants.kHatchSolenoidPorts[1]);
23
24  /** Grabs the hatch. */
25  public void grabHatch() {
26    m_hatchSolenoid.set(kForward);
27  }
28
29  /** Releases the hatch. */
30  public void releaseHatch() {
31    m_hatchSolenoid.set(kReverse);
32  }
33
34  @Override
35  public void initSendable(SendableBuilder builder) {
36    super.initSendable(builder);
37    // Publish the solenoid state to telemetry.
38    builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == kForward, null);
39  }
40}

请注意,子系统从外部代码隐藏了DoubleSolenoid的存在(被声明为``private’’),而是公开公开了两个更高级别的描述性机器人动作:``grabHatch()’’和``releaseHatch()’’ ``。以这种方式“隐藏”双螺线管之类的“实现细节”是极为重要的;这样可以确保子系统外部的代码永远不会导致螺线管处于意外状态。它还允许用户更改实现方式(例如,可以使用电动机代替气动方式),而子系统外部的任何代码都不必随之更改。

Alternatively, instead of writing void public methods that are called from commands, we can define the public methods as factories that return a command. Consider the following from the HatchBotInlined example project (Java, C++):

 5package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
 6
 7import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
 8import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
 9
10import edu.wpi.first.util.sendable.SendableBuilder;
11import edu.wpi.first.wpilibj.DoubleSolenoid;
12import edu.wpi.first.wpilibj.PneumaticsModuleType;
13import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
14import edu.wpi.first.wpilibj2.command.CommandBase;
15import edu.wpi.first.wpilibj2.command.SubsystemBase;
16
17/** A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */
18public class HatchSubsystem extends SubsystemBase {
19  private final DoubleSolenoid m_hatchSolenoid =
20      new DoubleSolenoid(
21          PneumaticsModuleType.CTREPCM,
22          HatchConstants.kHatchSolenoidPorts[0],
23          HatchConstants.kHatchSolenoidPorts[1]);
24
25  /** Grabs the hatch. */
26  public CommandBase grabHatchCommand() {
27    // implicitly require `this`
28    return this.runOnce(() -> m_hatchSolenoid.set(kForward));
29  }
30
31  /** Releases the hatch. */
32  public CommandBase releaseHatchCommand() {
33    // implicitly require `this`
34    return this.runOnce(() -> m_hatchSolenoid.set(kReverse));
35  }
36
37  @Override
38  public void initSendable(SendableBuilder builder) {
39    super.initSendable(builder);
40    // Publish the solenoid state to telemetry.
41    builder.addBooleanProperty("extended", () -> m_hatchSolenoid.get() == kForward, null);
42  }
43}

Note the qualification of the RunOnce factory used here: this isn’t the static factory in Commands! Subsystems have similar instance factories that return commands requiring this subsystem. Here, the Subsystem.runOnce(Runnable) factory (Java, C++) is used.

For a comparison between these options, see Instance Command Factory Methods.

Periodic

Subsystems have a periodic method that is called once every scheduler iteration (usually, once every 20 ms). This method is typically used for telemetry and other periodic actions that do not interfere with whatever command is requiring the subsystem.

117  @Override
118  public void periodic() {
119    // Update the odometry in the periodic block
120    m_odometry.update(
121        Rotation2d.fromDegrees(getHeading()),
122        m_leftEncoder.getDistance(),
123        m_rightEncoder.getDistance());
124    m_fieldSim.setRobotPose(getPose());
125  }

There is also a simulationPeriodic() method that is similar to periodic() except that it is only run during Simulation and can be used to update the state of the robot.

Default Commands

备注

In the C++ command-based library, the CommandScheduler owns the default command object.

“Default commands” are commands that run automatically whenever a subsystem is not being used by another command. This can be useful for “background” actions such as controlling the robot drive, or keeping an arm held at a setpoint.

为子系统设置默认命令非常容易:简单地调用“CommandScheduler.getInstance().setDefaultCommand()”,或调用“Subsystem”接口的“setDefaultCommand()”方法:

CommandScheduler.getInstance().setDefaultCommand(exampleSubsystem, exampleCommand);
exampleSubsystem.setDefaultCommand(exampleCommand);

备注

A command that is assigned as the default command for a subsystem must require that subsystem.