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

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

##FIRST
最初に。
Drive関係ってのは要は足回りのことです。DriveBaseを制御する部分。
ほかのサブモジュール同様Stateクラスから情報を受け取って操作します。

ここからは割と地味に話が続くので見どころさんは少ないです。

##このクラスに必要なこと
・コントローラーから入力されたものを反映して足元のモーターを動かせること。
・必要に応じて低出力モードになるようにすること。
(・ライントレースをできるようになること)
(・認識した線に近づけること)
 

##変数
普通に走るのとPID制御部分。
PID制御はルール発表前に書いてた部分。まさかautonomousが半分なくなったようなものになるとは…使えなくもないが、期間等が足りず断念…解説はします。
　
<details><summary><font Color = "Blue">enum </font> PIDMode {Straight, Rotate, Default}</summary><div>
PID制御で、直進成分と回転成分で処理の分岐があるので、それ用に作ったもの。
</div></details>

<details><summary><font Color = "Blue">double </font> straightOutput, rotateOutput</summary><div>
PID制御の計算によって得られた数値を保存するためのもの。
</div></details>

<details><summary><font Color = "Blue">double </font> preStraightOutput,preRotateOutput</summary><div>
加速度を制限するために作ったもの。
</div></details>

<details><summary><font Color = "Blue">EncoderGroup</font> e_drive　<font Color = "Blue">ADXRS450_Gyro</font> g_drive</summary><div>
そのまま。エンコーダとジャイロ。
</div></details>

<details><summary><font Color = "Blue">PIDController</font> straightController, rotateController</summary><div>
PID制御をするための計算とかを行うためのもの。
</div></details>



##関数一覧
コードに書いてあることのコピペです。軽い説明も。
 
<details><summary><font Color = "purple">public void </font> applyState()
</summary><div>
引数...
・<font Color = "Blue">State</font> state
 
>stateから情報を受け取り入力する
</div></details>

<details><summary><font Color = "purple">private void </font> setSpeed()
</summary><div>
引数...
・<font Color = "Blue">double</font> straightSpeed,
・<font Color = "Blue">double</font> rotateSpeed,
(・<font Color = "Blue">boolean</font> is_lowInputOn)
 
>arcadeDrive()に代入する
低出力モードにするかを第三引数に加えたものも実装(オーバーロードっていうんでしたっけ)
</div></details>

<details><summary><font Color = "purple">private void </font> setRelativeStraightSetpoint()
& <font Color = "purple">private void </font> setRelativeTurnSetpoint()
& <font Color = "purple">private void </font> setRelativeSetpoint()
</summary><div>
引数...
・<font Color = "Blue">double</font> setpoint　など
　
>Setpointを相対位置で代入する
	  本来のsetSetPointでは現在位置からではなくEncoder系を作動させた時の初期位置からの距離、角度を指定する必要がある。
	  PIDControllerのcalculate()をみるとinputにpidGet()をいれ、setSetpoint()でsetされた目標値との偏差をとっている。
	  つまり、何も考えずに入れるとEncoder系を作動させた時の初期位置からのsetpointになり、思うように動作しない。
	  例えば、1ｍ前進した後に2ｍ後進しようとしてsetSetpoint(-2000)などとすると実際には3ｍ後進してしまう。
	  だから、setSetpoint(getDistance()(=1000) + setpoint(=-2000))とする。
	  しかし、少し扱いづらいので"Relative"(=相対)にして見えないところで処理する。
	 
</div></details>

<details><summary><font Color = "purple">private void </font> setStraightSetpoint()
& <font Color = "purple">private void </font> setTurnSetpoint()
& <font Color = "purple">private void </font> setSetpoint()
</summary><div>
引数...
・<font Color = "Blue">double</font> setpoint　など
　
>Setpointを絶対座標で代入する
</div></details>

<details><summary><font Color = "purple">private void </font> PIDEnable()
</summary><div>
引数...
なし
 
>二つのPIDControllerをenableにする
</div></details>

<details><summary><font Color = "purple">private void </font> PIDDisable()
</summary><div>
引数...
なし
　
>二つのPIDControllerをdisableにする
</div></details>

<details><summary><font Color = "purple">private boolean</font> is_PIDEnabled()
</summary><div>
引数...
なし
 
返値...
二つのPIDControllerがenableかどうか
</div></details>

<details><summary><font Color = "purple">public void </font> setStraightP,I,D();
& setTurnP,I,D(); 
</summary><div>
引数...
・<font Color = "Blue">double</font> p,i,d　(それぞれに応じたもの)
 
>ゲインの調整をしやすくするため
</div></details>

<details><summary><font Color = "purple">public void </font> PIDReset();
</summary><div>
引数...
なし
 
>PIDのリセット
</div></details>

<details><summary><font Color = "purple">private double</font> limitAcceleration() 
</summary><div>
引数...
・<font Color = "Blue">double</font> preOutput, 
・<font Color = "Blue">double</font> output 

>加速度の制限
</div></details>

<details><summary><font Color = "purple">public void </font>　printVariables</summary><div>
引数...
なし

>クラスが持っている変数の値を表示する関数
デバッグ用。
</div></details>


メンバークラス---public class DrivePIDOutput　(PIDOutput継承)
  その関数 
	<details><summary><font Color = "purple">public</font> DrivePIDOutput()
</summary><div>
引数...
・<font Color = "Blue">PIDMode</font> pidmode
 
>PIDModeによって後述のpidWrite()の動作が変わる。


<details><summary><font Color = "purple">public void </font> pidWrite()</summary><div>
引数...
・<font Color = "Blue">double</font> output
 
>outputを受け取り処理して代入する。PIDModeによって代入対象が異なる。
</div></details>

</div></details>
　



