【世界最大級の中高生ロボコン】FRCでのプログラムについて ~Main関係~

この記事は[こちらの記事](https://qiita.com/amidaandhero02/items/59c69d8facd2335f96ce)の一部です。まずはこちらをご覧ください。

##FIRST
最初に。

Main関係とは述べたものの、ふかーいふかーいわけがあって(ほんとはそんなにない)Robot.java関係の話になります。
要はMainの役割を担うのがRobot.javaってこと。

地味に重要なパートです。

##プログラムの方針

一番大事なのは**誤作動(意図しない挙動)をしない**こと。

その次に大事なものは、**ロボットに負荷をかけさせないこと**だと思う。

ロボットはループ処理を行っています。
普通にコードを書いていくとしたら、何かしらの入力があればその入力が入った瞬間にモーター等に命令を出すって形になりますよね？
しかし、この方式だと問題が生じる(ことがあります)

例えば...
※関数は簡単のため架空のものです

>足回りをコントローラーで操作するところを実装しよう！

```Java:
setSpeed(controller.getX());
```

>段差に上るとき自動で足回り動かしたいなあ。いろんな部分動かすし、段階的にするからいろいろ判定させないとな。
setSpeed()のところにif else使って付け足しとくか！

```Java:
setSpeed(controller.getX());

(中略)

if(controller.is_cimbMode) {
    if(is_liftup){
    (中略)
    if(is_advance) {
        setSpeed(1);
    }
}
```

ここで一つ目の壁。
このままだと、モーターへの入力が短時間の間にコントローラーからの入力と1をひたすら繰り返すようになってしまう。

そもそもこれだとうまく動かないし、
足回りならともかくリフトでやるとロボットに負担がかなりかかる。
(ヘッドバンキングを思い浮かべるとわかりやすいかもしれない)
　
そこで、if elseで対処するようにしようとしても、複雑化 + どういう流れで作業が行われるのかかなりわかりにくくなってしまう。
こんな風に、、、

```Java:
if(controller.is_climbMode) {
    if(is_StopperOn && !is_liftMoving) {
        setSpeed(0.3);
        Timer.reset();
        Timer.start();
    }else if(is_liftUp && Timer.get() > 1) {
        setSpeed(1);
    }else {
        setSpeed(0);
    }
}else if(controller.hoge) {
    if(fuga) {
        setSpeed(-0.7);
    }else {
        setSpeed(0);
    }
}else {
    setSpeed(controller.getX());
```
※これはイメージです

これが足回りだけならともかく、いろんなところに量産されていく...:wave:

ここで僕は思いついた
>いろいろ判定させて状況を確定させてから動かしゃええやん...

と。メンター(指導者的なもの)によると`StateBot`とか言うらしい。


・コントローラーパート
・段差登るパート
・ほげパート
みたいに分けられるし。

実行する内容の優先順位を決めて、優先順位が高い処理をコード的に後に書けばOK

状況を確定させるためになにかしらクラスが欲しいから、作ったクラスが`State`

ただし、毎ループ`State`のフィールドは初期化した方がいいです。
一度操作された値がもう一度操作されないと変わらないかもしれないからです。else等でつけたせばいいんですが、一応。


また、`Const`というクラスを作って定数はそこでまとめるようにした。
メリットは**値を後から変えるのが容易＋何の値なのかわかりやすくなる**こと

追記(2019/3/17):大事なことを忘れていた気が
基本的にコントローラーから手を離したら動作が止まるように！
PID制御にしても、ボタンが押されなくなったら必ず止まるようにすること。

あと、足回りがステック操作でそれ以外はボタン操作です。

##やってくこと
`IterativeRobot`クラスを継承したclassの中に書いていく。(実行はいい感じにやってくれるようになってる。)
基本的にIterativeRobotの関数をオーバーライドしていく。(厳密にはIterativeRobotが継承しているlsrIterativeRobotBaseの関数)


## オーバーライドする主な関数

三角をタップorクリックすれば内容見えます。(IE及びEdgeはできない模様)

<details><summary>public void RobotInit()</summary>
     ロボットを起動した時の初期化部分
     `Robot`のコンストラクターでもそれほど変わらない。
変数にインスタンスを代入したり、初期値を設定したり、カメラのキャプチャを開始したりする。

エンコーダの`setDistancePerPulse`について少々。
エンコーダとは、モーターがどれほど回転したかを測る機械なのですが、その方法がプログラムに若干かかわるので説明しておきます。(ネットで探せばもっといい解説があると思います)

エンコーダの中に、ライトと受光部があり、その間に一定間隔で隙間が空いているスリット式の仕切りがあって、その仕切りが回転して光が遮られたりするごとにカウントしていきます。
そのカウント数をもとに一回転当たりの距離をかけたりしてどれだけロボットが動いたかとかを推測していきます。
一回転当たりの距離はギア比とかを考慮して計算したり、実測したりして決めます。理論的に求められるほうがいいです。その方法はここでは割愛させてください。
</details>

<details><summary>public void TeleopInit()
</summary>
     コントローラー制御の時の初期化部分
自分たちは使わなかった。
</details>

<details><summary>public void TeleopPeriodic()</summary>
     コントローラー制御の時の関数部分
　　　必要なことは[プログラムの方針](#プログラムの方針)に殆ど書いてます。
　　　クライムは(段差に登るのは)[クライムの記事]()で紹介します。
</details>

<details><summary>public void autonomousInit()</summary>
     autonomousの時の初期化部分
自分たちは使わなかった</details>

<details><summary>public void autonomousPeriodic()</summary>
     autonomousの時の関数部分
今年はSandStorm中。
TeleopPeriodic()を呼び出している。
</details>



## class Robotの変数一覧(必要なもの)
Robot自体が持っているのはモータードライバとかのオブジェクトだけだが、
Stateの変数もここでまとめる。
それぞれの変数の意味はそれぞれの章でまとめる。

<details><summary>Robot自体が持っている変数</summary><div>

```Java:variables.java

    private State state;

    /** Controller 
     * 二つで操作する形にした。
     */
    private XboxController driver, operator;

    /** Motors　
     * 足元に左右一つずつSpark(モータードライバ)が、 
     * リフトにTalon(こちらもモータードライバ)、腕と、段差に 
     * 上る用の足にVictorSP(モータードライバ)があるので宣言
     * 
     * 一つの箇所に二つモータを使うだとかで同じようなモータードライバがある場合は
     * SpeedControllerというクラスでこれらのモータードライバをまとめることができる。
     * 今回は僕たちは物理的にまとめたので使用していない。
     */
    private Spark driveLeft, driveRight;
    private Talon liftMotor;
    private VictorSP rollerMotor;
    private VictorSP climbMotor;

    /** Encoder, Gyro
     * 足元左右とリフトにに一つずつエンコーダ(進んだ距離を測 
     * れたりするやつ)と足元のをまとめたやつ。
     * ジャイロ(回転を計測できたりするやつ)を宣言
     * EncoderGroupはオリジナルのクラス
     */ 
    private Encoder rightDriveEncoder, leftDriveEncoder;
    private EncoderGroup driveEncoder;
    private Encoder liftEncoder;
    private ADXRS450_Gyro gyro;

    /** Solenoid
     * ソレノイド(エアシリンダを動かすもの)を宣言
     * 腕をしまったり板をつかんだり段差を上るのに使用
     */
    private Solenoid armSolenoid, barSolenoid;
    private Solenoid climbSolenoid;

    /** Compressor
     * コンプレッサー(空気をためるやつ)を宣言
     */
    private Compressor compressor;

    /** Timer for climb
     * 段差に上る時に使用するタイマー
     */
    private Timer climbTimer;

    /* SubModule
     * ロボットを大まかに分けた時のモジュール
     * オリジナルのクラス
     */
    private Drive drive;
    private Lift lift;
    private Grabber grabber;
    private Climb climb;

    // ライントレース用のセンサー　
    // 0～3.3V 白線があると電圧が上がる 
    // 残念ながら未実装
    //private AnalogInput rightFrontSensor, 
    //                  rightBackSensor, 
    //                  leftFrontSensor, 
    //                  leftBackSensor;

    /** Camera
     * エレベーターについているのと、段差に上る用の枠についているもの。
     */
    private CameraServer elevatorCamera, frameCamera;

    /** NetWorkTable
     * 画像処理の結果を受け取るためのもの
     * こちらも未実装
     */
    private NetworkTable networkTable;

```

</div></details>

ここからState

<details><summary>Drive関係</summary><div>
足回り。ここでタイヤを全部制御する。

``` Java:State.java 
     public enum DriveState {
        kManual,
        kLineTrace,
        kCloseToLine
    }    // モード切替　後半二つは未実装
    public DriveState driveState;
    public double driveStraightSpeed, driveRotateSpeed;    // コントローラー制御の値 
    public double driveStraightSetpoint, driveRotateSetpoint;    // PID制御の目標値
    public boolean is_drivePIDOn;    // PID制御するかどうか
    public boolean is_lineTraceOn;    // ライントレースするかどうか
    public boolean is_lowInputOn;    // 低出力モードにするかどうか
```

</div></details>


<details><summary>Lift関係</summary><div>
アーム(後述)を持ち上げるもの。高さ制御くらいしかしない。

```Java:State.java
    public double liftSpeed;    // コントローラー制御の値
    public double liftSetpoint;   // PID制御の目標値
    public boolean is_liftPIDOn;    // PID制御するかどうか
```
</div></details>

<details><summary>Arm関係</summary><div>
ボールを掴んで発射したり板を掴んで付けるところ。
`Grabber`という名前にしている。

```Java:State.java
    public enum CargoState {
        kHold,
        kRelease,
        kDoNothing
    }
　　public CargoState cargoState;    // カーゴをどうするか  
    public boolean is_toHoldPanel;    // パネルを保持するかどうか
    public boolean is_toRetractArm;    //  アームをしまうかどうか
```
</div></details>

<details><summary>Climb関係</summary><div>
最後の段差に登るところに使うもの。

```Java:State.java
    public enum ClimbSequence {
        kDoNothing("kDoNothing"),
        kLiftUp("kLiftUp"),
        kLocking("kLocking"),
        kAdvance("kAdvance"),
        kUnlocking("kUnlocking"),
        kLiftDown("kLiftDown");

        String name;
        ClimbSequence(String name) {
            this.name = name;
        }
    }    // 詳しい内容はClimb部分で... Stringはデバッグ用
    public ClimbSequence climbSequence = ClimbSequence.kDoNothing;    //    自動クライム中の状態
    public boolean is_autoClimbOn;    // 自動クライムしているかどうか
    public boolean is_lockingClimb;    // ストッパーを出すかどうか
    public boolean is_lockedClimb;    //ストッパーが完全に出されたかどうか
    public double climbMotorSpeed;    // クライムの時の枠のモーターの値
```
</div></details>


##Constの内容
定数です。
ここは上のように列挙(~~コピペ~~)するときりがないので日本語で。

大まかに分けると

**・ポート**
**・フィールドの寸法**
**・ロボットの寸法**
**・その他定数**

の四種。

ポートはモータードライバ等が[roboRIO]()(ロボリオ)にぶっ刺さってるポートです。

フィールドの寸法は今回のルールでいえばボールを入れたりするところの高さとかです。

ロボットの寸法は[エンコーダの1パルス当たりの距離]()とかロボットにまつわる数値だったりします。

その他定数は[不感帯やPIDの係数]()とかです。

大まかに分けとくと管理しやすいと思います。

##autonomousについて(Sandstorm中の制御について)
最初の15秒間の自動制御。例年は完全自動で動くものだが、今年は何故かカメラを使ってコントローラー制御も出来るように
まさかのSandstormという…

自分たちはカメラを使ってコントローラー制御にしました。処理は全部Teleopと同じ、というか`teleopPeriodic()`呼び出してます。

##LAST
最後に。
次は足回りについて話す前に、PID制御についての話となります。

では！

前の記事⇒[FIRST](https://qiita.com/amidaandhero02/items/59c69d8facd2335f96ce)
次の記事⇒[PID制御について](https://qiita.com/amidaandhero02/items/5d29006dd693be420c4a)