bujindianji

/*----------------------------------------------------------------------------*/


#include "timer.h"
#include "delay.h"
#include "sys.h"
#include <math.h>
#include <stdlib.h>
#include "stepmotor.h"



typedef enum
{
    FALSE,
    TRUE        
}BOOL_enum;

typedef enum
{
    X_AXIS,
    Y_AXIS,
    N_AXIS
}AXIS_enum;

typedef enum
{
    X_MOVE,
    Y_MOVE,
}MOVE_type_enum;

typedef enum
{
    Circle_CW,
    Circle_CCW,
}Circle_type_enum;

typedef struct
{
    double xc;
    double yc;
    double off_xc;
    double off_yc;
}Circle_param_struct;

//定时器装载值表
s16 ACB_C[]={2499,2421,2350,2284,2224,2168,2117,2068,2023,1981,1942,1904,1869,
1835,1804,1774,1745,1718,1692,1667,1643,1620,1598,1577,1557,1537,1519,1501,1483,
1466,1450,1434,1419,1404,1390,1376,1362,1349,1337,1324,1312,1300,1289,1278,1267,
1256,1246,1236,1226,1217,1207,1198,1189,1180,1172,1163,1155,1147,1139,1132,1124,
1117,1109,1102,1095,1088,1082,1075,1069,1062,1056,1050,1044,1038,1032,1026,1021,
1015,1010,1004,999,994,989,984,979,974,969,964,959,955,950,946,941,937,933,928,
924,920,916,912,908,904,900,896,893,889,885,882,878,875,871,868,864,861,857,854,
851,848,844,841,838,835,832,829,826,823,820,817,814,812,809,806,803,801,798,795,
793,790,787,785,782,780,777,775,772,770,768,765,763,760,758,756,754,751,749,747,
745,743,740,738,736,734,732,730,728,726,724,722,720,718,716,714,712,710,708,707,
705,703,701,699,697,696,694,692,690,689,687,685,684,682,680,679,677,675,674,672,
670,669,667,666,664,663,661,660,658,657,655,654,652,651,649,648,646,645,644,642,
641,639,638,637,635,634,633,631,630,629,627,626,625,623,622,621,620,618,617,616,
615,613,612,611,610,609,607,606,605,604,603,602,600,599,598,597,596,595,594,593,
592,590,589,588,587,586,585,584,583,582,581,580,579,578,577,576,575,574,573,572,
571,570,569,568,567,566,565,564,563,562,561,560,559,558,558,557,556,555,554,553,
552,551,550,550,549,548,547,546,545,544,543,543,542,541,540,539,538,538,537,536,
535,534,534,533,532,531,530,530,529,528,527,527,526,525,524,523,523,522,521,520,
520,519,518,518,517,516,515,515,514,513,512,512,511,510,510,509,508,508,507,506,
505,505,504,503,503,502,501,501,500,499,499,498,498,497,496,496,495,494,494,493,
492,492,491,491,490,489,489,488,487,487,486,486,485,484,484,483,483,482,481,481,
480,480,479,478,478,477,477,476,476,475,474,474,473,473,472,472,471,471,470,469,
469,468,468,467,467,466,466,465,465,464,464,463,463,462,461,461,460,460,459,459,
458,458,457,457,456,456,455,455,454,454,453,453,452,452,451,451,450,450,450,449,
449,448,448,447,447,446,446,445,445,444,444,443,443,442,442,442,441,441,440,440,
439,439,438,438,438,437,437,436,436,435,435,434,434,434,433,433,432,432,431,431,
431,430,430,429,429,429,428,428,427,427,426,426,426,425,425,424,424,424,423,423,
422,422,422,421,421,420,420,420,419,419,418,418,418,417,417,417,416,416,415,415,
415,414,414,414,413,413,412,412,412,411,411,411,410,410,409,409,409,408,408,408,
407,407,407,406,406,406,405,405,404,404,404,403,403,403,402,402,402,401,401,401,
400,400,400,399,399,399,398,398,398,397,397,397,396,396,396,395,395,395,394,394,
394,393,393,393,392,392,392,391,391,391,391,390,390,390,389,389,389,388,388,388,
387,387,387,387,386,386,386,385,385,385,384,384,384,383,383,383,383,382,382,382,
381,381,381,381,380,380,380,379,379,379,379,378,378,378,377,377,377,377,376,376,
376,375,375,375,375,374,374,374,373,373,373,373,372,372,372,372,371,371,371,370,
370,370,370,369,369,369,369,368,368,368,368,367,367,367,366,366,366,366,365,365,
365,365,364,364,364,364,363,363,363,363,362,362,362,362,361,361,361,361,360,360,
360,360,359,359,359,359,358,358,358,358,357,357,357,357,356,356,356,356,356,355,
355,355,355,354,354,354,354,353,353,353,353,352,352,352,352,352,351,351,351,351,
350,350,350,350,349,349,349,349,349,348,348,348,348,347,347,347,347,347,346,346,
346,346,346,345,345,345,345,344,344,344,344,344,343,343,343,343,342,342,342,342,
342,341,341,341,341,341,340,340,340,340,340,339,339,339,339,339,338,338,338,338,
337,337,337,337,337,336,336,336,336,336,335,335,335,335,335,334,334,334,334,334,
333,333,333,333,333,333,332,332,332,332,332,331,331,331,331,331,330,330,330,330,
330,329,329,329,329,329,328,328,328,328,328,328,327,327,327,327,327,326,326,326,
326,326,326,325,325,325,325,325,324,324,324,324,324,324,323,323,323,323,323,322,
322,322,322,322,322,321,321,321,321,321,321,320,320,320,320,320,319,319,319,319,
319,319,318,318,318,318,318,318,317,317,317,317,317,317,316,316,316,316,316,316,
315,315,315,315,315,315,314,314,314,314,314,314,313,313,313,313,313,313,312,312,
312,312,312,312,312,311,311,311,311,311,311,310,310,310,310,310,310,309,309,309,
309,309,309,308,308,308,308,308,308,308,307,307,307,307,307,307,306,306,306,306,
306,306,306,305,305,305,305,305,305,305,304,304,304,304,304,304,303,303,303,303,
303,303,303,302,302,302,302,302,302,302,301,301,301,301,301,301,301,300,300,300,
300,300,300,300,299,299,299,299,299,299,299,298,298,298,298,298,298,298,297,297,
297,297,297,297,297,296,296,296,296,296,296,296,296,295,295,295,295,295,295,295,
294,294,294,294,294,294,294,293,293,293,293,293,293,293,293,292,292,292,292,292,
292,292,291,291,291,291,291,291,291,291,290,290,290,290,290,290,290,290,289,289,
289,289,289,289,289,289,288,288,288,288,288,288,288,288,287,287,287,287,287,287,
287,286,286,286,286,286,286,286,286,286,285,285,285,285,285,285,285,285,284,284,
284,284,284,284,284,284,283,283,283,283,283,283,283,283,282,282,282,282,282,282,
282,282,282,281,281,281,281,281,281,281,281,280,280,280,280,280,280,280,280,280,
279,279,279,279,279,279,279,279,278,278,278,278,278,278,278,278,278,277,277,277,
277,277,277,277,277,277,276,276,276,276,276,276,276,276,276,275,275,275,275,275,
275,275,275,275,274,274,274,274,274,274,274,274,274,273,273,273,273,273,273,273,
273,273,272,272,272,272,272,272,272,272,272,272,271,271,271,271,271,271,271,271,
271,270,270,270,270,270,270,270,270,270,270,269,269,269,269,269,269,269,269,269,
269,268,268,268,268,268,268,268,268,268,267,267,267,267,267,267,267,267,267,267,
266,266,266,266,266,266,266,266,266,266,265,265,265,265,265,265,265,265,265,265,
264,264,264,264,264,264,264,264,264,264,264,263,263,263,263,263,263,263,263,263,
263,262,262,262,262,262,262,262,262,262,262,261,261,261,261,261,261,261,261,261,
261,261,260,260,260,260,260,260,260,260,260,260,260,259,259,259,259,259,259,259,
259,259,259,259,258,258,258,258,258,258,258,258,258,258,258,257,257,257,257,257,
257,257,257,257,257,257,256,256,256,256,256,256,256,256,256,256,256,255,255,255,
255,255,255,255,255,255,255,255,254,254,254,254,254,254,254,254,254,254,254,254,
253,253,253,253,253,253,253,253,253,253,253,252,252,252,252,252,252,252,252,252,
252,252,252,251,251,251,251,251,251,251,251,251,251,251,251,250,250,250,250,250,
250,250,250,250,250};

