# FRC Java Command-Based

快比賽了來複習

### 專案架構
robot
|- commands
|- Command_1.java
|- Command_2.java
|- .......
|- subsystems
|- Subsystem_1.java
|- Subsystem_2.java
|- .......
|- Constants.java
|- RobotContainer.java
|- Main.java
|- Robot.java

### Subsystem
Subsystem 可以把他想成結構的程式實作，就像是生物課教的人的系統。
以呼吸系統舉例，呼吸系統內有呼吸道、肺和呼吸肌等等器官組織，可以將他們視為一個 Subsystem class 內的物件(像是馬達、氣壓管等)，而呼吸、和肺泡交換氧氣等動作，就可以想像成 Subsystem class 內的函式(像是設定馬達速度等動作)。一個 Subsystem 為機器人中的一個系統，而多個 Subsystem 就可組成你的機器人啦，像是人就需要8個Subsystem。

這個類別內有這個結構內所需的所有變量、執行動作的函式供 Command 來使用。
舉個例子，當今天我們的結構做出了一個可以用兩顆馬達射 note 的機器人
Like this:

而我們可以發現除了 shooter，還有 swerve 底盤、intaker 這些部件，那我們就可以創建出 Shooter.java, Swerve.java, Intaker.java 三個檔案來實作結構的程式。這邊以 Shooter.java 為例子:

// Shooter.java

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Shooter extends SubsystemBase {

private Spark lmotor = new Spark(0);
private Spark rmotor = new Spark(1);

private double value;

// 建構子，讓馬達停下
public Shooter() {
stop();
}

public void stop() {
value = 0;
}

public void set(double value) {
this.value = value;
}

@Override
public void periodic() {
lmotor.set(value);
rmotor.set(-value);
}
}

假設我們的 Motor controller 為 Spark，直接用 Spark 宣告我們的馬達

private Spark lmotor = new Spark(0);
private Spark rmotor = new Spark(1);

還有馬達要設的值

private double value;

