###### tags: `程式組教程`

{%hackmd theme-dark %}

# Command Based Programming

A simplified note with our experience included

If you want a more complete but more sophisticated explanation of command based programming, visit [WPILib's Doc](https://docs.wpilib.org/en/stable/docs/software/commandbased/index.html)

Table of Contents
* [Introduction of Concepts](#Introduction-of-Concepts)
* [What is Command Based Programming](#What-is-Command-Based-Programming)
* [Command Structure](#Command-Structure)
* [Subsystems](#Subsystems)
* [Command Based Project Structure](#Command-Based-Project-Structure)
* [The Basics](#The-Basics)
* [Creating a command based project](#Creating-a-command-based-project)
* [Subsystems](#Subsystems1)
* [Subsystem Structure: the details](#Subsystem-Structure-the-details)
* [Subsystem Tips](#Subsystem-Tips)
* [Commands](#Commands)
* [Command Structure: the details](#Command-Structure-the-details)
* [Command Tips](#Command-Tips)
* [CommandScheduler](#CommandScheduler)
* [What is it?](#What-is-it)
* [How do I use it?](#How-do-I-use-it)
* [Advanced Usage](#Advanced-Usage)
* [Command Groups](#Command-Groups)
* [Inline Command](#Inline-Command)

## Introduction of Concepts

### What is Command Based Programming?

In its essence, command based programming is a type of robot code structure in which complicated robot instruction and code are simplified and packaged into "commands" that we can use repetitively. It is very similar to how we use functions to simplify complicated process. However, commands comes with a lot of handy tools that the wpilib library provides.

### Command Structure

Each command is a java class that extends one of wpilib's command superclasses (Usually CommandBase). For example:

public class IntakeCmd extends CommandBase

Each command must implement the following methods:
* Constructor: IntakeCmd()
* initialize()
* execute()
* end()

Usually, you will also need isFinished() or interrupted()
Here is how it works!


After constructing the command instance and calling it, **initialize()** will be called. This is similar to things like TeleopInit(), in which necessary things are done before the actions take place.

Then the **execute()** method is called, this is where most actions are done during the process. For example, the intake motors will move so that the ball is carried into the robot's carrier.

Lastly, after executing all actions, the **end()** method will be called. This method make sure that everything is returned to necessary condition after finishing (for example, setting motor speed back to 0)

### Subsystems

You may or may not have heard about subsystems, but they are a crucial part of command based programming.

Normally, we just directly refer to the motors and other parts of our robot in our robot.java file. You might have noticed that as the number of devices increase, this makes the robot.java file increasingly complex. To make everything less like a spaghetti code, we organize related parts of the robot into subsystems: a collection of devices and functions to control them.

For example, the drive subsystem DriveTrain will include all motors on the base that controls the movement of the robot, as well as functions like drive() that controls them.

### Command Based Project Structure

Here's part of the project structure from last year


This might look complicated, but it's actually pretty simple.

We have one folder Subsystems that contains all the java files that defines the subsystems of the robot. The Commands folder has sub-folders according to the subsystems, and respective commands in them.

The other files are located at the base folder.

## The Basics

### Creating a command based project

The easiest way to do this is to use the template in WPILib Visual Studio Code.

1. In WPILib Visual Studio Code, execute the command "Create a new project"


2. When selecting project type, select "Template", "java", "Command Robot"
3. Enter the rest of the information as usual
4. Create the project, you should see the default structure show up as follows:


### Subsystems

#### Subsystem Structure: the details

Let's take a look at the more detailed part of a subsystem using an example. I use the intake subsystem from 2022 code base because it's one of the more simple one.

package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.IntakeConstants;
//import frc.robot.Constants.TransporterConstants;
import frc.robot.Constants.TransporterConstants;

public class Intake extends SubsystemBase {

  private final WPI_TalonSRX intaker = new WPI_TalonSRX(IntakeConstants.kIntakeID);;
  private final WPI_TalonSRX downTransporter = new WPI_TalonSRX(TransporterConstants.kDownTransporterID);

  /** Creates a new IntakeSubsystem. */
  public Intake() {
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }

  public void intakeRun(){
    intaker.set(IntakeConstants.intakeSpeed);
    downTransporter.set(IntakeConstants.transportSpeed);

  }

  public void intakeSet(double speed) {
    intaker.set(speed);
    downTransporter.set(speed);
  }

  public void intakeStop(){
    intaker.set(0);
    downTransporter.set(0);
  }

  public void intakeReverse() {
    intaker.set(-IntakeConstants.intakeSpeed);
    downTransporter.set(-IntakeConstants.transportSpeed);
  }

  public void downRun() {
    downTransporter.set(-IntakeConstants.transportSpeed);
  }

  public void downStop() {
    downTransporter.set(0);
  }
}

We can analyze this subsystem section by section:

**1.Class declaration**

public class Intake extends SubsystemBase {

Every subsystem must extend the class SubsystemBase from WPILib. Make sure to include the corresponding import.

**2.Field declaration**

private final WPI_TalonSRX intaker = new WPI_TalonSRX(IntakeConstants.kIntakeID);;
private final WPI_TalonSRX downTransporter = new WPI_TalonSRX(TransporterConstants.kDownTransporterID);

Here we declare the devices and possibly other variables that we will use in the subsystem. The declared ones are the 2 motors for the intake subsystem using WPI_TalonSRX motor controller.

Notice the use of **private** keyword. Although it is not necessary, it is usually a good practice to limit the access of fields. Also be mindful of the **final** keyword. We only use it here because the motors are unchanging and the subsystem will always use these two motors.

**3.Constructor**

public Intake() {
}

The constructor of the intake subsystem is not particularly interesting, but as you can see in the Turret subsystem constructor, we usually set up the property of certain important fields such as devices in the constructor.

public Turret() {
    masterFlyWheel.setInverted(true);
    slaveFlyWheel.setInverted(false);
    slaveFlyWheel.follow(masterFlyWheel, true);
}

**4.periodic()**
This function will be executed periodically starting from the point when the subsystem's constructor is called. Take a look at Drive subsystem's periodic()

public void periodic() {
    // System.out.println(m_mecanumOdometry.getPoseMeters());
    m_differentialOdometry.update(
        m_gyro.getRotation2d(),
        motorFL.getSelectedSensorPosition() * DriveConstants.kEncoderDistancePerPulse / 10,
        motorFR.getSelectedSensorPosition() * DriveConstants.kEncoderDistancePerPulse / 10
    );
  }

Since the odometry (essentially the robot's predicted position) need to be updated whenever the base motor moves, the subsystem periodically updates it from the point when the Drive subsystem is initiated.

**5.Other methods**
According to each subsystem, other functions/methods might be added to make the subsystem easier to use.
For example, the intake subsystem has the following functions:

  public void intakeRun(){
    intaker.set(IntakeConstants.intakeSpeed);
    downTransporter.set(IntakeConstants.transportSpeed);

  }

  public void intakeSet(double speed) {
    intaker.set(speed);
    downTransporter.set(speed);
  }

  public void intakeStop(){
    intaker.set(0);
    downTransporter.set(0);
  }

  public void intakeReverse() {
    intaker.set(-IntakeConstants.intakeSpeed);
    downTransporter.set(-IntakeConstants.transportSpeed);
  }

  public void downRun() {
    downTransporter.set(-IntakeConstants.transportSpeed);
  }

  public void downStop() {
    downTransporter.set(0);
  }

Each serves to simplify the working of a subsystem.

To sum up, the structure of a subsystem is:

imports
class declaration {
    
    fields
    
    constructor
    
    periodic()
    
    other methods
    
}

#### Subsystem Tips

* **Avoid refering to other subsystems when writing a subsystem**: each subsystem should be able to work individually without cross reference. The reference may cause potential tangling of access priority to hardware.
* **Do not involve inputs from higher level**: For example, do not include joystick input into the subsystem (that is, unless it is a joystick subsystem). These higher level input should be deal with at a higher level (usually robotContainer.java). Using it at lower level will likely cause conflict for the control of the access to the device.
* **Organize constants by subsystems**: 1. always put the constants in constants.java instead of the subsystem itself for easier organization. 2. always organize the constants by their respective subsystem. For example, in constants.java, constants used for Drive subsystem should be under DriveConstants class. For instance, DriveConstants.wheelDiameter.
* **Be careful about the encapsulation**:You shouldn't be able to directly access the subsystem's motor using things like subsystem.motor1. The motor and other fields should be encapsulated using **private** or other suitable keyword.

### Commands

#### Command Structure: the details

Besides the previously explained initiate(), execute(), and end(), there are other things a command will need. Let's explore this by writing an example.

First, declare the command. We may want to start from an easy one, like DriveForward.

public class DriveForward extends CommandBase {
}

We'll then need to add the needed fields. Most importantly, the subsystem that we'll use. Remember that each command will use at least one subsystem. We'll need to declare them here. Since we are driving, we'll need the DriveSubsystem.

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
}

To use the subsystem, however, we'll need to get the subsystem from a higher level. Hence, we'll need the constructor.

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
    
    public DriveForward(DriveSubsystem drive) {
        this.drive = drive;
        addRequirements(drive);
    }
}

This make sure that when we declared the command later, we can get to use the subsystem. The **addRequirements()** function is very important because it makes sure that other commands will not conflict with the usage of certain subsystem.

Let's say we want this command to let the robot go forward for 5 second at 50% speed.

We'll need to add some necessary fields and initialize

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
    double initTime;
    
    public DriveForward(DriveSubsystem drive) {
        this.drive = drive;
        addRequirements(drive);
    }
    
    @Override
    public void initialize(){
        initTime=Timer.getFPGATimestamp();
    }
}

Then the execution part, where the robot actually moves.

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
    double initTime;
    
    public DriveForward(DriveSubsystem drive) {
        this.drive = drive;
        addRequirements(drive);
    }
    
    @Override
    public void initialize(){
        initTime=Timer.getFPGATimestamp();
    }
    
    @Override
    public void execute(){
        drive.arcadeDrive(0.5,0);
    }
}

Lastly, we want the bot to stop at the end.

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
    double initTime;
    
    public DriveForward(DriveSubsystem drive) {
        this.drive = drive;
        addRequirements(drive);
    }
    
    @Override
    public void initialize(){
        initTime=Timer.getFPGATimestamp();
    }
    
    @Override
    public void execute(){
        drive.arcadeDrive(0.5,0);
    }
    
    @Override
    public void end(boolean Interrupted){
        drive.arcadeDrive(0,0);
    }
}

You might wonder that how does the bot know when to stop?
This is where **isFinished()** kicks in.
isFinished() will tell the bot when this command should end. It returns true when the command should end, false if it should continue. In this case, we want the command to end after 5 seconds.

public class DriveForward extends CommandBase {
    private DriveSubsystem drive;
    double initTime;
    
    public DriveForward(DriveSubsystem drive) {
        this.drive = drive;
        addRequirements(drive);
    }
    
    @Override
    public void initialize(){
        initTime=Timer.getFPGATimestamp();
    }
    
    @Override
    public void execute(){
        drive.arcadeDrive(0.5,0);
    }
    
    @Override
    public void end(boolean Interrupted){
        drive.arcadeDrive(0,0);
    }
    
    @Override
    public boolean isFinished(){
        if (Timer.getFPGATimestamp()>initTime+5){
            return true;
        }
        return false;
    }
}

In some commands, you might need another special function, **interrupted()**. Sometimes, a command is set to end only when other command is scheduled in its place. Therefore, we need a way to turn off the executing devices when this happens. Interrupted() will execute when the command is interrupted, solving the issue. For example:

public void interrupted(){
    drive.arcadeDrive(0,0);
}

This will make sure that when the command is interrupted, the robot will stop automatically.

To sum up, the structure of a command is:

imports
class declaration {
    fields (usually subsystems)
    
    constructor()
    
    initialize()
    
    execute()
    
    end()
    
    isFinished()
    
    interrupted() //optional
}

#### Command Tips

* **Do not create a new instance of a subsystem inside the command**:always use the subsystem by passing it in from a higher level. Creation of another subsystem instance maybe often cause conflict for the access of devices.
* **Make sure there is always a way to stop the command**: be careful when designing isFinished() and interrupted() so that the command can be ended correctly. Try not to use interruption as a way to end the command, as this usually will make the program a lot more complex, especially with the **CommandScheduler**(sometimes this might be necessary though).

### CommandScheduler

#### What is it?

Now, we have learned how to build commands, but how do we use them? This is the job of the CommandScheduler

The CommandScheduler allows us to **schedule** command, which it will run accordingly. The CommandScheduler is the highest level access point of commands, and should be positioned inside robotContainer.java

CommandScheduler is a class with one single instance (singleton), so to get it, you must use CommandScheduler.getInstance()

#### How do I use it?

First, add CommandScheduler.getInstance().run() to the robotPeriodic() function in robot.java, this allows the scheduled commands to actually execute.

Then, inside robot container, you may use CommandScheduler.getInstance().schedule() to add any command into the queue and execute it. For example:

CommandScheduler.getInstance().schedule(new DriveForward(drive))
// drive is a DriveSubsystem instance

This is the basic of using CommandScheduler, and usually you won't need much beside this knowledge.

However, if you are interested, or if you encountered strange issues, check [WPILib's explanation](https://docs.wpilib.org/en/stable/docs/software/commandbased/command-scheduler.html)

## Advanced Usage

### Command Groups

Sometimes, we want to organize multiple commands together into a larger command, or we might want multiple commands to execute in a certain way. This is the job of command groups. There are several types of command groups, each with its own special function.

* SequentialCommandGroup

SequentialCommandGroup, as its name suggests, execute the commands sequentially. Meaning it execute command1, command2, command3...

Example:

SequentialCommandGroup group = new SequentialCommandGroup(new Command1(), new Command2())

* ParallelCommandGroup

Runs all commands concirrently (meaning at the same time). One of the most important command group because it allows parallel execution of complex actions.
**This command group ends only when every command inside of it finished**

* ParallelRaceGroup

Same as ParallelCommandGroup. **Except it ends whenever any command inside it finished**

* ParallelDeadlineGroup

Same as ParallelCommandGroup. However, it ends when the deadline command ends. During its declaration, the first command in the constructor will be set as the deadline command

new ParallelDeadlineGroup(new Command1(), new Command2())
    //Command1 is the deadline

**Declaring Command Groups**

We usually just declare commands using its inline form as shown above.

However, you can also create a new java class for the command group as well.

public class ExampleCommandGroup extends SequentialCommandGroup {
    public ExampleCommandGroup() {
        addCommands(
            new Command1(),
            new Command2()
        )
    }
}

**Recursive Command Group Statement**

You can have command groups within command groups, for instance:

new SequentialCommandGroup(
    new Command1(),
    new ParallelCommandGroup(
        new Command2(),
        new Command3()
    )
)

### Inline Command

As we mentioned before, although most commands will extend class CommandBase, there are other commands available for us. However, instead of extending these classes, we usually declare these other commands inline, which is why we call them inline commands.

There are too many types of inline commands, so I won't include them in this tutorial. You can easily access them at [WPILib's doc](https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html). (It's a bit down in the page though)

Some frequently used inline commands are **ConditionalCommand** and **RunCommand**, which I strongly suggests you to take a look.