//XY轴位置
double pos[N_AXIS];
//圆心偏移
double pos_rel[N_AXIS];

//辅助中间变量
s32 Xe,Ye,L,F,L2;

//当前XY坐标
s32 XC,YC; 
//目标XY坐标
s32 XT,YT;
//设置速度 
s16 v = 700; 

//设置定时器处初值
u16 Tnum = 2499;
// 脉冲数当量 pul/mm
s16 stepnum = 200;

 
//定义XY运动标志位
MOVE_type_enum XYmove;

//设置中断标志位
u8 timb=0;
//加速计数位标志
u16 char_ct=0;

void Interpolation_circle_ccw(void);
void Interpolation_circle_cw(void);
void Interpolation_linear(void);
u8 Quadrant_judge(void);
//圆插补
void CNC_circle(Circle_param_struct *param, Circle_type_enum cir_type);
//直线插补
void CNC_line(double target_x, double target_y);

void Y_pos_move(void);
void Y_neg_move(void);
void X_pos_move(void);
void X_neg_move(void);



int main(void)
{
    Circle_param_struct para ={0};

	delay_init(); 
    SM_port_init();
 	//设置NVIC中断分组2:2位抢占优先级，2位响应优先级
 	NVIC_Configuration();
    
	TIM3_Int_Init(2499,35);

    #if 0
    //执行一次直线插补运动
    pos[X_AXIS] = 2.0;
    pos[Y_AXIS] = 4.0;
    CNC_line(pos[X_AXIS],pos[Y_AXIS]);
    #endif

    //目标XY
    para.xc = -1.0;
    para.yc = 1.0;
    //圆心偏移
    para.off_xc = -1.0;
    para.off_yc = 0.0;

    CNC_circle(&para,Circle_CCW);

    
    while(1);

    
		 
 }


