グルグル回る軸の角度変動の平均値を出す方法#2
浮動小数点でX、Y値と合計値を保持するのが不安
(誤差の累積)なので、整数で保持するように変えて
みました。
sin、cosで出たXとYの値は0~±1.0。
一万倍して±10000なので、ワードデータに納まります。
// 浮動小数点計算での誤差を累積ささないため
// X,Yを10000倍したwordデータで計算する
// sin,cos ±1.0が最大なので2バイトで足りる
WORD smz359b(short deg)
{
static short smz_x[SMZ_SIZ]; // Xスムージングバッファ
static short smz_y[SMZ_SIZ]; // Y ★
static long ttl_x, ttl_y; // 合計値 ★
static BYTE f1 = 0; // 初めての処理
static short p = 0; // データポインタ
short i;
short x, y;
float d;
PH3_H; // (!!!)PH3 17pin H/L
d = (PAI / 180.0) * (float)(lim359(deg)); // 0~2πに
x = round(cos(d) * 10000.0); // X,Yの値 ★
y = round(sin(d) * 10000.0); // 10000倍して整数で
if(f1 == 0){ // 初めて
ttl_x = x * SMZ_SIZ; // X,Y合計値 ★
ttl_y = y * SMZ_SIZ;
for(i = 0; i <SMZ_SIZ; i++){ // バッファを初期値で埋める
smz_x[i] = x;
smz_y[i] = 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; // 最後まで来たら先頭に
}
d = (180.0 / PAI) * atan2( // 角度
(float)ttl_y / (10000.0 * SMZ_SIZ), // X,Y平均値 ★
(float)ttl_x / (10000.0 * SMZ_SIZ));
PH3_L; // (!!!)PH3 17pin H/L
return lim359(round(d)); // 0~359
}
※全角スペースを使ってます。
★の所が変更点。
shortの整数に。
intと書くと、チップが32ビットなのでlongのデータになって
しまいます。
それと、atan2()のX、Yを求めている所、
(float)ttl_y / 10000.0 / SMZ_SIZ
と書くと、コンパイラは除算を2回繰り返していました。
(float)ttl_y / (10000.0 * SMZ_SIZ)
だと、( )内が定数になって、除算は1回だけ。
※検索:角度の平均値計算
※続き:グルグル回る軸の角度変動の平均値を出す方法#3
| 固定リンク
「電子回路工作」カテゴリの記事
- 容量マルチプライヤ回路(2023.05.22)
- 液晶表示コントローラ HD44780で迎撃(2023.05.16)
- 出窓:鯉のぼりを振る(2023.05.04)
- Arduino UNOで3相モーターを回す(2023.05.01)
- 出窓:鯉のぼりを振れたらというリクエスト(2023.04.26)
コメント