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

②からの続きです。

静止時に値が細かい変動を繰り返す現象は抑えることが出来ました。次は、一方向しか動かしていないのに、逆方向へ動いたことを示す現象についてです。

このセンサについていろいろ調べてみると、どうやら"角速度"ではなく"角加速度"を出力しているらしいです。角加速度は角速度の微分を示していますが、では実際に動きがどのような関係になっているのでしょうか？下図で考えてみます。

この図のとおり、角加速度は回転開始時にプラスに、回転終了間際でマイナスを示します。このセンサが角加速度を出しているという事であれば、実際の出力値と辻褄が合います。

ではセンサの出力値が角加速度であるという前提で、そこから角速度を求めてみます。角速度を求めるためには角加速度の積分を行います。プログラムは以下のとおり。
==================================================
#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, deg_s_g1, deg_s_g2;
boolean xlearn;

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

// 角速度センサ初期値学習
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;
xlearn = true;
MsTimer2::stop();
}
}

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角速度

if (xlearn == true){
deg_s_g1 = deg_s_g1 + deg_g1 / 100;
deg_s_g2 = deg_s_g2 + deg_g2 / 100;
}
// PCに送信
Serial.print(time);
Serial.print(" ");
Serial.print(deg_g1) ; // G1角加速度
Serial.print(" ") ;
Serial.print(deg_s_g1) ; // G1角速度
Serial.print(" ") ;
Serial.print(deg_g2) ; // G2角加速度
Serial.print(" ") ;
Serial.println(deg_s_g2) ; // G2角速度

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

⑩作動結果
以下のとおりになりました。角速度が徐々に積み上がっていき、回転させてないのにもかかわらず角速度が出ているかのようになっております。これを直す必要があります。

⑪平均化処理、オフセットをリセットさせる
誤差が積み上がっている原因として、角加速度の値が細かく変動しているのが解ります。それを平均化処理によって抑えるのと、角加速度が一定時間0付近で安定したら積み上がった角速度をリセットする処理を加えます。

==================================================
#include ＜MsTimer2.h>

unsigned long time, time_old, time3, time4, g1_init;
float deg_g1, g1, g1_lrn, i, deg_s_g1;
boolean    xlearn;
unsigned int    count = 0;

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

// 角速度センサ初期値学習
void learn() {
i = i + 1;
g1_lrn = g1_lrn + analogRead(1);
if (i > 5){
g1_init = g1_lrn / i;
xlearn = true;
MsTimer2::stop();
}
}

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

// 角速度算出
g1 =0;
for (i2 = 0 ; i2 < 10 ; i2++) { g1 = g1 + analogRead(1);}
g1 = g1 / 10;

if (abs(g1 - g1_init) <= 3) {
g1 = g1_init;
count = count + 1;
} else {count = 0;}

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

if (xlearn == true){
if (count > 20){ deg_s_g1 = 0;}
else    { deg_s_g1 = deg_s_g1 + deg_g1 / 100;}
}
// PCに送信
Serial.print(time);
Serial.print(" ");
Serial.print(deg_g1) ; // G1角加速度
Serial.print(" ") ;
Serial.print(deg_s_g1) ; // G1角速度

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

⑫作動結果
以下のとおりになりました。対策仕様により前より近い値を取得できているのではないかと思います。

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