//逆时针圆插补
void Interpolation_circle_ccw(void)
{			
    s32 RX=0; //定义圆心X坐标 
    s32 RY=0; //定义圆心Y坐标 
    s32 RXC=0;//以圆心为原点建立坐标系并重新计算坐标值   起点坐标X
    s32 RYC=0;//以圆心为原点建立坐标系并重新计算坐标值  起点坐标Y
    XT=(pos[0]*stepnum);
    YT=(pos[1]*stepnum);	 
    RX=(pos_rel[0]*stepnum)+XC;
    RY=(pos_rel[1]*stepnum)+YC;
    RXC=XC-RX;
    RYC=YC-RY;

    Xe=XT-XC;
    Ye=YT-YC;

    //计算步数
    L=abs(Xe)+abs(Ye);
    L2=(L>>1);
    F=0;					

    if(RXC==0)
    {
        if(RYC==0)
        {
        }
        else if(RYC>0)
        {
            //第二
            while(L!=0)
            {
                if(F>=0)
                {
                    Y_neg_move();
                    F=F-2*abs(RYC)+1;
                    RYC=abs(RYC)-1;
                }
                else 
                {
                    X_neg_move();
                    F=F+2*abs(RXC)+1;
                    RXC=abs(RXC)+1;
                }
                
                L--;
            }
        }
        else 
        {
            //第四
            while(L!=0)
            {
                if(F>=0)
                {
                    Y_pos_move();
                    F=F-2*abs(RYC)+1;
                    RYC=abs(RYC)-1;
                }
                else 
                {
                    X_pos_move();
                    F=F+2*abs(RXC)+1;
                    RXC=abs(RXC)+1;
                }
                
                L--;
            }
        }		
    }
    else if(RXC>0)
    {
        if(RYC==0)
        {
            //第一
            while(L!=0)
            {
                if(F>=0)
                {
                    X_neg_move();
                    F=F-2*RXC+1;
                    RXC--;
                }
                else 
                {
                    Y_pos_move();
                    F=F+2*RYC+1;
                    RYC++;
                }
                
                L--;
            }
        }
        else if(RYC>0)
        {
            //第一
            while(L!=0)
            {
                if(F>=0)
                {
                    X_neg_move();
                    F=F-2*RXC+1;
                    RXC--;
                }
                else 
                {
                    Y_pos_move();
                    F=F+2*RYC+1;
                    RYC++;
                }
                
                L--;
            }
        }
        else
        {
            //第四
            while(L!=0)
            {
                if(F>=0)
                {
                    Y_pos_move();
                    F=F-2*abs(RYC)+1;
                    RYC=abs(RYC)-1;
                }
                else 
                {
                    X_pos_move();
                    F=F+2*abs(RXC)+1;
                    RXC=abs(RXC)+1;
                }
                L--;
            }
        }
    }
    else if(RYC==0)
    {
        //第三
        while(L!=0)
        {
            if(F>=0)
            {
                X_pos_move();
                F=F-2*abs(RXC)+1;
                RXC=abs(RXC)-1;
            }
            else 
            {
                Y_neg_move();
                F=F+2*abs(RYC)+1;
                RYC=abs(RYC)+1;
            }
            L--;
        }
    }
    else if(RYC>0)
    {
        //第二
        while(L!=0)
        {
            if(F>=0)
            {
                Y_neg_move();
                F=F-2*abs(RYC)+1;
                RYC=abs(RYC)-1;
            }
            else 
            {
                X_neg_move();
                F=F+2*abs(RXC)+1;
                RXC=abs(RXC)+1;
            }
            L--;
        }
    }
    else
    {
        //第三
        while(L!=0)
        {
            if(F>=0)
            {
                X_pos_move();
                F=F-2*abs(RXC)+1;
                RXC=abs(RXC)-1;
            }
            else 
            {
                Y_neg_move();
                F=F+2*abs(RYC)+1;
                RYC=abs(RYC)+1;
            }
            L--;
        }
    }

}

