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

この記事はこちらの記事の一部です。まずはこちらをご覧ください。

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

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

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

##変数
普通に走るのとPID制御部分。
PID制御はルール発表前に書いてた部分。まさかautonomousが半分なくなったようなものになるとは…使えなくもないが、期間等が足りず断念…解説はします。
　

enum PIDMode {Straight, Rotate, Default}

PID制御で、直進成分と回転成分で処理の分岐があるので、それ用に作ったもの。

double straightOutput, rotateOutput

PID制御の計算によって得られた数値を保存するためのもの。

double preStraightOutput,preRotateOutput

加速度を制限するために作ったもの。

EncoderGroup e_drive　ADXRS450_Gyro g_drive

そのまま。エンコーダとジャイロ。

PIDController straightController, rotateController

PID制御をするための計算とかを行うためのもの。

##関数一覧
コードに書いてあることのコピペです。軽い説明も。

public void applyState()

引数... ・State state

stateから情報を受け取り入力する

private void setSpeed()

引数... ・double straightSpeed, ・double rotateSpeed, (・boolean is_lowInputOn)

arcadeDrive()に代入する
低出力モードにするかを第三引数に加えたものも実装(オーバーロードっていうんでしたっけ)

private void setRelativeStraightSetpoint() & private void setRelativeTurnSetpoint() & private void setRelativeSetpoint()

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

private void setStraightSetpoint() & private void setTurnSetpoint() & private void setSetpoint()

引数... ・double setpoint　など 　 >Setpointを絶対座標で代入する

private void PIDEnable()

引数... なし

二つのPIDControllerをenableにする

private void PIDDisable()

引数... なし 　 >二つのPIDControllerをdisableにする

private boolean is_PIDEnabled()

引数... なし

返値...
二つのPIDControllerがenableかどうか

public void setStraightP,I,D(); & setTurnP,I,D();

引数... ・double p,i,d　(それぞれに応じたもの)

ゲインの調整をしやすくするため

public void PIDReset();

引数... なし

PIDのリセット

private double limitAcceleration()

引数... ・double preOutput, ・double output

加速度の制限

public void 　printVariables

引数... なし

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

メンバークラス---public class DrivePIDOutput　(PIDOutput継承)
その関数

public DrivePIDOutput()

引数... ・PIDMode pidmode

PIDModeによって後述のpidWrite()の動作が変わる。

public void pidWrite()

引数... ・double output

outputを受け取り処理して代入する。PIDModeによって代入対象が異なる。

　

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

applyState()

Stateを受け取って、それのフィールドの必要なものを取り出して判定等を行う。

switchで大まかに挙動を分ける。

マニュアル

これはコントローラーで入力して進んでいるときのもの。 リフトを上げていたりしているときや、段差に上るとき、細かい動きの調整をしたい時用に低出力モードを実装しました。

ライントレース

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

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

線に近づく

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

setSpeed()

そのままスピードを設定する関数。

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方向の入力を入れることになります。

limitAcceleration

加速度を制限するもの。危険なので、もちろん減速する場合は制限しない。

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

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除算&減速するときも制限してしまうので、それ対策に

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	制限対象か
+	+	+	◎
+	+	-	☓
+	-	+	◎
+	-	-	存在しない
-	+	+	存在しない
-	+	-	◎
-	-	+	☓
-	-	-	◎
※同じ場合は考慮してません

printVariables()

持っている変数に加え、エンコーダやジャイロ、PIDの値を表示するようにした。

DrivePIDWrite.pidWrite()

PID制御での操作部分。 StraightとRotateで別の操作をしなければならないので`enum`で分けた。

##LAST
最後に。

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

では。

前の記事⇒PID制御について
次の記事⇒Lift関係(この記事はまだ書けてないです　後々更新します)