spmc75_main.c

基于凌阳单片机spmc75f2413的步进电机3轴联动空间圆弧插补源程序

/* ========================================================================= */
/*                毕业设计题目：基于单片机的多轴联动控制系统的设计           */
/*                                                                           */
/* File Name   : main.c	                                                     */	
/* Author      : 孙英良                                                      */	
/* ========================================================================= */
#define tm 0.00625
#define LV 2095
#include "unspmacro.h"      /* Description : inline assembly macro definition*/
#include "spmc75f2413a.h"  /* Description : SPMC75F2413A register definition */	
#include "math.h"
unsigned int TIMERFLAG=0,MFLAG=0,allstep=0,step=0;
unsigned int s[8]={25,61,118,206,333,491,670,860};
unsigned int TIMERDATA[8]={2012,1830,1697,1508,1380,1116,960,750};//TIMERDATA[8]为速度表
                                                             //即计数器的设定值
void main()
{  
   float xs=10.0;                    //(xs,ys,zs)为起始点坐标
   float ys=0;
   float zs=0;
   float xm=5*sqrtf(2);              //(xm,ym,zm)为在圆弧上任一点的坐标
   float ym=5;
   float zm=5;
   float xe=0;                     //(xe,ye,ze)为终点坐标
   float ye=5*sqrtf(2);
   float ze=5*sqrtf(2);
   float Jrx,Jry,Jrz,Jx,Jy,Jz,R1,R2; //Jx,Jy,Jz分别为X、Y、Z轴被积函数值
                                     //Jrx,Jry,Jrz为累加余数值
                                     //R1为空间圆弧半径值
   int xi=0,yi=0,zi=0;
   int xcwi=0,ycwi=0,zcwi=0,i=0,xmflag=0,ymflag=0,zmflag=0;
   int xcwflag=1,ycwflag=1,zcwflag=1,ctab,yctab,zctab;
   float Ii=0,Ji=0,Ki=0,xo1,yo1,zo1,xn,yn1,zn;
   float xsm,ysm,zsm,xme,yme,zme,dae;
   float xoe,yoe,zoe;
   float xa,ya,za,dx,dy,dz,xp,yp,zp,xq,yq,zq;
   float A11,A12,A13,A21,A22,A23,A31,A32,A33,b1,b2,b3,D_A;
   float XN,YN1,ZN,D_N;
   float fa_xse,fa_yse,fa_zse,max_axis,J,sa=2.7;

//cwtab[]为步进电机1/8微步距正相运行控制数据表 ccwtab[]反相数据表//
   unsigned int cwtab[32]={0xf900,0xcd00,0xf500,0x5500,0x7500,0x4d00,0x7900,
   0x4300,0x7600,0x4e00,0x7a00,0x2a00,0x3a00,0x0e00,0x3600,0x0300,0x3900,
   0x0d00,0x3500,0x9500,0xb500,0x8d00,0xb900,0x8300,0xb600,0x8e00,0xba00,
   0xea00,0xfa00,0xce00,0xf600,0xc300};
   unsigned int ccwtab[32]={0xf600,0xce00,0xfa00,0xea00,0xba00,0x8e00,0xb600,0x8300,
   0xb900,0x8d00,0xb500,0x9500,0x3500,0x0d00,0x3900,0x0300,0x3600,0x0e00,0x3a00,
   0x6a00,0x7a00,0x4e00,0x7600,0x4300,0x7900,0x4d00,0x7500,0xd500,0xf500,0xcd00,
   0xf900,0xc300};


//IO initialize  IOA0,IOD15,IOD14,IOB15,IOB14,IOB13设置为输出模式
   P_IOC_Dir->W=0xFFFF;    
   P_IOC_Attrib->W=0xFFFF;
   P_IOC_Data->W=0x0000;
   P_IOC_SPE->W=0x0000;
   P_IOB_Dir->W=0xFFFF;
   P_IOB_Attrib->W=0xFFFF;
   P_IOB_Data->W=0xE000;
   P_IOB_SPE->W=0x0000;
   P_IOD_Dir->W=0xFFFF;
   P_IOD_Attrib->W=0xFFFF;
   P_IOD_Data->W=0x0000;
//IO initialize  end
// TPM2设置为标准计数模式
   P_TMR2_Ctrl->W=0xe0e8;
   P_TMR2_INT->W=0x0010;        //定时器周期寄存器中断使能
   P_TMR2_TPR->W=LV;

   P_TMR_Start->W=0x0004;       //TPM2启动 
   xsm=xm-xs;                   //(xsm,ysm,zsm)为向量SM
   ysm=ym-ys; 
   zsm=zm-zs;
   xme=xe-xm;                   //(xme,yme,zme)为向量ME
   yme=ye-ym; 
   zme=ze-zm;
   xp=xs+(xm-xs)/2;             //(xp,yp,zp)为SM中点
   yp=ys+(ym-ys)/2;
   zp=zs+(zm-zs)/2;
   xq=xm+(xe-xm)/2;             //(xq,yq,zq)为SM中点
   yq=ym+(ye-ym)/2;
   zq=zm+(ze-zm)/2;
   XN=ysm*zme-yme*zsm;          //(XN,YN1,ZN)为向量N,其值为向量SM和向量ME的差乘
   YN1=xme*zsm-xsm*zme;
   ZN=xsm*yme-xme*ysm;
   D_N=sqrtf(XN*XN+YN1*YN1+ZN*ZN);
   xn=XN/D_N;
   yn1=YN1/D_N;
   zn=ZN/D_N;
   A11=yme*zn-zme*yn1;          //矩阵A代数余子式
   A12=xn*zme-xme*zn;
   A13=xme*yn1-xn*yme;
   A21=yn1*zsm-ysm*zn;
   A22=xsm*zn-xn*zsm;
   A23=xn*ysm-xsm*yn1;
   A31=ysm*zme-yme*zsm;
   A32=xme*zsm-xsm*zme;
   A33=xsm*yme-xme*ysm;
   D_A=xsm*yme*zn-xsm*yn1*zme-xme*ysm*zn+xn*ysm*zme+xme*yn1*zsm-xn*yme*zsm;//矩阵A行列式
   b1=xp*xsm+yp*ysm+zp*zsm;
   b2=xq*xme+yq*yme+zq*zme;
   b3=xs*xn+ys*yn1+zs*zn;
   xo1=(A11*b1+A21*b2+A31*b3)/D_A;           //(xo1,yo1,zo1)为空间圆弧的圆心坐标
   yo1=(A12*b1+A22*b2+A32*b3)/D_A;
   zo1=(A13*b1+A23*b2+A33*b3)/D_A;
   xoe=xe-xo1;
   yoe=ye-yo1;
   zoe=ze-zo1; 
   R2=xoe*xoe+yoe*yoe+zoe*zoe;
   R1=sqrtf(R2);                             //R1为空间圆弧的半径
   Jrx=R1/2;Jry=R1/2;Jrz=R1/2;
   xa=xs;ya=ys;za=zs;                        //空间圆弧上一插补动点
   INT_IRQ();
   Ii=xa-xo1;                                //(Ii,Ji,Ki)为向量O1A
   Ji=ya-yo1;
   Ki=za-zo1;
   
 
   fa_xse=fabsf(xe-xs);
   fa_yse=fabsf(ye-ys);
   fa_zse=fabsf(ze-zs);
   if(fa_xse>=fa_yse)max_axis=fa_xse;
   else if(max_axis<fa_zse)max_axis=fa_zse;  //求出运行步数最多的轴
    
   
   if(max_axis<2*sa)MFLAG=0;                 //若总距离长度为短距离则低速运行
   else
   {  allstep=max_axis/0.003125;       
      
   }  
rn:
   dx=(yn1*Ki-zn*Ji);                        //(dx,dy,dz)为动点A的方向向量
   Jx=fabsf(dx);
   Jrx=Jrx+Jx;
   if(Jrx>=R1){                              //正X进给将进给标志置1反向置-1，若无进给则为0
                    Jrx=Jrx-R1;
                    if(dx>0){xmflag=1;xa+=0.003125;Ii+=0.003125;xi++;}
                    else if(dx<0){xmflag=-1; xa-=0.003125;Ii-=0.003125;xi--;}
               }                   
   else xmflag=0;
   dy=(zn*Ii-xn*Ki);
   Jy=fabsf(dy);
   Jry=Jry+Jy;
   if(Jry>=R1){
   	            Jry=Jry-R1;
                    if(dy>0){ymflag=1;ya+=0.003125;Ji+=0.003125;yi++;}
                    else if(dy<0){ymflag=-1; ya-=0.003125;Ji-=0.003125;yi--;}
                   }                    
   else ymflag=0;
   dz=(xn*Ji-yn1*Ii);
   Jz=fabsf(dz);
   Jrz=Jrz+Jz;
   if(Jrz>=R1){
   	            Jrz=Jrz-R1;
                    if(dz>0){zmflag=1;za+=0.003125;Ki+=0.003125;zi++;}
                    else if(dz<0){zmflag=-1; za-=0.003125;Ki-=0.003125;zi--;}
                   }                    
   else zmflag=0;
   switch(xmflag)                                  //数据表中取值到临时变量
   {case 1:ctab=cwtab[xcwi];break;
    case -1:ctab=ccwtab[xcwi];break;
    case 0:ctab=0x0000;break;
   }
   P_IOC_Data->B.bit12=1;
   i=0;
   while(1){                                       //在四个下降沿将数据输入到DATA A,DATA B
                     i++;
                     P_IOC_Data->B.bit10=(ctab&0x8000);
                     ctab=ctab<<1;
                     P_IOD_Data->B.bit11=(ctab&0x8000); 
                     NOP();
                     NOP();
                     P_IOC_Data->B.bit12=0;
                     NOP();
                     NOP();
                     if(i>=4){i=0;break;}
            }
   if(xmflag==1){
                  if(xcwflag==0){    //如果本次进给为正相，而上一次进给为负向则调整数据表中取值顺序为xcwi--
                  	          xcwi--;
                  	          if(xcwi==-1)xcwflag=1;
                  	         }
                  else{ xcwi++;      //如果本次进给为正相，而上一次进给为正向则调整数据表中取值顺序xcwi++
                        xcwflag=1;
                        if(xcwi==31)xcwi=0;
                      }
               }
    else if(xmflag==-1){
                	 if(xcwflag==1){ //如果本次进给为负相，而上一次进给为正向则调整数据表中取值顺序为xcwi--
                            	          xcwi--;
                            	          if(xcwi==-1)xcwflag=0;
                            	         }
                         else{ xcwi++;    //如果本次进给为负相，而上一次进给为正向则调整数据表中取值顺序为xcwi++
                               xcwflag=0;
                               if(xcwi==31)xcwi=0;
                             }
                       }
   switch(ymflag)                         //数据表中取值到临时变量
   {case 1:yctab=cwtab[ycwi];break;
    case -1:yctab=ccwtab[ycwi];break;
    case 0:yctab=0x0000;break;
   }
   switch(zmflag)
   {case 1:zctab=cwtab[zcwi];break;
    case -1:zctab=ccwtab[zcwi];break;
    case 0:zctab=0x0000;break;
   }
   P_IOB_Data->B.bit5=1;
   i=0;
   while(1){
                     i++;
                     P_IOB_Data->B.bit1=(yctab&0x8000);
                     ctab=ctab<<1;
                     P_IOB_Data->B.bit0=(yctab&0x8000);
                     P_IOB_Data->B.bit4=(zctab&0x8000);
                     ctab=ctab<<1;
                     P_IOB_Data->B.bit3=(zctab&0x8000); 
                     NOP();
                     NOP();
                     P_IOC_Data->B.bit12=0;
                     NOP();
                     NOP();
                     if(i>=4){i=0;break;}
           }
   if(ymflag==1){
                  if(ycwflag==0){
                  	          ycwi--;
                  	          if(ycwi==-1)ycwflag=1;
                  	       }
                  else{ ycwi++;
                        ycwflag=1;
                        if(ycwi==31)ycwi=0;
                      }
               }
    else if(ymflag==-1){
                	 if(ycwflag==1){
                            	          ycwi--;
                            	          if(ycwi==-1)ycwflag=0;
                            	         }
                         else{ ycwi++;
                               ycwflag=0;
                               if(ycwi==31)ycwi=0;
                             }
                       }
   if(zmflag==1){
                  if(zcwflag==0){
                  	          zcwi--;
                  	          if(zcwi==-1)zcwflag=1;
                  	       }
                  else{ zcwi++;
                        zcwflag=1;
                        if(zcwi==31)zcwi=0;
                      }
               }
    else if(zmflag==-1){
                	 if(zcwflag==1){
                            	          zcwi--;
                            	          if(zcwi==-1)zcwflag=0;
                            	         }
                         else{ zcwi++;
                               zcwflag=0;
                               if(zcwi==31)zcwi=0;
                             }
                       }
     P_IOC_Data->B.bit12=1;        //strobe输出脉冲
     P_IOB_Data->B.bit2=1;
     NOP();
     NOP();
     NOP();
     P_IOC_Data->B.bit12=0;                                             
     P_IOB_Data->B.bit2=0;
     dae=sqrtf((xa-xe)*(xa-xe)+(ya-ye)*(ya-ye)+(za-ze)*(za-ze));   
     if(dae>=0.003125){                                         //终点判断
     	              while(1){        
     	             	      if(TIMERFLAG==1){TIMERFLAG=0;goto rn;}
     	             	     }
     	        } 
     else while(1){NOP(); }
}