可以發現他們是用 private 宣告的，這些 class 內的物件跟變數盡量設為私有成員，避免可以在外改到裡面的資料，讓程式碼非常混亂(多寫就知道了 ¬ (・∀・)¬
所以我們創建了兩個 public 宣告的 function 來操控馬達，分別為 stop() 跟 set()

public void stop() {
value = 0;
}

public void set(double value) {
this.value = value;
}

我們的 Command 主要就是透過這些函式來操控馬達跟變量
然後你會發現，最下面我們覆寫(Override)了一個 SubsystemBase 內的函式叫 periodic，這個函式將會在你的 Shooter 被實例化後開始執行(就是在機器人打開後一直跑)。
仔細觀察，我們的 stop() 跟 set() 都沒有直接去操控馬達，而是在 periodic() 這個函式內去操控馬達，這是為了防止再 Command 結束前動作還沒執行完就被停止而無法繼續執行到目標狀態的事情發生，像是我寫了一個漸進式加速的 Shooter 並直接用 set 去操控馬達:

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Shooter extends SubsystemBase {

private Spark lmotor = new Spark(0);
private Spark rmotor = new Spark(1);

private double speed, target_speed;

public Shooter() {
speed = 0;
set(0);
}

public void set(double value) {
target_speed = value;

if(speed < target_speed) speed += 0.5;
else if(speed > target_speed) speed -= 0.5;

if(speed>1) speed = 1;
f(speed<-1) speed = -1;

lmotor.set(value);
rmotor.set(-value);
}

}

這段程式碼會在 Command 執行 set 函式時進行加速或減速直到 speed = target_speed，但是當今天 Command 結束，並執行 set(0) 嘗試使馬達停下時， Subsystem 就會因為 Command 已經結束而沒辦法計算跟設置馬達的速度，然後馬達就會一直轉，直到 Subsystem 再次執行這個 Command。

所以我們可以修改一下上面的程式碼:

package frc.robot.subsystems;

import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class Shooter extends SubsystemBase {

private Spark lmotor = new Spark(0);
private Spark rmotor = new Spark(1);

private double speed, target_speed;

public Shooter() {
speed = 0;
target_speed = 0;
}

public void stop() {
target_speed = 0;
}

public void set(double value) {
target_speed = value;
}

@Override
public void periodic() {
if(speed < target_speed) speed += 0.05;
else if(speed > target_speed) speed -= 0.05;

if(speed>1) speed = 1;
if(speed<-1) speed = -1;

lmotor.set(speed);
rmotor.set(speed);
}
}

這樣子這個 Subsystem 在 Command 結束後也會繼續偵測 speed 跟 target_speed 的值並繼續設置馬達的速度(Command 結束時要記得讓馬達停下，stop())。(以一開始介紹的人體系統為例，periodic 就像是呼吸為呼吸系統不間段、要一直做的事情，像是跟肺泡交換氧氣。而 set(值) 就像是我們去控制呼吸的量而已。）
其他兩個 Subsystem 也就差不多(swerve 有很多的計算!)，可以自己嘗試寫寫看。

### Command
前面我們已經做好了 Shooter 的 Subsystem 了(用第一個寫的 Shooter.java 為例)，但我們要怎麼去執行裡面的 set() 跟 stop()函式呢?就是透過 Command 啦!
可以把 Command 想想成你大腦傳送的指令，去操控你身體上部位的系統們(Subsystem)，而要注意的是，一個 Subsystem 同時只能給一個 Command 操控(你應該沒辦法同時吸氣跟吐氣吧?)。

假如我今天要新增一個指令讓 Shooter 可以射 note，也就是把馬達的速度調到 1, 可以這麼寫:

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Shooter;

public class ShootNote extends Command {

private Shooter m_shooter;

public ShootNote(Shooter shooter) {
m_shooter = shooter;
addRequirements(m_shooter);
}

@Override
public void initialize() {
// do nothing
}

@Override
public void execute() {
m_shooter.set(1);
}

@Override
public void end(boolean interrupted) {
m_shooter.stop();
}

@Override
public boolean isFinished() {
return false;
}
}

我們需要宣告一個目標 Subsystem 的參考

private Shooter m_shooter;

要注意這只是一個參考值，不要給他實例化!

// Don't do this
private Shooter m_shooter = new Shooter();

以上的程式碼可以發現，建構子需要得到這個 Command 所需要用到的的 Subsystem，然後把他們拿過來變稠自己的(m_subsystem)，之後做 addRequirements 這個動作。addRequirements 這個動作非~常重要，他的意思是，向這個 Subsystem 詢問是否有 Command 正在操作它，沒的話，直接使用;有的話先看正在使用此 Subsystem 的 Command 內的 InterruptedBehavior 設置，如果為 CancelSelf 或此 Command 為此 Subsystem 的 DefaultComamnd，那麼此 Subsystem 的 Command 退出，我這個指令就可以使用這個 Subsystem; 如果為 CancelIncomming 則繼續執行正在使用此 Subsystem 的 Command。上面這一長串可能不太好懂，可以看看下面這張圖:


然後我們分別覆寫了四個函式 initialize(), execute(), end() 和 isFinished()

1. initialize()
指令開始時做的事情(只跑一次)。
2. execute()
指令執行中做的事情(一直跑，直到指令結束)。
3. end()
指令結束時做的事情(只跑一次)。
4. isFinished()
如果回傳 false 繼續執行此指令;
如果回傳 true 則結束此指令。

然後記得，我們要操控的對象為 Subsystem 的參考，也就是這裡的 m_shooter，可以想像成我們成功拿到此 Subsystem 的控制權後(addRequirement 成功)，操控我們拿到的 Subsystem (在建構子拿到 Shooter 實例的 m_shooter)。

但今天不只是要做一件固定的事情，而是要由從搖桿獲得的值來操控 Subsystem 呢? 可以這麼寫:

package frc.robot.commands;

import java.util.function.Supplier;

import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.Shooter;

public class ShootNote extends Command {

private Shooter m_shooter;
private Supplier<Double> m_value;

public ShootNote(Shooter shooter, Supplier<Double> value) {
m_shooter = shooter;
m_value = value;
addRequirements(m_shooter);
}

@Override
public void initialize() {
}

@Override
public void execute() {
m_shooter.set(m_value.get());
}

@Override
public void end(boolean interrupted) {
m_shooter.stop();
}

@Override
public boolean isFinished() {
return false;
}
}

在這裡我們用 Supplier 宣告了一新的物件儲存拿到的函式 (Supplier<Double> 的 Double 指的是這個物件回傳的值為 double 型態)，用這個物件來即時獲得此函數(通常是 Joystick.GetRawAxies())所取得的值。(通常用在 Default command 上)

### RobotContainer.java

直接翻譯的話是......容器?
Subsystem 的實例都會宣告在此。
這個檔案功能就是把我們所寫好的 Command 綁定到特定的搖桿按鈕或 Subsystem 的 DefualtCommmand 上，統整所有 Subsystem 跟 Command 與 搖桿的關係。

### Constants.java
放常數的地方，例如機器人的長、寬、高、intake 馬達的速度、Shooter 仰角的最大最小值等等固定不變的數，都用 final(c++ 的 const) 在這裡宣告宣告。
這樣子只需在這改數字就可以一次改到所有用到此變數的值，不用一個一個去改。

### Robot.java
將 RobotContainer 實例化並開始 CommandSchedualer 的地方，通常不會去改到任何程式碼。

### Main.java
不要動他 !! Dangerous

## 進階

### 更多 Command
不止這種只執行單個指令的 Command，還有可以同時執行多個指令、等待時間等等雜七雜八的 Command 種類。
(非常多，不只這些)
每種 Command 都有其獨特的功能，以下是一些經常用到的指令種類。

#### PrintCommand
這個 Command 會在你的 Console 上輸出你給他的值，用來 Debug 很方便。

#### WaitCommand
這個 Command 會讓你的 Subsystem 停下你設定的時間(秒)，直到時間結束或 Subsystem 被其他 Command 拿去用才會停下。

#### WaitUntilCommand
WaitCommand 的昇級版，吃一個 BooleanSupplier(會傳布林值的 Command)，他會跑到你的 Function 回傳 True 後或 Subsystem 被其他 Command 拿走才會停下(像 while())。通常會跟 ParallelCommandGroup 一起使用。

#### ParallelCommandGroup
可以創建一個同時執行多個 Command 的 Comamnd，通常組合鍵的 Command 會繼承此 class 來達成同時執行多個 Comamnd 的事情。

#### Commands.sequence
可以創建一個線性執行的函式，可以回傳讓 Command 照順序一個個執行下去的 Command。

#### 範例
我們來試試看創建一個 Command，我們想要在按下按鈕時，開始跑 Shooter，直到我們按下按鈕才運送 Note 到 Shooter 射出去。

// CombinationCommand.java

package frc.robot.commands;

import java.util.function.BooleanSupplier;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitUntilCommand;

import frc.robot.subsystems.Shooter;
import frc.robot.subsystems.Transfer;

public class CombinationCommand extends ParallelCommandGroup {
public CombinationCommand(Shooter shooter, Transfer transfer, BooleanSupplier condition) {

addRequirements(transfer, shooter);

addCommands(
new ShooterShoot(shooter),
Commands.sequence(
new WaitUntilCommand(condition),
new Transport(transfer)
)
);

}
}

我們這次繼承的 class 為 ParallelCommandGroup，也就是一個可以同時執行多個 Command 的 Comamnd。我們在建構子先 addRequirements 會用到的 Subsystem

addRequirements(transfer, shooter);

之後用 ParallelCommandGroup 內建的 add() 函式來新增此 class 要執行的 Command

addCommands(
new ShooterShoot(shooter),
Commands.sequence(
new WaitUntilCommand(condition),
new Transport(transfer)
)
);

分別為 ShooterShoot 跟 Commands.sequence。而 Commands.sequence 內有著兩個 Command 會依次執行

Commands.sequence(
new WaitUntilCommand(condition),
new Transport(transfer)
)

來整理這個 Command 發生什麼事吧。首先，addRequiredment，然後用 addCommands 函式將數個 Command 新增到 group 中，group 內的 Command 會同時開始執行。在 addCommands 裡的 Command 都是同時進行的，而 Commands.sequence 內的 Command 是依序進行的。所以會先執行 ShooterShoot 跟 Commands.sequence 內的 Command WaitUntilCommand。在這之後直到 condition 內函式所回傳的值為 True 後(也就是 WaitUntilCommand 結束後)，才會執行 sequence 的下個函式 Transport (注意!到這邊 ShooterShoot 還是在運行的)，最後跑 Transfer，射出 note。

還有很多宣告 Command 的方式，團隊盡量討論出一個固定的格式在開始編碼不然會hen~亂。