横向きに照射している光センサからの情報を使って、車体の姿勢を制御する方法を紹介します。
基本的に(ステッピングモータなら)P制御でうまく走ります。
P制御は、proportion=比例 制御の事で、
目標(ここでは、迷路の中央)からのずれと比例した出力をするような制御です。
制御の世界では目標とのずれを偏差、出力を制御量と呼びます。
式にすると
偏差 = (左センサの値 - 真ん中に置いた時の左センサの値) - (右センサの値 - 真ん中に置いた時の右センサの値)
制御量 = 制御定数 * 偏差
右モータ速度 = 速度 + 制御量
左モータ速度 = 速度 - 制御量
こんな感じです。
両方に壁がある迷路なら、制御定数を調整すればこの式だけでも走れるはずです。
最後に、日本語の式ではここから先見づらいので、上の式を書きなおして、ここから先に繋げます。
Error = (sen_l.value - sen_l.ref) - (sen_r.value - sen_r.ref); Control = Kp * Error; Speed_r = Speed + Control; Speed_l = Speed - Control;
閾値(しきいち、いきち、Threshold)は、制御をかけるかどうかを決める定数です。
いきちと読むか、しきいちと読むかで争いが起きる事がありますが、
どうでも良いので早々に切り上げましょう。
さて、上の式で制御をかけている時に、片方の壁がなくなるとどうなるでしょうか?
たとえば右の壁がなくなると、真ん中を走っているにも関わらず、Errorが大きな値になってしまいます。
これを回避するためには、壁のあり/なしを判断して、
壁がない場合には、Errorに反映しないようにしなければなりません。
こんな感じです.
if( (r_sen.value > r_threshold) && (l_sen.value > l_threshold) ){ //両方のセンサが使える時 Error = (sen_l.value - sen_l.ref) - (sen_r.value - sen_r.ref); }else if( (r_sen.value <= r_threshold) && (l_sen.value < l_threshold) ){ //両方のセンサが使えない時 Error = 0; }else if( r_sen.value > r_threshold ){ //右センサだけ使える時 Error = -2*(sen_r.val - sen_r.ref); }else{ //左センサだけ使える時 Error = 2*(sen_l.val - sen_l.ref); }
r_threshold、l_thresholdは、壁が無い時のセンサの値~リファレンス値の間で調整しましょう。
このようにすることで、壁がない時に必要のない制御がかかってしまう事がなくなるはずです。
が、現実はそんなに甘くありません。 次の項で説明します。
上の制御式で実際に走らせてみたでしょうか?
すると、壁の切れ目に吸いこまれてしまうような挙動をする事が確認できると思います。
下のグラフは、まっすぐに車体を走らせた場合の、壁の切れ目でのセンサの値の変化です。
黒が理想的なセンサ値の変化、赤が実際のセンサの変化です。
徐々にセンサの値が変化するせいで、変化している最中、
真ん中を走っているのに制御がかかってしまってるため、変な動きをしてしまっています。
これの対策として、有効に動いているものの一つとして、
センサの値の変化量が一定より大きかったら、壁制御の閾値をリファレンス値より少し大きくする
という物があります。
式にすると、閾値の項のコードの前の部分に、
if( abs(r_sen.diff) > DIFF_THRESHOLD ){ r_threshold = r_sen.ref + 10; //変化量が一定以上なら、閾値を引き上げる }else{ r_threshold = r_sen.thredhold; //変化量が一定以下なら、設定通りの閾値 } if( abs(l_sen.diff) > DIFF_THRESHOLD ){ l_threshold = l_sen.ref + 10; //変化量が一定以上なら、閾値を引き上げる }else{ l_threshold = l_sen.thredhold; //変化量が一定以下なら、設定通りの閾値 }
というような感じで追加してあげると、良い動きをするようになると思います。
ちなみに、l_sen.diff とかr_sen.diffは、各々のセンサの変化量です。
普通に、
l_sen.diff = l_sen.val - l_sen.old_val;
みたいな感じで、変化量を見てあげれば良いです。
センサの配置角度がきつい場合には、
センサの変化量がプラスの場合とマイナスの場合で、
変化量の閾値を変えてあげると良い結果になる場合があります。
後日編集予定