/*****************************************************************************/
/* IRQ4() : IRQ4 interrup service routine, possibile int. sources are		 */
/* 			TPM2IF															 */
/*****************************************************************************/
void IRQ4(void)	__attribute__ ((ISR));
void IRQ4(void)
{
	TIMERFLAG=1;                   //中断标志置1
    if(MFLAG==1)                       //MFLAG=1表示曲线加减速工作模式
    {    
          step++;
                                       //在各个离散点设置计数器的值
          if(step==s[0])P_TMR2_TPR->W=TIMERDATA[0];
          else if(step==s[1])P_TMR2_TPR->W=TIMERDATA[1];
          else if(step==s[2])P_TMR2_TPR->W=TIMERDATA[2];
          else if(step==s[3])P_TMR2_TPR->W=TIMERDATA[3];
          else if(step==s[4])P_TMR2_TPR->W=TIMERDATA[4];
          else if(step==s[5])P_TMR2_TPR->W=TIMERDATA[5];
          else if(step==s[6])P_TMR2_TPR->W=TIMERDATA[6];
          else if(step==s[7])P_TMR2_TPR->W=TIMERDATA[7];
          else if(step==(allstep-s[7]))P_TMR2_TPR->W=TIMERDATA[6];
          else if(step==(allstep-s[6]))P_TMR2_TPR->W=TIMERDATA[5];
          else if(step==(allstep-s[5]))P_TMR2_TPR->W=TIMERDATA[4];
          else if(step==(allstep-s[4]))P_TMR2_TPR->W=TIMERDATA[3];
          else if(step==(allstep-s[3]))P_TMR2_TPR->W=TIMERDATA[2];
          else if(step==(allstep-s[2]))P_TMR2_TPR->W=TIMERDATA[1];
          else if(step==(allstep-s[1]))P_TMR2_TPR->W=TIMERDATA[0];
          else if(step==(allstep-s[0])){P_TMR2_TPR->W=LV;MFLAG=0;}
   }
}