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

2017年2月23日 (木)

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

2016年09月07日：グルグル回る軸の角度変動の平均値を出したい　で
話題にしました「0～359度」ぐるぐる回る軸の回転角度の平均値算出。
これをまとめておきます。

関連するのは初めての「部品」は失敗するかもの
「ストッパー無しのボリューム」。
この抵抗値から「0～359度」を得たい時の平均値の算出方法です。
単純な加算平均だと「359←0→1」を行き来すると、「180」なんて
値が出てしまいます。

そこで、角度をsinとcosでXとY値に分解して、2つ値を移動平均。
平均値として出てきたXとYから「atan2」で角度を得るという手法です。

sin、cos、atan2関数をそれぞれ1回ずつ実行します。
値により実行時間は変動しますが、32MHzのRX220マイコン
（非力なんでハード的な浮動小数点機能は無い）で処理時間
は100～130μSくらいでした。
　　（空きの出力ポートにパルスを出してオシロで時間確認）

//　非力なマイコンなので浮動小数点はfloatで。 doubleではない
//　BYTEとWORDの定義
typedef unsigned char　　　 BYTE;
typedef unsigned short　　　WORD;
//　πの値
#define PAI　　 3.141592654

/*****　四捨五入　　　　*****/
//　　　正負で±0.5して整数に変換
short round(float d)
{
　　if(d >= 0){
　　　　return (short)(d + 0.5);
　　}else{
　　　　return (short)(d - 0.5);
　　}
}

/*****　360度1回転規制　　　*****/
//　　　0～359度を得る
WORD lim359(short d)
{
　　if(d < 0){　　　　　　　// マイナスかもしれない
　　　　d = -d;　　　　　　 // 正の数に
　　　　d = d % 360;　　　　// 0～359に
　　　　d = 360 - d;　　　　// 360から減算
　　}
　　d = d % 360;　　　　　　// 0～359に
　　return d;
}

/*****　角度をスムージング　　　*****/
//　in/out deg:0～359度を得る
#define SMZ_SIZ　　 20　　　　　// スムージング回数

WORD smz359(short deg)
{
static float smz_x[SMZ_SIZ];　　　　　　// Xスムージングバッファ
static float smz_y[SMZ_SIZ];　　　　　　// Y
static float ttl_x, ttl_y;　　　　　　　// 合計値
static BYTE f1 = 0;　　　　　　　　　　 // 初めての処理
static short p = 0;　　　　　　　　　　 // データポインタ
short i;
float x, y, d;
　　PH3_H;　　　　　　　　　　　　　　　// (!!!)PH3 17pin H/L
　　d = (PAI / 180.0) * (float)(lim359(deg));　// 角度を0～2πに
　　x = cos(d);　　　　　　　　　　　　 // X,Yの値
　　y = sin(d);
　　if(f1 == 0){　　　　　　　　　　　　// 初めて
　　　　ttl_x = ttl_y = 0.0;　　　　　　// バッファを初期値で埋める
　　　　for(i = 0; i <SMZ_SIZ; i++){
　　　　　　smz_x[i] = x;
　　　　　　smz_y[i] = y;
　　　　　　ttl_x += x;　　　　　　　　 // 合計値
　　　　　　ttl_y += y;
　　　　}
　　　　p　= 0;　　　　　　　　　　　　 // データポインタ
　　　　f1 = 1;　　　　　　　　　　　　 // 初回処理終了
　　}
　　else{　　　　　　　　　　　　　　　 // 2回目以降
　　　　ttl_x = ttl_x - smz_x[p] + x;　 // 古い値を引いて新値を加算
　　　　ttl_y = ttl_y - smz_y[p] + y;
　　　　smz_x[p] = x;　　　　　　　　　 // 新値をセット
　　　　smz_y[p] = y;
　　　　p++;　　　　　　　　　　　　　　// ポインタを後ろに
　　　　if(p >= SMZ_SIZ)　　p = 0;　　　// バッファの最後まで来たら先頭に
　　}
　　x = ttl_x / SMZ_SIZ;　　　　　　　　// X,Y平均値
　　y = ttl_y / SMZ_SIZ;
　　d = (180.0 / PAI) * atan2(y, x);　　// atan2でX,Yから角度に
　　PH3_L;　　　　　　　　　　　　　　　// (!!!)PH3 17pin H/L
　　return lim359(round(d));　　　　　　// 0～359
}