この記事はこちらの記事の一部です。まずはこちらをご覧ください。
##FIRST
最初に。
Drive関係ってのは要は足回りのことです。DriveBaseを制御する部分。
ほかのサブモジュール同様Stateクラスから情報を受け取って操作します。
ここからは割と地味に話が続くので見どころさんは少ないです。
##このクラスに必要なこと
・コントローラーから入力されたものを反映して足元のモーターを動かせること。
・必要に応じて低出力モードになるようにすること。
(・ライントレースをできるようになること)
(・認識した線に近づけること)
##変数
普通に走るのとPID制御部分。
PID制御はルール発表前に書いてた部分。まさかautonomousが半分なくなったようなものになるとは…使えなくもないが、期間等が足りず断念…解説はします。
enum PIDMode {Straight, Rotate, Default}
double straightOutput, rotateOutput
double preStraightOutput,preRotateOutput
EncoderGroup e_drive ADXRS450_Gyro g_drive
PIDController straightController, rotateController
##関数一覧
コードに書いてあることのコピペです。軽い説明も。
public void applyState()
stateから情報を受け取り入力する
private void setSpeed()
arcadeDrive()に代入する
低出力モードにするかを第三引数に加えたものも実装(オーバーロードっていうんでしたっけ)
private void setRelativeStraightSetpoint() & private void setRelativeTurnSetpoint() & private void setRelativeSetpoint()
private void setStraightSetpoint() & private void setTurnSetpoint() & private void setSetpoint()
private void PIDEnable()
二つのPIDControllerをenableにする
private void PIDDisable()
private boolean is_PIDEnabled()
返値...
二つのPIDControllerがenableかどうか
public void setStraightP,I,D(); & setTurnP,I,D();
ゲインの調整をしやすくするため
public void PIDReset();
PIDのリセット
private double limitAcceleration()
加速度の制限
public void printVariables
クラスが持っている変数の値を表示する関数
デバッグ用。
メンバークラス---public class DrivePIDOutput (PIDOutput継承)
その関数
public DrivePIDOutput()
PIDModeによって後述のpidWrite()の動作が変わる。
public void pidWrite()
outputを受け取り処理して代入する。PIDModeによって代入対象が異なる。
##関数説明
具体的な挙動の仕方とか。自明なものは省略。
applyState()
switchで大まかに挙動を分ける。
マニュアル
ライントレース
考えていたのは、
ロボットの前の方にセンサーを４つ横並びにつけて、左と右のセンサーの値の差が0になるようにPID制御するという方法。
内側が感知しているときより外側が感知している時の方が操作量を大きくしたいので、外側の方の値を大きくなるように調整する。
線に近づく
setSpeed()
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()(絶対値を返す関数)を使う。
多分見たほうが早い。(一行が長いのは許してください)
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()
DrivePIDWrite.pidWrite()
##LAST
最後に。
Driveは割と頑張った部分だけどほとんど生きなかった部分でした。
これが今後役立ってくれたら幸いです...
では。
後々更新します)