迭代法定点求actan代码实现

最新推荐文章于 2022-04-06 21:12:22 发布

逃跑de木偶

最新推荐文章于 2022-04-06 21:12:22 发布

阅读量1.1k

点赞数 3

分类专栏：马达控制

本文链接：https://blog.csdn.net/qq_21513281/article/details/80748691

版权

马达控制专栏收录该内容

4 篇文章 6 订阅

订阅专栏

#define   NEG_PI_BY_2	  (-16384)
#define	  PI_BY_2	  (16384)
#define	  PI		  (32767)
#define	  NEG_PI	  (-32767)
uint16_t CORDIC_DATA[16] = {0x2000,0x12E4,0x09FB,0x0511,0x028B,0x0146,0x00A3,0x0051
                            ,0x0029,0x0014,0x000A,0x0005,0x0003,0x0001,0x0001,0x0000
						   };
int32_t atan2CORDIC(int32_t XX, int32_t YY)
{
	int32_t Angle = 0;
	int32_t j;                  // for iteration      
	int32_t N = 16;          // loop times     
	int32_t abs_XX[17] = {0};   
	int32_t abs_YY[17] = {0}; 
	if (XX == 0 || YY == 0) 
	{   
        if (XX ==0) {   
            if (YY > 0)    
                Angle = PI_BY_2;   
            else if(YY < 0)   
                Angle = NEG_PI_BY_2;   
            else Angle = 0;   
        }   
        if (YY ==0) {   
            if (XX >= 0)    
                Angle = 0;   
            else if(XX < 0)   
                Angle = PI;   
        }   
    }
	else
	{
		abs_XX[0] = Abs(XX);
		abs_YY[0] = Abs(YY);
	
		for (j = 0; j < N-1 ; j++) 
		{   
				if (abs_YY[j] > 0) {   
					Angle = Angle + CORDIC_DATA[j];   
					abs_XX[j + 1] = abs_XX[j] + (abs_YY[j] >> j);   
					abs_YY[j + 1] = abs_YY[j] - (abs_XX[j] >> j);   
				}   
				else if (abs_YY[j] < 0) {   
					Angle = Angle - CORDIC_DATA[j];   
					abs_XX[j + 1] = abs_XX[j] - (abs_YY[j] >> j);   
					abs_YY[j + 1] = abs_YY[j] + (abs_XX[j] >> j);   
				}   
				else Angle = Angle;   
		 } 
		 // judge which XXuadrant estiTheta is    
        if (XX > 0) {   
            if (YY >0) {          // the first XXuadrant   
                Angle = Angle;   
            }   
            else if (YY < 0) {    // the forth XXuadrant   
                Angle = -Angle;   
            }   
        }   
        else if (XX < 0) {   
            if (YY >0) {          
                Angle = (32767 - Angle);  
            }   
            else if (YY < 0) {    // the third XXuadrant   
                Angle = (Angle - 32767);   
            }   
        }   
	}	
	return (Angle);
}