##関数説明
具体的な挙動の仕方とか。自明なものは省略。

<details><summary>applyState()</summary><div>
Stateを受け取って、それのフィールドの必要なものを取り出して判定等を行う。

switchで大まかに挙動を分ける。
<details><summary>マニュアル</summary><div>
これはコントローラーで入力して進んでいるときのもの。
リフトを上げていたりしているときや、段差に上るとき、細かい動きの調整をしたい時用に低出力モードを実装しました。
</div></details>

<details><summary>ライントレース</summary><div>
実際には実装できなかったライントレース機能...
コードもないです。

考えていたのは、
ロボットの前の方にセンサーを４つ横並びにつけて、左と右のセンサーの値の差が0になるようにPID制御するという方法。
内側が感知しているときより外側が感知している時の方が操作量を大きくしたいので、外側の方の値を大きくなるように調整する。

</div></details>

<details><summary>線に近づく</summary><div>
ライントレース同様、実装できなかったもの。
ロボットにつけたカメラとかで線を認識してそれに近づくためのもの。

</div></details>
</div></details>

<details><summary>setSpeed()</summary><div>
そのままスピードを設定する関数。

```Java:setSpeed.java
private void setSpeed(double straightSpeed, double rotateSpeed, boolean is_lowInputOn = false) {
        //低出力モード
	straightSpeed *= is_lowInputOn ? 0.6 : 1;
	rotateSpeed *= is_lowInputOn ? 0.6 : 1;

	arcadeDrive(straightSpeed, rotateSpeed);
}
```

>直接arcadeDrive通してるだけやんけ！意味ある？

と思いますよね。意味はあります。もし足元のモーターがイカれたり(もしそんなことがあったら棄権する気もするが)、何かしらの作業がしたかったりしてモーターを動かしたくなったときにこの部分をコメントアウトするだけで動かなくできる。

と言っていたんですが、この記事書いてる途中に思いついて、第三引数に`boolean is_lowInputOn = false`(簡略化のための表記)をいれて低出力モード機能をここに実装しました。
あと、加速度制限も...
こういうふうに機能を簡単に追加できるようになります(身をもって体現)

あと、ちょっと使い方が直感とは違うので書いておきます。
第一引数が真っ直ぐ進む要素で、第二引数が回転要素です。
つまり、スティックで操作するときは第一引数にY方向の入力、第二引数にX方向の入力を入れることになります。
</div></details>

<details><summary>limitAcceleration</summary><div>
加速度を制限するもの。危険なので、もちろん減速する場合は制限しない。

まず、ひとつ前の入力と現在の入力の差から加速度を出す(単位は秒で) 。
最大加速度(`Const`にて定義)と比べて小さい方を採用して先程の作業の逆をして戻す。
比べるときは正の値にして比べたいので`abs()`(絶対値を返す関数)を使う。
多分見たほうが早い。(一行が長いのは許してください)

```Java:limitAcceleration.java
double accelration = (output - preOutput) / Const.PIDLoopPeriod;
acceleration =  Math.signum(acceleration) * Math.min(Math.abs(acceleration), Const.maxAcceleration);
// signnumは引数の符号を返す関数

output = preOutput + acceleration * Const.PIDLoopPeriod;

```

これだと0除算&減速するときも制限してしまうので、それ対策に

```Java:
if(preOutput == output) {
	// 0除算回避
	return output;
}

double acceleration = (output - preOutput) / Const.PIDLoopPeriod;
if(acceleration * preOutput < 0 && acceleration * output < 0) {
	// 0へ向かう加速度だったらそのまま
	return output;
}
// 制限
acceleration =  Math.signum(acceleration) * Math.min(Math.abs(acceleration), Const.maxAcceleration);

output = preOutput + acceleration * Const.PIDLoopPeriod;

```

「0へ向かう加速度ならそのまま」の意味が分かりづらいと思うので説明します。

まず、前の入力が+で加速度が-、-で+というのは減速方向ですね。これはプログラム的(数学的?)に解釈すると、加速度と前の入力の符号が違うということです。
前の入力が1.0で加速度が-o.5とかなら減速してますね。符号が逆のときも然り。

ただ、これだと、前の入力が0.1、現在の入力が-1.0とかになったときの対処ができないので(前の入力と加速度の符号は違うが、急激に後ろに下がってしまう)、加速度と現在の入力についても同じことをします。

対応表おいておきます👇

|   output   | preOutput  |acceleration|制限対象か|
|:----------:|:----------:|:----------:|:-------:|
|+           |+           |+           |◎　　　　|
|+           |+           |-           |☓　　　　|
|+           |-           |+           |◎　　　　|
|+           |-           |-           |存在しない|
|-           |+           |+           |存在しない|
|-           |+           |-           |◎　　　　|
|-           |-           |+           |☓　　　　|
|-           |-           |-           |◎　　　　|
※同じ場合は考慮してません
</div></details>



<details><summary>printVariables()</summary><div>
持っている変数に加え、エンコーダやジャイロ、PIDの値を表示するようにした。
</div></details>

<details><summary>DrivePIDWrite.pidWrite()</summary><div>
PID制御での操作部分。
StraightとRotateで別の操作をしなければならないので`enum`で分けた。
</div></details>

##LAST
最後に。

Driveは割と頑張った部分だけどほとんど生きなかった部分でした。
これが今後役立ってくれたら幸いです...

では。

前の記事⇒[PID制御について](https://qiita.com/amidaandhero02/items/5d29006dd693be420c4a)
次の記事⇒[Lift関係](https://qiita.com/amidaandhero02/items/f90c21aab867a3e2b4b2#_reference-89bd284bca9c5766d445)(この記事はまだ書けてないです:pray:　後々更新します)