PWM制御と直進補正

ロボット作ろう：シェーキー製作記

PWMは結局フリーPWMを使うことにしました。発熱を嫌ったのと、結局、両者の速度制御の性能にあまり違いが無かったからです。ギアが固く、外力の影響を受けないからではないでしょうか。

この写真はホイールエンコーダの波形です。時間軸は10ms/div、電圧軸は500mV/divです。車輪１回転で50波形、書き込みのように立ち上がり、立ち下がりでカウントすることで、１回転100パルスの分解能を持っています。（クリックすると拡大します）

このカウント値で左右の駆動輪ごとにオドメータ（距離計）を積算します。前進方向でプラス、後退方向でマイナスになる積算距離計です。

また、移動命令ごとに移動距離を勘定する左右のトリップメータも持たせています。これは移動命令の前にリセットして、移動距離を測ります。オドメータがあるのにこれを持たせているのは、直進補正のとき便利だからです。

前進、後退、その場回転のように左右の駆動輪を同じだけ回したいときに、「直進補正」を掛けます。
今回採用したアルゴリズムは、

「左右のトリップメータを比べて、先に行っている方のモーターを切る。同じ時は何もしない」という処理をPWMのパルス生成と同時に行う

という、例によって素朴なものです。
実際のソースコードは下記のようになります。(ブログに張り込むとインデントが失われて読みにくいですが、エディタに張るなどして読んでみてください)　この処理を100usに１回、タイマー割り込みで周期的に行います。

/*
モータープロセス
port_Cのbit0,1が左モーター、bit2,3が右モーター
モーターパワー変数 Rpwm,Lpwm 0->Stop 64->Full
回転方向変数 Rdir,Ldir 0x1->FD,0x2->BK,0x3->BREAK
*/

//PWMの処理
++pwmcounter; //PWMの基準になるカウンタをインクリメント
if(pwmcounter > 63) pwmcounter = 0; //上限値63を超えたらリセット
i = 0; //作業用変数に左右モーターストップをセット
if(Lpwm > pwmcounter) {
i = Ldir; //左モーターパワー値が基準カウンタを超えているときだけモーター駆動
}
if(Rpwm > pwmcounter) {
i = i | (Rdir << 2); //右モータパワー値が基準カウンタを超えているときだけモーター駆動
}

//トリップカウンタを比べて直進補正
//モーターパワーが等しいとき（前進・後退・超信地旋回）に左右駆動輪の回転数を合わせる
if((Rdir != MTBRK) && (Ldir != MTBRK)){
if(Rpwm == Lpwm){
if(Rtrip > Ltrip){
i = i & 0xf3; //右駆動輪が回りすぎているので右モーターを切る
}
if(Rtrip < Ltrip){
i = i & 0xfc; //左駆動輪が回りすぎているので左モーターを切る
}
}
}
port_C = (port_C & 0xf0) | i; //完成したPWM情報を書き出し

左モータードライバがport Cのbit0,bit1、右モータードライバがbit2,bit3に接続されています。直進補正がかかるのは両方のパワーの指定が一緒の場合で、かつブレーキ(定数MTBRK)が設定されていないときだけです。

これで、きわめて安定に直進補正がかかりパワー制御もスムーズです。でも、前に書いたようあまり速度は変わりません。
また、PWMを40%以下にするとモーターが起動しなくなります。このモーターはこんな性能のようで、安定化電源で電圧を掛けても、12V定格に対して4.5Vくらいは掛けないと起動しません。