//顺时针圆插补
void Interpolation_circle_cw(void) 
{	
    s32 RX=0; 
    s32 RY=0;
    s32 RXC=0;
    s32 RYC=0;
    s32 RXT=0;
    s32 RYT=0;
    
    XT=(pos[0]*stepnum);
    YT=(pos[1]*stepnum);	 
    RX=(pos_rel[0]*stepnum)+XC;
    RY=(pos_rel[1]*stepnum)+YC;
    RXC=XC-RX;
    RYC=YC-RY;
    RXT=XT-RY;
    RYT=YT-RY;
    
    if ((RXC<0&&RXT>0)||(RYC<0&&RYT>0)||(RXC>0&&RXT<0)||(RYC>0&&RYT>0))
    {
        //计算圆弧半径 根据半径求交点
        //s32 YHR=0;
        //YHR=sqrt((RXC*RXC)+(RYC*RYC));

    }
    else 
    {
        Xe = XT - XC;
        Ye = YT - YC;

        //计算步数
        L=abs(Xe)+abs(Ye);
        L2=(L>>1);
        
        F=0;
        
        if(RXC==0)
        {
            if(RYC==0)
            {
            }
            else if(RYC>0)
            {
                //第一象限
                while(L!=0)
                {
                    if(F>=0)
                    {
                        Y_neg_move();													
                        F=F-2*RYC+1;
                        RYC--;
                    }
                    else
                    {
                        X_pos_move();
                        F=F+2*RXC+1;
                        RXC++;
                    }
                    
                    L--;
                }
            }
            else 
            {
                while(L!=0)
                {           
                    //第三象限
                    if(F>=0)
                    {
                        Y_pos_move();
                        F=F-2*abs(RYC)+1;
                        RYC=abs(RYC)-1;																		
                    }
                    else
                    {
                        X_neg_move();
                        F=F+2*abs(RXC)+1;
                        RXC=abs(RXC)+1;
                    }
                    
                    L--;
                }				
            }		
        }
        else if(RXC>0)
        {
            if(RYC==0)
            {
                //第四象限
                while(L!=0)
                {
                    if(F>=0)
                    {
                        X_neg_move();
                        F=F-2*abs(RXC)+1;
                        RXC=abs(RXC)-1;
                    }
                    else
                    {
                        Y_neg_move();
                        F=F+2*abs(RYC)+1;
                        RYC=abs(RYC)+1;
                    }
                    
                    L--;
                }
            }
            else if(RYC>0)
            {
                //第一象限
                while(L!=0)
                {
                    if(F>=0)
                    {
                        Y_neg_move();													
                        F=F-2*RYC+1;
                        RYC--;
                    }
                    else
                    {
                        X_pos_move();
                        F=F+2*RXC+1;
                        RXC++;
                    }
                    
                    L--;
                }
            }
            else
            {
                //第四象限
                while(L!=0)
                {
                    if(F>=0)
                    {
                        X_neg_move();
                        F=F-2*abs(RXC)+1;
                        RXC=abs(RXC)-1;
                    }
                    else
                    {
                        Y_neg_move();
                        F=F+2*abs(RYC)+1;
                        RYC=abs(RYC)+1;
                    }
                    
                    L--;
                }
            }
        }
        else if(RYC==0)
        {
            //第二象限
            while(L!=0)
            {
                if(F>=0)
                {
                    X_pos_move();
                    F=F-2*abs(RXC)+1;
                    RXC=abs(RXC)-1;
                }
                else
                {
                    Y_pos_move();
                    F=F+2*abs(RYC)+1;
                    RYC=abs(RYC)+1;
                }
                
                L--;
            }
        }
        else if(RYC>0)
        {
            //第二象限
            while(L!=0)
            {
                if(F>=0)
                {
                    X_pos_move();
                    F=F-2*abs(RXC)+1;
                    RXC=abs(RXC)-1;
                }
                else
                {
                    Y_pos_move();
                    F=F+2*abs(RYC)+1;
                    RYC=abs(RYC)+1;
                }
                
                L--;
            }
        }
        else
        {
            //第三象限
            while(L!=0)
            {           //第三象限顺园插补
                if(F>=0)
                {
                    Y_pos_move();
                    F=F-2*abs(RYC)+1;
                    RYC=abs(RYC)-1;																		
                }
                else
                {
                    X_neg_move();
                    F=F+2*abs(RXC)+1;
                    RXC=abs(RXC)+1;
                }
                
                L--;
            }				
        }
    }
}



