UVW 3 phases Brushless DC motor control
Dependencies: QEI mbed-rtos mbed
Fork of DCmotor by
fast_math.cpp@14:7f83c4b96d34, 2013-06-07 (annotated)
- Committer:
- kosaka
- Date:
- Fri Jun 07 08:49:44 2013 +0000
- Revision:
- 14:7f83c4b96d34
- Parent:
- 13:791e20f1af43
mbed-rtos is updated
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kosaka | 12:a4b17bb682eb | 1 | #include "mbed.h" |
kosaka | 12:a4b17bb682eb | 2 | #include "fast_math.h" |
kosaka | 12:a4b17bb682eb | 3 | |
kosaka | 13:791e20f1af43 | 4 | unsigned short sin60[DEG60+1]; // 0~60度, 振幅65535のsinテーブル(最大誤差0.003%) from 0 to 60 deg. (max precision error is 0.003%) |
kosaka | 12:a4b17bb682eb | 5 | |
kosaka | 12:a4b17bb682eb | 6 | long _sin(unsigned short th){ // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) |
kosaka | 13:791e20f1af43 | 7 | // 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI) |
kosaka | 13:791e20f1af43 | 8 | // 出力 : 65535*sin(th) |
kosaka | 13:791e20f1af43 | 9 | // init_fast_math(); |
kosaka | 12:a4b17bb682eb | 10 | // if( th>2.*PI ){ th -= 2*PI*(float)((int)(th/(2.*PI)));} |
kosaka | 12:a4b17bb682eb | 11 | // th_int = (unsigned short)(th/(PI/3.)*(float)DEG60+0.5); // rad to deg |
kosaka | 12:a4b17bb682eb | 12 | // sin = (float)_sin(th)/65535.; |
kosaka | 12:a4b17bb682eb | 13 | unsigned short f_minus; |
kosaka | 12:a4b17bb682eb | 14 | long x; |
kosaka | 12:a4b17bb682eb | 15 | |
kosaka | 13:791e20f1af43 | 16 | // sinがマイナスのとき、thから180度引いて、f_minus=1にする |
kosaka | 12:a4b17bb682eb | 17 | if( th>=DEG60*3){ f_minus = 1; th -= DEG60*3;} // if th>=180deg, th = th - 180deg; |
kosaka | 12:a4b17bb682eb | 18 | else{ f_minus = 0;} // else , f_minus = on. |
kosaka | 12:a4b17bb682eb | 19 | |
kosaka | 13:791e20f1af43 | 20 | if( th<DEG60 ){ // th<60度のとき |
kosaka | 12:a4b17bb682eb | 21 | x = sin60[th]; // sin(th) |
kosaka | 13:791e20f1af43 | 22 | }else if( th<DEG60*2 ){ // 60≦th<120度のとき |
kosaka | 12:a4b17bb682eb | 23 | x = sin60[DEG60*2-th] + sin60[th-DEG60]; // sin(th)=sin(th+60)+sin(th-60)=sin(180-(th+60))+sin(th-60) because sin(th+60)=s/2+c*root(3)/2, sin(th-60)=s/2-c*root(3)/2. |
kosaka | 13:791e20f1af43 | 24 | }else{ // 120≦th<180度のとき |
kosaka | 12:a4b17bb682eb | 25 | x = sin60[DEG60*3-th]; // sin(60-(th-120))=sin(180-th) |
kosaka | 12:a4b17bb682eb | 26 | } |
kosaka | 13:791e20f1af43 | 27 | if( f_minus==1 ){ x = -x;} // sinがマイナスのときマイナスにする |
kosaka | 12:a4b17bb682eb | 28 | return(x); |
kosaka | 12:a4b17bb682eb | 29 | } |
kosaka | 12:a4b17bb682eb | 30 | |
kosaka | 12:a4b17bb682eb | 31 | long _cos(unsigned short th){ // return( 65535*sin(th) ), th=rad*DEG60/(PI/3)=rad*(512*3)/PI (0<=rad<2*PI) |
kosaka | 13:791e20f1af43 | 32 | // 入力 : th = rad*DEG60/(PI/3)=rad*(512*3)/PI, (0<=rad<2*PI) |
kosaka | 13:791e20f1af43 | 33 | // 出力 : 65535*cos(th) |
kosaka | 13:791e20f1af43 | 34 | th += DEG60*3/2; |
kosaka | 12:a4b17bb682eb | 35 | th += DEG60*3/2; |
kosaka | 12:a4b17bb682eb | 36 | if( th>=DEG60*6 ){ th -= DEG60*6;} |
kosaka | 12:a4b17bb682eb | 37 | return( _sin(th) ); |
kosaka | 12:a4b17bb682eb | 38 | } |
kosaka | 12:a4b17bb682eb | 39 | |
kosaka | 12:a4b17bb682eb | 40 | void init_fast_math(){ // sin0-sin60deg; 0deg=0, 60deg=512 |
kosaka | 12:a4b17bb682eb | 41 | int i; |
kosaka | 12:a4b17bb682eb | 42 | |
kosaka | 13:791e20f1af43 | 43 | for( i=0;i<=DEG60;i++ ){ // 0~60度までのsinテーブルをつくるset sin table from 0 to 60 deg.. |
kosaka | 12:a4b17bb682eb | 44 | // sin60[i] = (unsigned short)(sin((float)i/512.*PI/3.)); |
kosaka | 12:a4b17bb682eb | 45 | sin60[i] = (unsigned short)(65535.*sinf((float)i/(float)DEG60*PI/3.)); |
kosaka | 12:a4b17bb682eb | 46 | } |
kosaka | 12:a4b17bb682eb | 47 | } |
kosaka | 12:a4b17bb682eb | 48 | |
kosaka | 12:a4b17bb682eb | 49 | #if 0 |
kosaka | 12:a4b17bb682eb | 50 | //float norm(float x[0], float x[1]){ // 2ノルムを計算 |
kosaka | 12:a4b17bb682eb | 51 | // return(sqrt(x[0]*x[0]+x[1]*x[1])); |
kosaka | 12:a4b17bb682eb | 52 | //} |
kosaka | 12:a4b17bb682eb | 53 | float sqrt2(float x){ // √xのx=1まわりのテイラー展開 √x = 1 + 1/2*(x-1) -1/4*(x-1)^2 + ... |
kosaka | 12:a4b17bb682eb | 54 | // return((1+x)*0.5); // 一次近似 |
kosaka | 12:a4b17bb682eb | 55 | return(x+(1-x*x)*0.25); // 二次近似 |
kosaka | 12:a4b17bb682eb | 56 | } |
kosaka | 12:a4b17bb682eb | 57 | #endif |