ジャイロコンパスまとめ（その２）

実験用ロボのここまでの回路図はこんな感じです。クリックでPDFを表示します。ただし、現在はブレットボードのノイズ対策のためPSDへの電源供給はカットしてあります。

「091120.pdf」をダウンロード
　
　
次はPICのプログラムの解説です。

ジャイロコンパスの更新部分です。この処理はタイマー割り込みでジャイロセンサのAD変換のたびに実行されます。周期は2msです。　
グローバル変数とその内容は次の通りです。longは16bit、long longは32bitです。

signed long long　　gyro_compass;　　ジャイロコンパスです
signed long long　　gyro_ma[10];　　　移動平均用の履歴を保存します
signed long long　　gyro_ma_result;　作業用の変数です
signed long long　　gyro_work;　　　　これも作業用の変数です
signed long　　　　gyro_initial;　　　　無信号時のセンサの値です
中にはグローバルでなくても良さそうなのもありますが、現在はこうなっています。

これが処理の本体です。

//直近8回分の計測値の記憶（正直にやるとこうなる）
gyro_ma[0] = gyro_ma[1];
gyro_ma[1] = gyro_ma[2];
gyro_ma[2] = gyro_ma[3];
gyro_ma[3] = gyro_ma[4];
gyro_ma[4] = gyro_ma[5];
gyro_ma[5] = gyro_ma[6];
gyro_ma[6] = gyro_ma[7];
gyro_ma[7] = adbuff[9];　　//adbuff[9]にはジャイロセンサのAD変換値

//直近8回分の移動平均の計算
gyro_ma_result = 0;
for(i = 0; i < 8; i++){
　　gyro_ma_result += gyro_ma[i];
}
gyro_ma_result = gyro_ma_result >> 3;　　//8で割る

//方位角の計算
gyro_work = gyro_ma_result - gyro_initial;　//変化分の計算
if((gyro_work > 1)||(gyro_work < -1)){　　　//閾値の設定
　　gyro_compass += gyro_work;　　　　　//コンパスの変更
}

つまり、2msごとに直近８回分の測定結果の平均値を取り、これを無信号時（静止時）のジャイロセンサの値と比較して、閾値を超えたら、その差分（正負）をコンパスの値に足し込むという操作をしています。
　
　
コンパスと無信号時のジャイロセンサの初期化は次の関数で行っています。この関数はロボットが静止しているときに実行します。

//ジャイロコンパスの初期化
//静止時の出力（基準値）を設定し、コンパスを０にリセットする。
void cal_gyro(void){
　char i;
　signed long xx;
　signed long long xxxx;

　//基準値を設定→その値でのドリフトを計測をドリフトが一定値に収まるまで繰り返す
　//注意！！無限ループ
　while(1){
　　//ジャイロスターの出力を5ms間隔で20回読んで平均を取る
　　xx = 0;
　　for(i= 0; i < 20; i++){
　　　xx = xx + adbuff[9];
　　　wait_proc(5);　//5ms待つ
　　}
　　gyro_initial = (xx / 20); //ジャイロスターの基準値を計算

　　//ドリフトを計測し一定の範囲内なら終了
　　xxxx = gyro_compass; //現在のコンパスをセーブ
　　wait_proc(500); //500mS待つ

　　if(gyro_compass > xxxx){
　　　//右回りのドリフト
　　　if(gyro_compass - xxxx < 20) break; //ドリフトが許容範囲内なら無限ループ抜ける
　　　}
　　else{
　　　//左回りのドリフト
　　　if(xxxx - gyro_compass < 20) break; //ドリフトが許容範囲内なら無限ループ抜ける
　　}
　}
　gyro_compass = 0; //コンパスを０にリセット
}

この関数は、無信号時（静止時）のジャイロセンサの値を20回測定して平均を取り、gyro_initialに基準値として保存します。次にこの基準値を使ってコンパスを500ms作動させ、ドリフトが許容範囲内かどうかを調べます。OKならコンパスを現在の方向が0としてリセットし初期化終了です。ドリフトがあるようならもう一度やり直します。
コードの中にはコンパスの処理がありませんが、前に説明したように、コンパスはタイマー割り込みで処理されているので、待っていれば結果は出ます。

現在はこのような素朴なコードで動作しています。