//TIM3中断
void TIM3_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
    {
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
        
        //判断线段长度是否符合速度条件
        if  (L2>v)
        {
            if(L>v)
            {
                char_ct++;
                Tnum=ACB_C[char_ct];
                if(char_ct>=v)
                {
                    char_ct=v;
                    Tnum=ACB_C[char_ct];
                }

            }
            else
            {
                char_ct--;
                Tnum=ACB_C[char_ct];
            }
        }
        else
        {
            Tnum=ACB_C[0];
        }

        //判断进行哪个轴的插补运动
        if(XYmove == X_MOVE)
        {
            PULSE_X = 1;
            delay_us(1);
            PULSE_X = 0;				
        }
        else if(XYmove == Y_MOVE)
        {
            PULSE_Y = 1;
            delay_us(1);
            PULSE_Y = 0;				
        }
        
        TIM3->ARR = Tnum;
        timb = 0;
    }
}


void CNC_circle(Circle_param_struct *param, Circle_type_enum cir_type)
{
    pos[X_AXIS] = param->xc;
    pos[Y_AXIS] = param->yc;

    pos_rel[X_AXIS] = param->off_xc;
    pos_rel[Y_AXIS] = param->off_yc;

    if(cir_type == Circle_CW)
    {
        Interpolation_circle_cw();
    }
    else
    {
        Interpolation_circle_ccw();    
    }
    
}

