Kosaka Lab / Mbed 2 deprecated BLDCmotor

UVW 3 phases Brushless DC motor control

Dependencies:   QEI mbed-rtos mbed

Fork of BLDCmotor by manabu kosaka

fast_math.cpp@17:1ac855d69c78, 2013-09-07 (annotated)

Committer:
kosakaLab
Date:
Sat Sep 07 02:47:09 2013 +0000
Revision:
17:1ac855d69c78
Parent:
13:791e20f1af43 
for TBLM-01 18T and OME-360-2MCA

Who changed what in which revision?

UserRevisionLine numberNew 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]){ // ２ノルムを計算
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