角速度(ジャイロ)センサ値を正しい角速度に変換する②

①からの続きです。

⑥サンプリング周期を変える
センサ応答性は50Hzの為、標本化定理によりサンプリング周期は50 x 2 =100Hzのため、周期は1/100 = 0.01s = 10ms以下を必要とします。

そのためスケッチを修正する必要があるのですが、単にdelay時間を10msにするだけでは駄目だと考えます。 delay関数でサンプリング周期をコントロールするには精度が荒いからです。そこで以下関数を使います。

delayMicroseconds(us)
プログラムを指定した時間だけ一時停止します。単位はマイクロ秒です。16383マイクロ秒以内の値を指定した時に正確に作動します。

micros()
プログラムを実行した時から現在までの時間をマイクロ秒単位で返します。約70分間でオーバーフローしゼロに戻ります。分解能は4マイクロ秒です。
以上を踏まえてスケッチは以下のとおりになります。赤文字が変更点です。
==================================================
unsigned long time, time_old, time3, time4;
float deg_g1, deg_g2, g1, g2;

void setup()
{
Serial.begin(38400) ; // シリアルモニターの初期化
}
void loop()
{
// delay時間算出
time = micros();
time3 = time - time_old - time4; // 前回の遅れ時間
time_old = time; // 次回計算用として、今回までの演算時間を格納
time4 = 10000 - time3; // 今回のdelay時間を決定

// 角速度算出
g1 = analogRead(1);
g2 = analogRead(2);
deg_g1 = (g1 - 288) / 1023 * 5 / 0.00067; // G1角速度
deg_g2 = (g2 - 289) / 1023 * 5 / 0.00067; // G2角速度

// PCに送信
Serial.print(time);
Serial.print(" ");
Serial.print(deg_g1) ; // G1
Serial.print(" ") ;
Serial.println(deg_g2) ; // G2

delayMicroseconds(time4) ;
}
==================================================

⑦作動結果
以下のとおりになりました。値の動きは細かくなりましたが、やはり静止時の微小な変化やマイナスに振れる現象は残ったままです。

⑧不感帯を設定する
微小な変化を抑えるための対策です。入力値が静止時の値に近い場合は、静止時の値と等しいとする処置を加えます。
==================================================
unsigned long time, time_old, time3, time4;
float deg_g1, deg_g2, g1, g2;
unsigned long g1_init = 288;
unsigned long g2_init = 289;

void setup()
{
Serial.begin(38400) ; // シリアルモニターの初期化
}
void loop()
{
// delay時間算出
time = micros();
time3 = time - time_old - time4; // 前回の遅れ時間
time_old = time; // 次回計算用として、今回までの演算時間を格納
time4 = 10000 - time3; // 今回のdelay時間を決定

// 角速度算出
g1 = analogRead(1);
g2 = analogRead(2);

if (abs(g1 - g1_init) <= 2) { g1 = g1_init;}
if (abs(g2 - g2_init) <= 2) { g2 = g2_init;}

deg_g1 = (g1 - g1_init) / 1023 * 5 / 0.00067; // G1角速度
deg_g2 = (g2 - g1_init) / 1023 * 5 / 0.00067; // G2角速度

// PCに送信
Serial.print(time);
Serial.print(" ");
Serial.print(deg_g1) ; // G1
Serial.print(" ") ;
Serial.println(deg_g2) ; // G2

delayMicroseconds(time4);
}
==================================================

結果は以下のとおり、静止時の細かい変動を抑えることが出来ました。

静止時の値はセンサによってもばらつきがありますし、その日の気温度などでも結構ずれてしまいます。その都度微修正するのも大変なので、以下の対応を取ります。

⑨静止時の初期学習を行う。
プログラムを作動させてから、最初の数秒間はセンサを静止させたままにしておき、初期学習を行います。学習にはタイマー関数(MsTimer2)を使いますので、こちらの手順に従い関数をインストールしてください。
学習プログラムは以下になります。
==================================================
#include ＜MsTimer2.h>

unsigned long time, time_old, time3, time4, g1_init, g2_init;
float deg_g1, deg_g2, g1, g2, g1_lrn, g2_lrn, i;

// 角速度センサ初期値学習
void learn() {
i = i + 1;
g1_lrn = g1_lrn + analogRead(1);
g2_lrn = g2_lrn + analogRead(2);

if (i > 10){
g1_init = g1_lrn / i;
g2_init = g2_lrn / i;
MsTimer2::stop();
}
}

void setup(){
Serial.begin(38400) ; // シリアルモニターの初期化
MsTimer2::set(500, learn); // 500ms period
MsTimer2::start();
}

void loop(){
// delay時間算出
time = micros();
time3 = time - time_old - time4; // 前回の遅れ時間
time_old = time; // 次回計算用として、今回までの演算時間を格納
time4 = 10000 - time3; // 今回のdelay時間を決定

// 角速度算出
g1 = analogRead(1);
g2 = analogRead(2);
if (abs(g1 - g1_init) <= 2) { g1 = g1_init;}
if (abs(g2 - g2_init) <= 2) { g2 = g2_init;}
deg_g1 = (g1 - g1_init) / 1023 * 5 / 0.00067; // G1角速度
deg_g2 = (g2 - g1_init) / 1023 * 5 / 0.00067; // G2角速度

// PCに送信
Serial.print(time);
Serial.print(" ");
Serial.print(deg_g1) ; // G1
Serial.print(" ") ;
Serial.println(deg_g2) ; // G2

delayMicroseconds(time4);
}
==================================================

③へ続く

サブチャンネルあります。⇒ 何かのお役に立てればと