//直线插补
void CNC_line(double target_x, double target_y)
{	
    //计算目标位置
    XT = (target_x*stepnum);
    YT = (target_y*stepnum);
    
    Interpolation_linear();
}
 
 
 
 
//判断插补直线在哪个象限
u8 Quadrant_judge(void)
{	
    u8 R;
    
    Xe = XT - XC;
    Ye = YT - YC;
    
    if((Xe)>=0)
    {
        if((Ye)>=0)
        {
            R=1;		
        }
        else
        {
            R=4;
        }
    }
    else if(Ye>=0)
    {
        R=2;	
    }
    else
    {
        R=3;
    }

    
    return R;	
}

 
void Interpolation_linear(void)
{
    u8 k;
    k = Quadrant_judge();

    //移动距离
    L = abs(Xe) + abs(Ye);
    //L/2，移位运算快于除法
    L2=(L>>1);
    
    if(k == 1)
    {
        while(L != 0)
        {
            if(F >= 0)
            {
                X_pos_move();
                F = F - Ye;		
            }
            else
            {
                Y_pos_move();					
                F = F + Xe;		
            }
            
            L--;
        }
    }
    else if(k == 2)
    {
        while(L != 0)
        {
            if(F>=0)
            {
                X_neg_move();
                F = F - abs(Ye);		
            }
            else
            {
                Y_pos_move();
                F = F + abs(Xe);		
            }
            
            L--;
        }
    }
    else if(k == 3)
    {
        while(L != 0)
        {
            if(F >= 0)
            {
                X_neg_move();
                F = F - abs(Ye);		
            }
            else
            {
                Y_neg_move();
                F = F + abs(Xe);
            }
            
            L--;
        }
    }
    else if(k == 4)
    {
        while(L != 0)
        {
            if(F >= 0)
            {
                X_pos_move();
                F = F - abs(Ye);
            }
            else
            {
                Y_neg_move();
                F = F + abs(Xe);
            }
            
            L--;
        }
    }

}





void X_pos_move(void)
{
    XYmove = X_MOVE;
    DIR_X=1;
    timb=1;

    TIM_Cmd(TIM3, ENABLE); 
    while(timb==1);
    TIM_Cmd(TIM3, DISABLE); 
    
    XC++;
}


void X_neg_move(void)
{
    XYmove = X_MOVE;
    DIR_X=0;
    timb=1;

    TIM_Cmd(TIM3, ENABLE); 
    while(timb==1);
    TIM_Cmd(TIM3, DISABLE); 
    
    XC--;
}

void Y_pos_move(void)
{
    XYmove = Y_MOVE;
    DIR_Y=1;
    timb=1;

    TIM_Cmd(TIM3, ENABLE); 
    while(timb==1);

    TIM_Cmd(TIM3, DISABLE); 

    YC++;
}


void Y_neg_move(void)
{
    XYmove = Y_MOVE;
    DIR_Y=0;
    timb=1;

    TIM_Cmd(TIM3, ENABLE); 
    while(timb==1);
    TIM_Cmd(TIM3, DISABLE); 
    
    YC--;
}