今までニフテイとかに投稿した内容をまとめています。推考していないので ボロボロとミスがあるようです。
論理演算の利用 AND OR 演算を利用すると分岐無しに色々な計算が出来ます 分岐無しにすると処理時間が固定になり便利な事が多いですね 最近のCPUは分岐が少ない方が速い事も多いようです
/************************************************************
符号無し同士の加算をし、結果がオーバフローしたら最大値にする
unsigned ClipAddUU(unsigned a,unsigned b)
{ unsigned c=a+b;
if(c>=a && c>=b) return c;
return (unsigned)-1;
}
************************************************************/
unsigned ClipAddUU(unsigned a,unsigned b)
{
unsigned c;
_asm {
mov eax,a ;
add eax,b ;
xor ecx,ecx;
sbb ecx,0 ;
or eax,ecx ;
mov c,eax; /*帰値はeaxなので不要だが警告を黙らせる為*/
};
return c;
}
/************************************************************
符号無し同士の減算をし、結果が負数なら０にする
************************************************************/
unsigned ClipSubUU(unsigned a,unsigned b)
{
unsigned c;
_asm {
mov eax,a ;
sub eax,b ;
mov ecx,-1;
adc ecx,0 ;
and eax,ecx ;
mov c,eax;
};
return c;
}
/************************************************************
符号無し数に符号付数を加算し結果を符号付数範囲にクリップする
************************************************************/
unsigned ClipAddUI(unsigned a,int b)
{
int c;
_asm {
mov eax,b ;
cdq ;//bだけ符号拡張しておいて
add eax,a ;
adc edx,0 ;//edxが-1ならアンダーフロー 1ならオーバフロー
neg edx ;//edxが1ならアンダーフロー -1ならオーバフロー
or eax,edx;//オーバフローでクリップ
neg edx
sar edx,1 ;//アンダーフロ時だけ edx=-1にでなければedx=0
not edx
and eax,edx;//アンダーフローでクリップ
mov c,eax; //帰値はeaxなので不要だが警告を黙らせる為に
};
return c;
}
チャタリング取り
サンプリング周波数*2 >チャタリングが起きない時間 となるようにして ２回取込み、前回と同じならそれを有効とします。判断を使えば簡単です が複数ビットを一度に処理出来かつ判断を使わない 論理演算を使う例を 説明します。
;Aに今回の入力ビット（最大8BIT)を入れて
ChatOld EQU 2 ;IX.2 前回のデータ
ChatEdg EQU 1 ;IX.1 0->1へと変化のあったデータのみ１になる
CHATSUB:
LD B,(IX+ChatOld) ;Bには前回のデータ
LD (IX+ChatOld),A ;今回のデータは保存
XOR A,B
CPL A ;前回と同じビットだけ１になる
LD B,A ;Bには変化がない場所だけ１
LD A,(IX) ;バッファのデータを取って来て
XOR A,(IX+ChatOld) ;今回とバッファで違うビットが1
AND A,B ;かつ同じ状態のBITなら
LD B,A
XOR A,(IX) ;２回同じ状態でかつ異なるビットだけ反転
LD (IX),A
AND A,B ;今回１になったビットだけ取り出す
LD (IX+ChatEdg),A
RET
同じ事をCでstatic int key=0; int chatKey(int newkey) { int wk; static int oldkey=0; wk =(~(oldkey^newkey)) & (key^newkey); //２回同じで かつ異なるbitを１に oldkey=newkey; key ^=wk; return wk & key; //今回１に変化したビットだけ１にする }
割算を使わない整数の平方根の算出
奇数を何度加算すれば引数以上になるかという方法が知られています。
x
Σ (2*n-1) =x2
n=1
これではxが大きな数だと√x に比例する時間がかかってしまいます。
大きな数でも除算命令を持たないCPUで平方根を算出する方法
平方根の算出 y=√x x:32bit幅 y:16bit幅
1)xのMSBが１になるまで左シフト、
2)xの上位16bitをyの初期値とする (yは0x8000〜0xffff)
参考:小数点の位置をMSBの左と見ると yは0.5〜0.999である事に注意
3) y=y+ (~y)/4 (yは0x9fff〜0xffff)
4) y=y+ (x-y*y)/2を８回繰り返す
例: y+= (1+ (S16)((x-(U32)y*(U32)y)>>16L))>>1;
参考:ニュートン法ではy:= y+ (x-y*y)/(2y)であるが、yで割るのを省略
5)最初の左シフト回数÷2だけy0を右シフト
参考:√(a*2m)= √(a)*2(m/2)
6)最初の左シフト回数が奇数なら46341/216を掛ける
もう少し速度が必要だったので、級数展開を使ってみます
√(x)= √(1-y) =1- 1/2*y -1/8*y^2 - 1/16*y^3 - 5/128*y^3 .....
y=1-x ですが、少数点の位置はMSBにありますので、xのビット反転で代用します(1LSBだけの誤差)
つまり ~x は 1-xの代用となります。
#define U32 unsigned long
#define U16 unsigned short
#define S32 signed long
#define S16 signed short
unsigned isqrt(U32 x)
{
int i,sft=0;
U16 y,y0,y1,sum;
U32 x0=x;
if(x<2) return x;
while( 0<(S32)x){x<<=1L; sft++;}; /*MSB=1 になるまで左シフト */
y1=x>>16L;
y0=(~y1)&0xffff;
y=(y0*0x8000)>>16; sum=y;
y=(y*y0)>>16; y=(y*0x4000)>>16;sum+=y; /*この行と次の行は掛算でなくシフトで代用可 */
y=(y*y0)>>16; y=(y*0x8000)>>16;sum+=y;
y=(y*y0)>>16; y=(y* 40960)>>16;sum+=y; /* 40960 = 5*2^13 */
y=(y*y0)>>16; y=(y* 45875)>>16;sum+=y;
y=~sum;
y+= ( (S16)((x-(U32)y*(U32)y)>>16L))>>1;
if(sft&1)
{
sft>>=1;
y= (0x8000L+(U32)(y>>sft)*46341L)>>16L;
}else {
sft>>=1; y= y>>sft;
};
return y;
}
ちなみに、
浮動少数点形式の一般的な平方根 y = √x の計算は
平方根の逆数を求める方法が一番良く知られた方法です
v=1/(√x) のニュートン法を使います
v:=v*(3-x*v*v)/2
vを繰り返し求めた後は y=x*v と１回でyが求まります。
例 x=0.5の時
初期値 v=(3-x)/2=1.25
v:=v*(3-x*v*v)/2 = 1.386
v:=v*(3-x*v*v)/2 = 1.4134
v:=v*(3-x*v*v)/2 = 1.41421288
v:=v*(3-x*v*v)/2 = 1.4142135623726
v:=v*(3-x*v*v)/2 = 1.41421356237309505 5回目にほぼ完璧
固定少数点形式では、
最初にシフトしておいて xを0.25〜0.999の範囲に調整してから使う 工夫が必要です
vの範囲は2〜1となります
1) v:= 1.6988-x/2 ;
2) v:=v*(1.393 -x*v/2) ;
3) y:=x*v
4) v:=v*(3-y*v)/2 = v+(v-y*v*v)/2
3)と4)を数回繰り返します
5) y:=x*v
6) y:= y+ v*(x-y*y)/2
を1回計算すれば実用的な精度となります。
概算として 3)と4)の繰り返し数が 1回 14bit 2回 28bit 3回 52bit
---------- その実装例 ---------------------------
{DSPスタイルの掛け算のシミュレーションの為32bitの掛算の掛算を定義}
function mpy32(X, Y: DWORD): DWORD;register; {X*Y/2^32}
asm
MUL Y
MOV EAX,EDX
end;
{32bitのMSB左側に少数点があるとした場合の平方根算出}
function isqrt32(x:DWORD):DWORD;
var i,sft:Integer;
var y,yy,v:DWORD;
begin
if x<1 then begin Result:=0;exit;end;
sft:=0;
while x<$40000000 do begin
inc(sft);
x:=x shl 2;
end;
//1.6988*2^31 =3648145221
v:= 3648145221 -(x shr 2);
//1.393 *2^31 =2991444722
v:=mpy32(v,(2991444722 -(mpy32(x,v)shr 1) ) ) shl 1;
for i:=1 to 2 do begin //32bit精度が必要ならループ数は3に
y:=mpy32(x,v);
// if(y>$7fffFFFF) then y:=$7fffFFFF; ;-実際には越えないので不要
v:=mpy32(v ,($C0000000-(mpy32(y,v) ))) shl 1 ;
end;
y:=mpy32(x,v);
y:=(y shl 1);
yy:=mpy32(y,y);
// if(y>$7fffFFFF) then y:=$7fffFFFF; ;-実際には越えないので不要
if x>yy
then y:= y+ mpy32(v,(x-yy))
else y:= y- mpy32(v,(yy-x));
y:=y shr sft;
result := y;
end;
{整数の平方根を上記の関数を使って実現すると}
function isqrt2(x:DWORD):DWORD;
begin
result:=(isqrt32(x)+$7fff) shr 16;
end;
任意の関数近似を高速に得る方法
は説明が難しいです。 関数の性質を良く調べるしかありません。
基本的には
１、級数展開を使う(私はmathcadで級数展開して使っている)
２、ニュートン法
３、多次補間やスプラインで分解
のいずれかです 上の√計算にはニュートン法の変形と級数展開を使いました
http://www.and.or.jp/~ikeuchi/makedemo/index.htm
補間的な処理をした例として、ArcTan
/****************************************************
6bit程度の精度のarctan(y,x) =atan2(y,x)*256/(2π)
****************************************************/
static signed char i3atan(unsigned short y,unsigned short x)
{
if(y < 0x4000)y<<=1;else x>>=1;
if(y>x) return 19-13+((unsigned short)(((long)13*y)/x));
return +((unsigned short)(((long)19*y)/x));
}
static signed char i2atan(short y,short x)
{
if(y >x) return (unsigned char)0x40-i3atan( x,y) ;
return i3atan(y,x);
}
signed char iatan(short y,short x)
{
if(y>=0){
if(x<0) return i2atan(-x, y)+(unsigned char)0x40;
}else{
if(x < 0) return i2atan(-y,-x)+(unsigned char)0x80;
else return i2atan( x,-y)-(unsigned char)0x40;
}
return i2atan( y, x);
}
7bit精度のsin
// $10000 * sin(x/$10000*2*π)
function iSin16( x : integer):integer;
begin
x:=SmallInt(x);
if x =-$8000 then Result:= 0
else if x < 0 then Result:= -iSin16( - x )
else if x > $4000 then Result:= iSin16( $8000 - x )
else if x >=$2000 then
begin
x:=$4000-x;
Result := $10000-((x*x) div 3496);
end else begin
Result:=(x*(205887 - ( (x*x) div 3270))) div $8000;
end;
end;
10進
１０進加算
最初にCF=0
下位桁から順に
Acc=A[n]+B[n]+CFとし CFがあるか 10以上なら Acc-=10;CF=1 とする
となります。
対して、１０進減算は命令体系にもよるけど、4bit/8bitマイコン共通に使える
アルゴリズムとしては、
１０進減算 A[n]-B[n]を実行したい時
案１、最初に CF=1
下位桁から順に Acc=~B[n] ; Acc=A[n]+Acc+CF として
CFがなければ10を足す
注:~B[n]は1の補数=全部のBITを反転する事
案２、B[n]を９の補数化し、１０進加算ルーチンを使ってCFと共に加算。
注:９の補数は 各桁毎に B[n]=9-B[n]
１００進
CPU内部で値を持つ時、特に高精度の結果表示が必要な場合は100進
法も検討すべきです。私は結構利用します。理由は、
１、表示は通常１０進で表示するが、内部が２進で高精度だと
１０進変換の時間がバカにならないオーバヘッドになる。
が、１００進ならば１０進変換も簡単
２、掛算が２進でするよりも楽だったりします。
掛算をする時中間レジスタを１６BITレジスタに取れば、
１００進正規化を毎回行わなくても余程の長精度でなけれ
ばレジスタ溢れは無いので、毎回キャリーアップをする必
要がありません。最後に下位から正規化してゆけば良いの
です。
３、加算のオーバヘッドもそれ程ではありません。Z80ならパッ
ク１０進補正の命令があるので１０進の方が加算は多少楽
ですが、１桁あたりは以下のような感じです
LD A,(BC) ;(HL)=(HL)+(BC)
ADC A,(HL)
LD (HL),A
ADD A,256-100 ;１００を引いてみて
JR NC,SKP1 ;引けたら
LD (HL),A ;CFは次の桁へ
SKP1:
４、特に小数点以下の数字を表現しなければいけない時には頭
の中が整理出来て便利ですし、0.1も２進のように無限小数
にならないので手計算と結果が良くあいます。
Delphi用の100進 多倍長演算の例 + - * / % べきの解説
100進の減算は １０進と同じです。
割算
割算を整数演算で実現する
良く知られた汎用のアルゴリズムと
固定で除算する時に汎用より少し高速なアルゴリズムです
===固定の剰余だけを高速で求めたいなら==
これは普通アセンブラで書きますが、高級アセンブラＣで記述してみました。
/**************************************************
x/y =a 余り b を 求める良く知られた方法です。
考え方としては レジスタをNbit幅 とすると
x +(a*y + b)*2^N の全体を２倍=(x,a,bを２倍)しながら
1. xが2^N 以上なら bを1増やしてxから2^N を減らす
2. b>=y なら abを１増やして bからyを１回除く
という処理で等号が成り立つようにしながらN回繰り返すと
全体が2^Nされます。つまり a*y+b=xとなるa,bが求まります
****************************************************/
unsigned Udiv (unsigned x ,unsigned y,unsigned *mod)
{
const unsigned maxbit =~((unsigned)-1>>1);
unsigned a = 0;
unsigned b = 0;
int cnt = ( sizeof(unsigned int) )*8;
while(cnt--) {
a <<= 1u;
if( x & maxbit ) a |= 1; /*a の MSBを cのLSBに */
x <<= 1u;
b <<= 1u;
if( a >= y ){ ++b ; a -= y; };
}
*mod=a;
return b;
}
/**************************************************
割算をもっと高速にしたい時 固定ならば
xをyで割る時、
2^M=yなら xをM回 左シフトすれば 求まる x>>M
2^M>yとなるMから
1) a=x>>M
b=x-y*a とすれば y*a+b=x と等号が成り立つ
2) da=b>>M として
a=a+da
b=b-y*da とすれば y*a+b=x と等号が保てる
3) b が 大きい時に 2) を繰り返します。
4) bが十分小さくなれば da=1 として
while(b >= y) {
b -= y;
a += 1;
}
****************************************************/
#define MUL3(a)((a<<1u)+a)
unsigned div3(unsigned x ,unsigned *mod)
{
unsigned a = x>>2u;
unsigned b = x-MUL3(a);
while(b > 3*2) {
unsigned da= b>>2u;
b -= MUL3(da);
a += da;
}
while(b >= 3) {
b -= 3;
a += 1;
}
*mod=b;
return a;
}
#define MUL5(a)((a<<2u)+a)
unsigned div5(unsigned x ,unsigned *mod)
{
unsigned a = x>>3u;
unsigned b = x-MUL5(a);
while(b >= 5*2) {
unsigned da= b>>3u;
b -= MUL5(da);
a += da;
}
while(b >= 5) {
b -= 5;
a += 1;
}
*mod=b;
return a;
}
実際には、割算はa/bでbが固定なら c=2^n/bとして a*c/2^n
と掛算すればそれで良い場合もあります。
bが固定でない場合でも、bがゆっくり変化する場合は
ニュートン法でcを更新すれば良い結果が得られます
ニュートン法x-f(x)/f'(x)を適用するのに
c*b-2^n=0 f(x)=b*x-2^n f'(x)=b とやったのではダメです
b-2^n/c=0 f(x)=b-2^n/x f'(x)= 2^n/x^2
x:=x-b*x^2/2^n+x=(x+x-b*x^2/2^n)=x+(x*(1-b*x/2^n));
c:=c+(c*(2^n-b*c))/2^n と更新します
例：333/10
10で割る代わりに c=2^16/10=6553 (333*6553) >> 16 =33
11に変更する場合は 6553+(6553*(65536 - (11*6553))) >>16
c=5898 再度計算 5898+(5898*(65536 - (11*5898))) >>16
c=5957
10進数で印刷するような場合１０で割るという処理は多いです
除算が無かったりlongで処理する場合の方法
x/10= x* 1/2* (1/4-1/16+1/64-1/256 ・・・)
= x* 1/2* 3 * (1/16 + 1/256 + 1/4096 +1/65536 + ・・・ )
= x* 1/2* 3 * 17 *(1/256 + 1/65536 + 1/ 16777216 + ・・・)
= x* 1/2* 3 * 17 * 257 * (1/65536 + 1/(2^32) + ・・ )
この級数方法だけで計算させるより、最初に書いた方法と
組み合わせると高速です。
unsigned long LongMul10(unsigned long x)
{ return ((x<<2L)+x)<<1L;
}
// 上位7 bit 程度の精度
unsigned long Div10Rough(unsigned long x)
{ return ((x>>2L)-(x>>4L)+(x>>6L)-(x>>8L))>>1L;
}
char div10L( unsigned long *dt)
{
unsigned x,r;
long res;
x = *dt ;
r = Div10Rough(x);
for(;;){
res= x-LongMul10(r);
if(res<0){
r -= 1+Div10Rough(-res);
}else
if(res<10) {
*dt = r; return res;
} else{
r += Div10Rough(res);
}
};
}
掛算 掛算は、掛算したい数を２進数で分解してやれば簡単。 固定の掛算は #define MUL2(a) ( a<<1 ) #define MUL3(a) ( (a<<1 ) + a) #define MUL4(a) ( a<<2 ) #define MUL5(a) ( (a<<2 ) + a) #define MUL6(a)( ( (a<<1 ) + a) <<1 ) #define MUL7(a) ( (a<<3 ) - a) #define MUL8(a) ( a<<3 ) ただしMUL7は MSBが１の場合オーバフローで正しい答えが得られない。
Digital Differential Analyzer デジタル微分解析器 直線や円を高速に描くためのアルゴリズムについては 画像処理とかの入門書に詳しいので、そちらを参考にするか、下記に詳しい解説を掲載なさっ ておられるので、そちらを参考にして下さい。 Bresenhamの線分発生アルゴリズム ライン・ルーチン(画面に直線を描画する)についての紹介 Fussy's HomePage 円の描画 http://www.netmark.ne.jp/doda8/yuji-330/java/circle/circle.htm DDAの描画以外の応用について書きます。
A/MAX というアナログ値をデジタルPORTに出す時や、モータをスイッチ
ングして調速したい時に PWMをソフトで実現する時があります。
普通のPWMは S というレジスタを用意して、一定時間毎に
extern int MAX;
int S=0;
void PWM_DAC(int A)
{
S=S+1;
if( S<A) PortOut(1); else PortOut(0);
if(S>=MAX)S=S-MAX;
}
------------------------ DDAによる方法は ------------------
extern int MAX;
int S=0;
void DDA_DAC(int A)
{
S=S+A;
if( S>=MAX) {PortOut(1):S=S-MAX;} else {PortOut(0);};
}
実際には MAXをワードサイズ丁度にすれば、インラインアセンブラで
累加算した後のCFを得て出力するだけでPWM出力が得らます。
つまり、PWM_DACより高速に演算出来、かつ分岐無しに実現出来る
メリットがあります。
それ以外にも
○低い周波数エネルギーが PWM_DACより小さくなりLPFが小さく出来る
○DAC値Aの値はいつでも変更可(PWM型は１周期待たないとマズイ事有)
とメリットがあります。
PWM_DACに比べての欠点はありません。
例 3/8 (A=3 MAX=8) の場合のPORT出力
PWM_DAC ~~~_____~~~_____~~~_____~~~_____
S 12345670123456701234567012345670
DDA_DAC __~__~_~__~__~_~__~__~_~__~__~_~ 上よりf/8の成分は減っています
S 36147250361472503614725036147250
DDA_DACはPWM_DACよりも低い成分が少ないのですが、それでもやはり特定
の周波数成分が強く出てしまいます。ですから音声用DACに使うと、だいぶ
高速に処理してもピーやジーという音が出て不快です。
それを改善するのが ΔΣと呼ばれている技術です。累加算を２重にして、
常に同じタイミングでCFが出るのを防ぎ、特定の周波数成分が集中する事を
減らす技術です。結果は、雑音量が減ると同時に、いわばノイズを足したよ
うな音に変質して不快感が減ります。
しかし制御分野で使う場合、ノイズ的な音よりピーの方が安定してるよう
に聞こえるというのもあってあまり有効ではないようです。
ΔΣ（デルタシグマの説明)
http://www.sharp.co.jp/sc/gaiyou/news/990616-1.html
ハードウエアカウンタを内蔵したチップでは、PWMの方が特性が良いですね。
この場合でも８ビットのPWMを16bit精度に拡張するのに DDAが使えます。
例えば1mSEC毎に処理したい時、水晶の値により丁度に分周出来ない事が
あります。もし、32,768Hzというタイマ入力がある時に1KHzで処理をしたい
場合、1/32 と 1/33分周を適切な比率で配置する必要があります。
DDSを使うとこれは簡単です。 32,768Hzの割り込みで
int S=32768/2; /*初期値は中央値に */
void DDA_DDS()
{
S=S+1000;
if( S>=0) {T1mSEC():S=S-32768;};
}
とすると、T1mSEC()はジッタがあるものの1mSEC毎に呼び出されます。
この形の周波数合成はＤＤＳ と呼ばれます。 F=A/B*f0 という形の周波数が
容易に得られるので、非常に便利なテクニックです。 DDA_DACと同じ形をし
ていますから、アセンブラレベルでは非常に高速に処理出来ます。
ＤＤＳ方式のファンクション・ジェネレータ
http://www.yokogawa.co.jp/Measurement/TI/gihou/fg/fg.html
AVR
http://www.toshu-ltd.co.jp/gaki/electronics/AVR/tech/DDS.html
アナデバIC
http://www.d5.dion.ne.jp/~tr1482s/dds-exp.htm
DDA:緯度経度座標とＸＹ座標スポットライトのコントロールをトラックボールでするとします。 ライトはモータ２つで緯度経度型に制御されます。 トラックボールを緯度経度に直結してしまうと、馴れないと非常に操作し難い ですね。出来ればライトが照らす軌跡がトラックボールに応じて動くように制 御したいものです。 これを考えてみましょう。 水平角度------------------- a 垂直角度------------------- b ライトの床面からの高さ h ライトの床面上の照射点迄の距離 r 照射点 X=r*sin a Y=r*cos a 移動量 重みL=sqrt(h'2 +r'2) このままx,yから a,bを逆算するのは面倒ですし、計算時間がかかります。 完全に直線を描かしたい訳でなく、操作性の改善なので、誤差があっても 高速演算出来る方が望ましいのだとします。 DDAは名前の通り微分解析ですから微分します。 水平角度の変位について dy1 =-X*da =-r*sin a da (daの影響は r に比例する。) dx1 = Y*da = r*cos a da 水直角度の変位について dy2 = cos a*L *db dx2 = sin a*L *db dy=dy1+dy2=cos a*L*db -r*sin a*da dx=dx1+dx2=sin a*L*db +r*cos a*da db=(dy*r*cos a+dx*r*sin a)/(r*cos a*L*cos a+r*sin a*L*sin a) ={r*(dy*cos a+dx*sin a)}/{r*L*(cos2a+sin2a)} db={(dy*cos a+dx*sin a)}/L da=(L*cos adx-L*sin ady)/(r*cos a*L*cos a+r*sin a*L*sin a) db=(sin a*dx+cos a*dy)/L ここで L=MB =H/(COS(b)2)) da=(cos a*dx-sin a*dy)/r ここで r=MA =H*TAN(b) まだL,rでの割り算がありますが、割算をDDAに分解するのは基本なので省略 sin,cos,1/COS2,TAN(b)の表は必要ですが、これらは荒い表で十分です。 となれば、a,bから求まる微小値を加算するだけなのでリアルタイム処理が可能 ととなった訳です。 注）上の方法では75°以上の時、操作感覚が悪くなる。実際に応用するなら平面で はなく楕円球上の軌跡を描くものとすると計算も楽で操作性も良くなります ヒント：MB = 一定 として MAは?
DDA:一般に応用するにはDDAは方程式を微分して、元の式を微小な変化の積み重ねの形で実現します。 x=f(t)-> dx=f'(t)dt , x=Σdx -->ですから、誤差について十分に検討しないと誤差が蓄積します。 差分方程式に出来れば誤差はゼロですが・・・・ -->計算に誤差があろうとデジタル累和ですから逆演算すれば元に戻る -->基本は X=A/B t です。 dX= A/B*dt なら 累和レジスタに Aを足して基準を 越えたらBを減じ同時にXをINCするという方法です。これは差分方程式であり、誤差 はありません。 係数類を実行中に調整する事も出来ます。 -->実行中 動的に係数を変化させても問題は少ないですが、誤差について要検討の事 -->結果はPID制御はに似て来ます。PID制御は係数を後で調整するのですが、 DDAはキチンと方程式を解いて制御するのでホントに最適制御であると自信が持てます
ステップモータの回し方
ステップモータドライバがあるとすれば
出力は矩形波で、回転方法によりUP/DNを出せば良い訳です。
出力周波数fについて
fmin<f <fminの間 f=aT (Tは時間)で加速しなければいけません。
設定値として fmin,fmax,a の３つが独立して設定出来た方が良い
です。例えばお客さんは常にフルスピードで動かしたいとは限りま
せん。時にはゆっくりと回したいかもしれません。時には特別重い
物を載せる為に加速度aを下げなければいけないかもしれません。
たぶんfmin,fmaxの代わりに Nmin=F/fmin Nmax=F/fmaxの方が楽
だと思います。
巧い方法があります。 DDAです。
今レジスタWとレジスタVがあり、このレジスタを 1/b秒に１回
W:=W+V if W>0 then W:=W-b
V:=V+a (減速中はaは負数になる)
と２つの計算をしながらWが正になる度に出力パルスを出せば
等加速処理を実現出来ます。ただし、bは十分に小さな値でな
ければならずこれでも厳しい場合があるので これをT回まとめ
て計算させると
W:=W+VT+aT(T+1)/2 ; while W>0 do W:=W-b
V:=V+ aT ;
aT,aT(T+1)/2は固定値なので予め計算しておく事が出来ます。
Tを２のべき乗に選べば VTはVをずらすだけです。
仮に1.05MHzで動くシステムがあるとします。b=1.05MHzとし
4,096KHzの割り込みで(T=256)
W:=W+VT+aT(T+1)/2 ; while W>0 do W:=W-b
V:=V+ aT
という演算をするとします。 V=10KHzの時、 VT=2.6Mですから
１回の割り込みで数パルス出さなければならなくなります。これ
は困りました。
しかしハードウエアカウンタがあります。ハードウエアカウン
タは1.05MHzで動きます。これを使えないでしょうか？
W:=W+VT+aT(T+1)/2 ; V:=V+ aT だけを 4,096Khzの割り込み
で処理し、 ハードウエアカウンタの割り込みで W:=W-b を実行さ
せたとします。
ハードウエアカウンタの設定と、現在の速度が一致していれば
Wはゼロになる筈ですし、ハードウエアカウンタが早すぎればWは
負数になります。ですからW:=W-bを実行した後のWの値によって現
在のカウンタを補正してやれば巧くゆきそうです。
さてこれで加速減速は巧くゆきそうですが問題はそれだけではありません
例えば片方の軸をLだけ動かさなければいけない時 加速は簡単だけど
いつ減速を始めれば良いでしょうか？方程式を解けば答えはでるでしょ
うが、そんな演算処理時間はありません。 でも簡単な解決方法があり
ます。加速にかかった距離だけ減速にかかることを利用すればよいのです。
● 加速中、f<fmaxの間にL/2を越えてしまったら即座に減速を開始
しなければいけません。
● f>fmaxに達したら定速運転に入りますが、それまでに動いた距離
だけ減速に必要な事を覚えておきます。
これを処理する為に カウンタを２つGDEC,GINC用意してみます。
最初 GDEC:=0 GINC:=0 です。
（簡単の為に移動量が正だけとして考えます)
1.設定カウンタはGDECに設定します GDEC:=GDEC+移動量
2,加速中は1STEP毎に GINCを+1し ,GDECを-1する
GINC>GDEC になれば=> 減速
3,定速度運転中はGDECだけを-1する
GINC>GDEC になれば=> 減速
4,減速中は1STEP毎に GINCを+1し ,GDECを+1する
● 移動中にLが変更されても上の事がそれなりに動くともっと良い
事を考えましょう。もちろんLが短縮されてしまった場合は止まれ
ない事もある訳で、常に成功させる必要はないでしょうが。
上の事を巧く解決しても、２軸の場合はもっと難しい問題があります。
移動量 XL,YLだけ移動する命令を渡した時のユーザーは当然の
ようにその間を直線を描いて動く事を期待しています。XL=YLでかつ
加減速パラメータが同じなら問題はないでしょうが、どれかが違えば
単純に１軸の場合の加減速処理を実行すれば直線を描きません。
簡単なステップモータ制御
pogを用意します。
up,信号のエッジで pog=pog+1
dn,信号のエッジで pog=pog-1
カウンタwaitcを用意します。
サブルーチン WAITSUB を用意し、WAITSUBではwaitcの命令サイクルだけ待ちます
例：
LD HL,WT_LAB
SUB HL,(waitc)
PUSH HL
ret ;JP HL と同じなので JP HLでも良い
nop つまり、waitcの個数だけNOPを実行するサブルーチン
nop
nop
nop
nop
nop
nop
nop
WT_LAB: ret
メインでは
RESTART:
waitc=INITWAIT;
MAIN:
CALL WAITSUB
if pog==0 then MAIN
if pog >0 then UP_ROTATE
else DN_ROTATE
UP_ROTATE:
LD A,(STEP)
ADD A,1
DAA １０進補正命令なので、0A -> 10になります
AND A,0fh
LD (STEP),a
CALL PortOUT ;aの状態を出力する
dec (pog)
; 待ち時間が INITWAITを越える迄
; 何命令後に pog >0 になったか記録。それを次のwaitcに
CALL POGWAIT
if pog==0 then goto RESTART
UP_ROTATELP:
LD A,(STEP)
ADD A,1
DAA
AND A,0fh
LD (STEP),a
CALL PortOUT ;aの状態を出力する
dec (pog)
CALL WAITSUB
if waitc!=0 and pog>1 then dec (waitc)
UP_ROTATEWT:
if pog<0 THEN GOTO UP_ROTATELP
inc (waitc)
if waitc>INITWAIT THEN GOTO RESTART
JP UP_ROTATEWT
ビットと数
まあCならビットフィールド、pascalなら集合型を使えば良いのだろうが、
& | , AND OR を使っても簡単。
Bを２進でnビット目だけがonとすると B = ( 1 << n )
Bより右側(下位ビット)が全部onなら B-1
Bより左側とBが onなら -B
例： xの LSB以外のビットがひとつでもonかどうか判定
if(x and -2) then BooBoo;
応用:モノクロプリンタなどの為のビットイメージ処理にて
/**************************************/
/* pの xbit目からybit目迄を fでマスクしてセットする */
static const unsigned char bittbl[]={0x80,0x40,0x20,0x10,0x08,0x04,0x02,0x01};
void YokoSet(unsigned x,unsigned y,unsigned char *p,unsigned char f)
{
if(x>y){unsigned w=x;x=y;y=w;}
{
unsigned char bs= bittbl[x&0x07];
unsigned char be= bittbl[y&0x07];
x>>=3;
y>>=3;
if(x<y){ p[x++]|= bs| (( (bs-1))&f);/*LSB方向にビットをセットする*/
for(;x<y;)p[x++]|= f;
p[x ]|= be| ((~(be-1))&f);/*MSB方向にビットをセットする*/
}else p[x ]|= be|bs|((bs-be)&f);/*同じbyteの時 */
}
}
bが２のベキ乗なら
a % b |
a & (b-1) |
| a -( a % b ) | a & ( -b) |
n個のデータの平均という時、そのデータが円周上にあるデータの場合がある。
この場合普通の平均は何の意味もない。
平均の式として D=(A+B)/2 ではなく D=A+(B-A)/2 か D=B+(A-B)/2 を使う
１）最初に C=(B-A)を求める。結果が、負数ならnを足す。
２）平均を求める程AとBが近くないなら平均を求める意味がない。そこで、
L<n/2であるLを定めておく
３） Cが L より小さいなら D=A+C/2
Cが n-L より大きいなら D=B+(n-C)/2
例 n= 60 L=20
A B C D
2 50 48 B+(60-C)/2=56
50 2 -48->12 A+ C/2 =56
40 50 10 A+ C/2 =55
50 40 -10->50 B+(60-C)/2=55
☆ MOD n M個の平均
２個以上の場合は上の方法が使えない。そこで
M個のデータ K1,K2,...,KM の平均に相当する値として
S=Σ SIN(2π*i/n)*Ki （SINテーブルを引いて足すだけ）
C=Σ COS(2π*i/n)+Ki （上のSINテーブルをn/4ずらして引けばよい）
と求めたS,C より
W=S*SIN(2π*X/n) +C*COS(2π*X/n)
であるWが最大になるXを平均の代用とする。
それは G=S*COS(2π*X/n) - C*SIN(2π*X/n)=0 となるXのどちらか
Xを実際に求めるには、最初に使ったSINテーブルより求める
S,Cの符号判断によって 1/4 の区間に狭まり、その区間内で
G= S*COS(2π*X/n) - C*SIN(2π*X/n) は単調増加なので
２分検索で探せば LOG(n) の検索で求まる
例 n=60 K1= 2 K2=50
S=SIN(2π*2/60)+SIN(2π*50/60) = -0.658
C=COS(2π*2/60)+COS(2π*50/60) = 1.4781
X=arctan(S/C)*60/2π=-4=56
つまり２個の場合、上と同じ結果が得られる。
ここで、Xが求まった後
W=S*SIN(2π*X/n) +C*COS(2π*X/n)
の大きさが精度を示している。バラツキが大きい程Wは小さい。
全てのKiが同じなら Wは Mに等しく、逆に均等にデータがばらついて
いればWは0になる。よって (1-W/M)かM/W をバラツキ指標として使う事が
出来る。
MOD n=12 の平均
S=Σ 12 SIN(2π*i/12)*Ki
C=Σ 12 COS(2π*i/12)*Ki
この計算は下のテーブルを検索して加算するだけで良い。
0 12
0 3 12
6 10
1 8 8
10 6
2 12 3
12 -0
3 12 -3
10 -6
4 8 -8
6 -10
5 3 -12
-0 -12
6 -3 -12
-6 -10
7 -8 -8
-10 -6
8 -12 -3
-12 0
9 -12 3
-10 6
A -8 8
-6 10
B -3 12
0 12
S,Cから 平均値 R を求めるには
１）S,Cの符号を見る これで 012 345 678 9AB と1/4迄絞り込める
とりあえず、 R= 1 -4 7 -A と しておく
R
S>=0 C>=0 1
S>=0 C< 0 -4
S< 0 C>=0 7
S< 0 C< 0 -A
２） |S|,|C|を比べる
その後小さい方の２倍の絶対値と 大きい方の絶対値を比べる
判断結果によって、Rを inc/dec して絶対値を取れば良い
R=1 -4 7 -A
S < C , 2S < C -> 0 5 6 B |R-1|
S < C , 2S >= C -> 1 4 7 A | R |
C < S , 2C >= S -> 1 4 7 A | R |
C < S , 2C < S -> 2 3 8 9 |R+1|
プログラムの流れ方の実現方法
１）メインルーチン サブルーチン
CALLによって メインルーチンから呼びだされ、RETURN命令によっ
てメインルーチンに帰ります。
実現は ＣＰＵに備わったCALL,RETをそのまま使えば良いので簡単
です。
※ しかし、常にメイン、サブと完全に分けられる訳ではありません。
例えばプログラムを外注に出して、データ発生部をＡ君に、処理部
をＢ君に発注したら、２人ともメインルーチンとしてコーデングし
てしまったとしましょう。
Ａ君の作ったPROGAは当然 CALL OUTPUT と、 サブルーチンとして
Ｂ君を呼ぼうとするし、反対にＢ君のPROGBは CALL INPUT とＡ君の
プログラムからデータを受け取ろうとします。 これを解決するのが
コルーチンです。
２）コルーチン
CALL によって互いに呼びだします。 片方から見れば片方が
サブルーチンに見えます。実現方法は簡単で、スタックポインタを
切り替えれば良く、高速です。
CO_GATE:
EX (SP_BUF),SP
RET
という簡単なゲートルーチンを " CALL CO_GATE "で互いに呼べば
２つのプログラムがきり替ります。
マルチタスク等のカーネルも原理はこれと同じです。
上の Ａ君、Ｂ君の プログラムはそのままで、OUTPUT,INPUTの
両方のラベルを CO_GATEと再定義し、始動ルーチンとして
LD SP,STACKA ;Ａ君用のスタックを確保して
LD HL,PROGA ;Ａ君の先頭アドレスを
PUSH HL ; スタックに種として用意する
LD (SP_BUF),SP
LD SP,STACKB ;次にＢ君用のスタックを用意して
CALL PROGB ;Ｂ君を呼び出す。
と用意してやればよいのです。
B君のルーチンから CALL INPUTを実行すれば CO_GATEのRETに
よってPROGAから走り出し、PROGAで必要なデータが作成されたな
らCALL OUTPUTによってB君のプログラムの CALL INPUT の次に
帰って来ます。
３）フロートサブルーチン
完全なコルーチンにするには、スタック領域が２つ必要なのでメモリの少ない
１チップマイコンでは使えません。 そこで、１）と２）の中間のような
テクニックが使われます。 それは、メインルーチンから見てサブルーチンの
先頭番地が刻々と変わるようなサブルーチンというイメージのテクニックです
GATE1: LD HL,(PRG1PC)
JP HL
上の ゲート を メインから "CALL GATE1"と呼びだすか、
LD HL,(PRG1PC)
CALL HL
という２行のマクロを使って呼び出し、
サブルーチン側のゲートは
GATE2: POP HL
LD (PRG1PC),HL
RET
と、帰り番地だけを取りだして保存しておきます。
これならサブルーチン側の実行番地の為の２バイトの固定記憶領域があれば実現
出来ます。少ないメモリを効率的に使わなければならない１チップに向いている
のです。ただし、この場合、スタックがメインルーチンと共通なので、
"CALL GATE2"を実行する時のスタックレベルに注意しなければいけません。
たとえば"CALL GATE2"を PUSH,POPで挟む事や、サブルーチンから 実行する事が
出来ない訳です。
★フロートサブルーチンの応用 １
123crと出力したい時、
SEND123:
LD A,'1'
CALL SEND
LD A,'2'
CALL SEND
LD A,'3'
CALL SEND
LD A,0DH
CALL SEND
RET
と簡単に書いてしまうと、SEND に時間が必要な場合、通常は問題が起きます
例えば、シリアルの送信やプリンタが相手では、一定時間待たなければ次ぎ
の送信が出来ません。そしてこれを呼びだしたメインルーチンは通常 急いで
います。 たとえば、必ず１定時間毎にADCからデータを取り出さなければな
らない仕様だったとしましょう。 SEND で処理が終わる迄メインに帰らない
場合、ADCからの受取処理を割込み中でやらなければならなくなります。しか
し、そのようなスタイルは割込み処理時間を増し、結局バグの種になります。
こんな時、このコーデングスタイルのままでフロートサブルーチンとします。
そのために少々の仕掛けを必要とします。メモリにSENDPCというレジスタを
用意し、
LDW (SENDPC),SEND123
と、SEND123の実行先頭番地を入れておいて、
CALL SER_SEND
という１行でメインの空き時間に呼び出します。SER_SENDでは、SENDPCの
アドレスにジャンプするようにしておきます。
もちろん、SENDの方にも仕掛けがあり、
送信処理をして直に
POP HL
LD (SENDPC),HL
RET
と、呼びだした側にRETしてしまいます。 91P640のシリアルの場合、具体的な
コーデングは、
SEND: LD (IRFH),78H/8 ;クリア
LD (SCBUF),A ;シリアル送信する。
LD (SV_BC),BC ;Acc以外の全部のレジスタを保存する
LD (SV_DE),DE
LD (SV_HL),HL
LD (SV_IX),IX
LD (SV_IY),IY
LD BC,(BX)
LD (SV_BX),BC
POP HL ;返り番地を保存
LD (SENDPC),HL
RET ;一つ手前に帰る
SER_SEND:
BIT 0,(IRFH) ;IRFTX 送信要求フラグ
RET Z ;送信中
LD HL,(SENDPC)
PUSH HL ;帰り番地として実行アドレスを記入
LD BC,(SV_BX)
LD (BX),BC
LD BC,(SV_BC)
LD DE,(SV_DE)
LD HL,(SV_HL)
LD IX,(SV_IX)
LD IY,(SV_IY)
RET
★フロートサブルーチンの応用 ２
フロートサブルーチンではサブルーチンが使えないと上で言っていますが、
実際にはサブルーチンは多用します。たとえば１６進の出力をしたい場合
LD A,(mem)
CALL HEXPRINTA と書けばAが１６進で出力されたらどうでしょう。
その為の仕掛けは簡単です。呼ばれたサブルーチン側で帰り値をメモリに
保存しておけばよいのです。
;++++++++++++++++++++++++++++++++++++++++
;++ 16進 出力処理 A,B,DEは壊れる
;++++++++++++++++++++++++++++++++++++++++
ISEG REL ZEROPAGE
HEX_RET: DS 2
CSEG REL PROGRAM
HEXPRINTA:
LD B,A
HEXPRINTB:
POP DE ;帰り番地を取り出す
LD (HEX_RET),DE
HEXPRINTB_:
LD A,B
CALL HEXAH
LD A,B
CALL HEXAL
LD DE,(HEX_RET)
JP DE ;帰り番地にJMP
HEXAH:
SRLA
SRLA
SRLA
SRLA
HEXAL: AND A,0FH
ADD A,'0'
CP A,'9'
JR ULE,TX_SEND ;->１文字出力
ADD A,'A'-'9'-1
JP TX_SEND
------------------------------------------------------------
長軸短軸径がa,b 傾斜角度がrad(ラジアン) 中心をx0,y0に
Win95の PolyBezier(hdc,dt,18+1) を利用していますから Delphi1では
使えません。
[アルゴリズム解説]
半径１単位円の1/nを ベジェ曲線に当て嵌め、それを座標変換する事で
楕円を描かせます。
■ベジェ曲線の方程式は
x(t)= (1-t)3*x1+3(1-t)^2*t*x2+3(1-t)*t^2*x3+t3*x4
y(t)= (1-t)3*y1+3(1-t)^2*t*y2+3(1-t)*t^2*y3+t3*y4
■ベジェ曲線への当て嵌め方は
1) 端点が一致する事
x1=cos(0),y1=sin(0) ,x4=cos(Φ),y4=sin(Φ);
2) 中間点が一致する事
(x1+3x2+3x3+x4)/8 =cos(Φ/2)
(y1+3y2+3y3+y4)/8 =sin(Φ/2)
3) 微分が t=0,t=1で一致する事
t=0 ---> x2=x1
t=1 ---> -x3+x4 / -y3+y4 =-tan(Φ)
上記で Φ=2π/4の時が fEllipse4 (CADなどで画面表示するのに使われています)
Φ=2π/6 として求めたものが fEllipse6 ( 印刷にも十分な精度です )
任意角度で求めたのが fEllipseM
座標点が必要なら、ここの BezierData を参考に
{ 精度 1/10000 の斜め楕円描画 }
function fEllipse6(hdc:THandle;a,b,rad,x0,y0:double):boolean;
var i,j:integer;
dt :array[0..18]of TPoint;
var dx :array[0..18]of double;
var dy :array[0..18]of double;
var x,y,s,c,r3:double;
begin
r3:=sqrt(3);
dx[0]:=1;
dy[0]:=0;
dx[1]:=1;
dy[1]:=8/3 -4.0/3.0*r3;
dx[2]:=4/3*r3 -3/2;
dy[2]:=7/6*r3 -4/3;
c:=2*pi/6;
s:=sin(c);
c:=cos(c);
for i:=1 to 5 do
for j:=0 to 2 do begin
x:= dx[ (i-1)*3+j];
y:= dy[ (i-1)*3+j];
dx[i*3+j]:= x*c-y*s;
dy[i*3+j]:= x*s+y*c;
end;
dx[18]:=1;
dy[18]:=0;
s:=sin(rad); c:=cos(rad);
for i:=0 to 18 do begin
dt[i].x:=round(c*a*dx[i]-s*b*dy[i]+x0);
dt[i].y:=round(c*b*dy[i]+s*a*dx[i]+y0);
end;
Result:=PolyBezier(hdc,dt,18+1);
end;
function fEllipse4(hdc:THandle;a,b,rad,x0,y0:double):boolean;
var i,j:integer;
dt :array[0..12]of TPoint;
var dx :array[0..12]of double;
var dy :array[0..12]of double;
var x,y,s,c:double;
begin
dx[0]:=1;
dy[0]:=0;
dx[1]:=1;
dy[1]:=4.0/3.0*(sqrt(2)-1.0);
dx[2]:=dy[1];
dy[2]:=1;
c:=2*pi/4;
s:=sin(c);
c:=cos(c);
for i:=1 to 3 do
for j:=0 to 2 do begin
x:= dx[ (i-1)*3+j];
y:= dy[ (i-1)*3+j];
dx[i*3+j]:= x*c-y*s;
dy[i*3+j]:= x*s+y*c;
end;
dx[12]:=1;
dy[12]:=0;
s:=sin(rad); c:=cos(rad);
for i:=0 to 12 do begin
dt[i].x:=round(c*a*dx[i]-s*b*dy[i]+x0);
dt[i].y:=round(c*b*dy[i]+s*a*dx[i]+y0);
end;
Result:=PolyBezier(hdc,dt,13);
end;
{
Nを変更すれば任意角度で分割出来ます。
これを応用すれば楕円弧が描けます。
}
function fEllipseM(hdc:THandle;a,b,rad,x0,y0:double):boolean;
var i,j:integer;
dt :array[0..94]of TPoint;
var dx :array[0..94]of double;
var dy :array[0..94]of double;
var x,y,s,c:double;
var N:integer;
var xc,yc,fa:double;
begin
N:=6; { N分割した場合です }
fa:=2*PI/N;
xc:=cos(fa/2);
yc:=sin(fa/2);
s:=sin(fa);
c:=cos(fa);
dx[0]:=1;
dy[0]:=0;
dx[1]:=1;
dy[1]:=-4*c*(1+c-2*xc)/s/3+4*(2*yc-s)/3;
dx[2]:=8*xc/3-c/3-4/3;
dy[2]:=4*c*(1+c-2*xc)/s/3+s;
for i:=1 to N-1 do
for j:=0 to 2 do begin
x:= dx[ (i-1)*3+j];
y:= dy[ (i-1)*3+j];
dx[i*3+j]:= x*c-y*s;
dy[i*3+j]:= x*s+y*c;
end;
dx[N*3]:=1;
dy[N*3]:=0;
s:=sin(rad); c:=cos(rad);
for i:=0 to N*3 do begin
dt[i].x:=round(c*a*dx[i]-s*b*dy[i]+x0);
dt[i].y:=round(c*b*dy[i]+s*a*dx[i]+y0);
end;
Result:=PolyBezier(hdc,dt,N*3+1);
end;
∫{√((sin(T)*a)2 +(cos(T))2)}dT を 0〜πの範囲で定積分
k = 1-a*aとおいて
∫{√(1-k*sin(T)2}dT でも同じ 0〜π/2を求めて２倍しても同じ
2*∫√(1-k*t*t)/(1-t*t)dt これを0〜１の範囲で積分しても同じ
(ルシャンドラの第2種完全楕円積分というらしい)
kを級数展開しても級数項の規則が結構厄介
k=0〜0.5 0.5〜1の範囲で分けないと少数以下3桁は厳しい
長軸と短軸の比率をaとします。長軸方向にa倍すれば 真円になります
真円と楕円をこうして対応させます。
微少角度dTの周長はラジアンで計算すると 真円なら常にr*dTになります。
対応する楕円の周長は dTが微少ならば 角度Tに対して
r*dT*√((sin(T)*a)2 +(cos(T))2) で近似出来ます
この式を0〜2πの区間で積分すれば求まります。
定積分は楕円積分という厄介な奴が相手なので数値積分という方法で
Dを長軸 aは短軸/長軸 として 以下で求めた S*Dが周長の近似値 です
あらかじめ aに応じてS表を作っておく方法もあると思います
dT<- π/N Nは大きな値
T <-0
S <-0 Sに求める周長の近似値が得られる
k <-1-a*a
for i=1 to N do
begin
S:= S+SQRT(1-k*sin(T)2)*dT ;
T:= T+dT;
end;
知られている近似式 π* √{(長軸2+短軸2)/2}
aが0付近では 2+a2/2
aが1付近では π( (2- a)/2)
しかし実際には aが0.8以上でやっと2桁の精度
少数２桁迄の近似式 (2-π)(1-x^1.55)^1.08+π