processg65.c

控制电机运动的G代码程序,如果对G代码不清楚,请先看懂G代码,然后再看本程序

#include "base.h"


INT32U ProcessG65(G65Para *pG65Para,INT8U *Err,INT8U *Jmp)
{
       
       INT8U  Bit;
       
       switch(pG65Para->H)
       {
          case  1:
                if(pG65Para->P_Change && pG65Para->Q_Change)
                {
                     if(pG65Para->P>=1100 && pG65Para->P<=1107)
                     {
                          write_bit(ChangeOut(pG65Para->P-1100),Bit=g_Sysparam.Public[pG65Para->Q-200]>0?1:0);
                          *Err=0;*Jmp=0;
                     }else
                     {
                          g_Sysparam.Public[pG65Para->P-200]=g_Sysparam.Public[pG65Para->Q-200];
                          *Err=0;*Jmp=0;
                     }     
                }
                else if(pG65Para->P_Change && !pG65Para->Q_Change)
                {
                     if(pG65Para->P>=1100 && pG65Para->P<=1107)
                     {
                          write_bit(ChangeOut(pG65Para->P-1100),Bit=pG65Para->Q>0?1:0);
                          *Err=0;*Jmp=0;
                          break;
                     }
                     g_Sysparam.Public[pG65Para->P-200]=pG65Para->Q;
                     *Err=0;*Jmp=0;
                }
                else
                {
                     *Err=1;*Jmp=0; 
                     DispErrCode(30);              
                }
                break;
          case  2:
                if(pG65Para->P_Change && pG65Para->Q_Change && pG65Para->R_Change)
                {
                      g_Sysparam.Public[pG65Para->P-200]=g_Sysparam.Public[pG65Para->Q-200]+g_Sysparam.Public[pG65Para->R-200];
                      *Err=0;*Jmp=0;
                }else if(pG65Para->P_Change && !pG65Para->Q_Change && pG65Para->R_Change)
                {
                      g_Sysparam.Public[pG65Para->P-200]=pG65Para->Q+g_Sysparam.Public[pG65Para->R-200];
                      *Err=0;*Jmp=0; 
                }else if(pG65Para->P_Change && pG65Para->Q_Change && !pG65Para->R_Change)
                {
                      g_Sysparam.Public[pG65Para->P-200]=g_Sysparam.Public[pG65Para->Q-200]+pG65Para->R;
                      *Err=0;*Jmp=0;  
                } 
                else 
                {
                      *Err=1;*Jmp=0; 
                      DispErrCode(31); 
                }    
                break;
          case  3:
                if(pG65Para->P_Change && pG65Para->Q_Change && pG65Para->R_Change)
                {
                     g_Sysparam.Public[pG65Para->P-200]=g_Sysparam.Public[pG65Para->Q-200]-g_Sysparam.Public[pG65Para->R-200];
                     *Err=0;*Jmp=0;
                }else if(pG65Para->P_Change && pG65Para->Q_Change && !pG65Para->R_Change)
                {
                     g_Sysparam.Public[pG65Para->P-200]=g_Sysparam.Public[pG65Para->Q-200]-pG65Para->R;
                     *Err=0;*Jmp=0; 
                }else if(pG65Para->P_Change && !pG65Para->Q_Change && pG65Para->R_Change)
                {
                     g_Sysparam.Public[pG65Para->P-200]=pG65Para->Q-g_Sysparam.Public[pG65Para->R-200];
                     *Err=0;*Jmp=0;  
                } 
                else 
                {
                     *Err=1;*Jmp=0;
                     DispErrCode(32);
                }   
                break;
          case  80:
                if(pG65Para->P_Change)
                {
                    *Err=0;*Jmp=1;return FindHang(g_Sysparam.Public[pG65Para->P-200]);
                }
                else
                {
                    *Err=0;*Jmp=1;return FindHang(pG65Para->P);
                }   
                break;
          case  81:
                if(!pG65Para->P_Change)
                {
                     if(pG65Para->Q_Change && pG65Para->R_Change)
                     {
                          if(g_Sysparam.Public[pG65Para->Q-200]==g_Sysparam.Public[pG65Para->R-200])
                          {
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(pG65Para->Q_Change && !pG65Para->R_Change)
                     {
                          if(g_Sysparam.Public[pG65Para->Q-200]==pG65Para->R)
                          {
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(!pG65Para->Q_Change && pG65Para->R_Change)
                     {
                          if(pG65Para->Q==g_Sysparam.Public[pG65Para->R-200])
                          {
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     } 
                }else
                {
                     *Err=1;*Jmp=0;
                     DispErrCode(33);
                }   
                break;
          case  82:
                if(!pG65Para->P_Change)
                {
                     if(pG65Para->Q_Change && pG65Para->R_Change)
                     {
                          if(g_Sysparam.Public[pG65Para->Q-200]!=g_Sysparam.Public[pG65Para->R-200])
                          {
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else
                          {
                          	*Err=0;*Jmp=0;
                          }
                     }
                     else if(pG65Para->Q_Change && !pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]!=pG65Para->R){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(!pG65Para->Q_Change && pG65Para->R_Change){
                          if(pG65Para->Q!=g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     } 
                }else{
                     *Err=1;*Jmp=0;
                     DispErrCode(33);
                }   
                break;
          case  83:
                if(!pG65Para->P_Change){
                     if(pG65Para->Q_Change && pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]>g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(pG65Para->Q_Change && !pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]>pG65Para->R){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(!pG65Para->Q_Change && pG65Para->R_Change){
                          if(pG65Para->Q>g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     } 
                }else{
                     *Err=1;*Jmp=0;
                     DispErrCode(33);
                }   
                break; 
          case  84:
                if(!pG65Para->P_Change){
                     if(pG65Para->Q_Change && pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]<g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(pG65Para->Q_Change && !pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]<pG65Para->R){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(!pG65Para->Q_Change && pG65Para->R_Change){
                          if(pG65Para->Q<g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     } 
                }else{
                     *Err=1;*Jmp=0;
                     DispErrCode(33);
                }   
                break;
          case  85:
                if(!pG65Para->P_Change){
                     if(pG65Para->Q_Change && pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]>=g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(pG65Para->Q_Change && !pG65Para->R_Change){
                          if(g_Sysparam.Public[pG65Para->Q-200]>=pG65Para->R){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     }
                     else if(!pG65Para->Q_Change && pG65Para->R_Change){
                          if(pG65Para->Q>=g_Sysparam.Public[pG65Para->R-200]){
                               *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                          }
                          else{*Err=0;*Jmp=0;}
                     } 
                }else{
                     *Err=1;*Jmp=0;
                     DispErrCode(33);
                }     
                break; 
          case  86:
                if(!pG65Para->P_Change){
                    if(pG65Para->Q_Change && pG65Para->R_Change){
                         if(g_Sysparam.Public[pG65Para->Q-200]<=g_Sysparam.Public[pG65Para->R-200]){
                             *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                         }
                         else{*Err=0;*Jmp=0;}
                    }
                    else if(pG65Para->Q_Change && !pG65Para->R_Change){
                         if(g_Sysparam.Public[pG65Para->Q-200]<=pG65Para->R){
                             *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                         }
                         else{*Err=0;*Jmp=0;}
                    }
                    else if(!pG65Para->Q_Change && pG65Para->R_Change){
                         if(pG65Para->Q<=g_Sysparam.Public[pG65Para->R-200]){
                             *Err=0;*Jmp=1;return FindHang(pG65Para->P);    
                         }
                         else{*Err=0;*Jmp=0;}
                    }
                }else{
                    *Err=1;*Jmp=0;
                    DispErrCode(33);
                } 
                break; 
          case  99:  
                DispErrCode(48);*Err=0;*Jmp=0;  
                break; 
          default:
                *Err=1;*Jmp=0;
                DispErrCode(34);
                break;          
       }
       return 0;
}

void ResetG65Para(void)
{
    ppG65Para.Q=0;
    ppG65Para.R=0;
    ppG65Para.H=0;
    ppG65Para.P=0;
    ppG65Para.Q_Change=0;
    ppG65Para.R_Change=0;
    ppG65Para.P_Change=0;
    ppG65Para.H_Change=0;
}

INT32U FindHang(INT32U Hang)
{

	INT32U     i;
	INT8U      j,k,Err;
	INT8U      buf[10];
	INT32U     FindHang;
	INT32U     hang;

	i=g_DataBuf.ProgAdd[g_Usrparam.CurrentFileNum];//取当前文名

	while(1)
	{
		if (g_DataBuf.DataBuf[i]=='N')
		{
			hang=i;
			j=0;i++;Err=0;
			while(g_DataBuf.DataBuf[i]>='0'&&g_DataBuf.DataBuf[i]<='9')
			{
				buf[j]=g_DataBuf.DataBuf[i];
				j++;i++;
				if (i>=g_DataBuf.DataLength)return 0xfffff;
				if (j>6){Err=1;break;}
			}
			if (Err==0)
			{
				k=0;FindHang=0;
				while(j)
				{
					FindHang=(buf[k]-'0')+FindHang*10; //递归调用
					k++; 
					j--;    
				}
				if (Hang==FindHang)
				{
					return hang; 
				}    

			}
//			else{;}
		}
		else
		{
			i++;
			if (i>=g_DataBuf.DataLength)return 0xfffff;//大于文件长度返回出错
			if (g_DataBuf.DataBuf[i]=='%')			   //遇到结速符返回
			{
				return 0xfffff; 
			}
		}
	}
}

INT8U ChangeOut(INT8U Num)
{
      
   switch(Num)
   {
       case 0:  return 1;
       case 1:  return 5;
       case 2:  return 12;
       case 3:  return 9;
       case 4:  return 10;
       case 5:  return 11;
   }
   return 1;
}