以前のリビジョンの文書です
横向きに照射している光センサからの情報を使って、車体の姿勢を制御する方法を紹介します。
基本的に(ステッピングモータなら)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); }
このようにすることで、壁がない時に必要のない制御がかかってしまう事がなくなります。