グルグル回る軸の角度変動の平均値を出す方法#3: 居酒屋ガレージ日記

2017年2月24日 (金)

グルグル回る軸の角度変動の平均値を出す方法#3

グルグル回る軸の角度変動の平均値を出す方法#2 は、
「移動平均」（関数を呼ぶたびに平均値を更新）でしたが、
単純な「加算平均」（一定期間ごとに平均値が出る）で
よければ、こんな方法をどうぞ。
例えば、0.1秒ごとにこれを呼び出して、2秒したら
結果が出るなんていうことでよければ、移動平均の
ためのバッファ（過去データを保持して新値で更新）
は不要です。

/*****　角度を加算平均化　　　　*****/
//　in/out deg:0～359度を得る
//　平均回数を過ぎて結果が出るまでは0を返す
#define AVR_SIZ　　 20　　　　　// 平均回数

WORD avr359(short deg)
{
static float ttl_x = 0.0;　　　　　　　 // X,Y合計値
static float ttl_y = 0.0;
static short cnt = 0;　　　　　　　　　 // 加算回数
static short avr = 0;　　　　　　　　　 // 平均値
float d, x, y;
　　PH3_H;　　　　　　　　　　　　　　　// (!!!)PH3 17pin H/L
　　d = (PAI / 180.0) * (float)(lim359(deg));　 // 0～2πに
　　x = cos(d);　　　　　　　　　　　　 // X,Yの値
　　y = sin(d);
　　ttl_x = ttl_x + x;　　　　　　　　　// 新値を加算
　　ttl_y = ttl_y + y;
　　cnt++;　　　　　　　　　　　　　　　// 回数+1
　　if(cnt >= AVR_SIZ){
　　　　x = ttl_x / AVR_SIZ;　　　　　　// X,Y平均値
　　　　y = ttl_y / AVR_SIZ;
　　　　cnt = 0;　　　　　　　　　　　　// 次処理のために
　　　　ttl_x = ttl_y = 0.0;　　　　　　// 合計値をゼロクリア
　　　　d = (180.0 / PAI) * atan2(y, x);　　// 角度 ±π
　　　　avr = lim359(round(d));　　　　　　 // 0～359
　　}
　　PH3_L;　　　　　　　　　　　　　　　// (!!!)PH3 17pin H/L
　　return avr;　　　　　　　　　　　　 // 0～359
}