processg9x.c

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

   #include "base.h"
/*   
	//用于G9X循环指令
	FP32	G9X_PosX=0,G9X_PosZ=0;
	FP32	G9X_PosU=0,G9X_PosW=0;
	FP32	G9X_PosR=0;
	BOOLEAN	G9X_XorU=FALSE;
	BOOLEAN G9X_ZorW=FALSE;


	G90Varible  G90;
	G92Varible  G92; 
	G94Varible  G94;
*/   
void ResetG90Para(INT8U Code)
{
    if(Code!=90)
    {
         G90.PosR=0;G90.PosX=0;G90.PosZ=0;
    }     
}

void ResetG92Para(INT8U Code)
{
    if(Code!=92)
    {
         G92.PosR=0;G92.PosX=0;G92.PosZ=0;
    }     
}

void ResetG94Para(INT8U Code)
{
    if(Code!=94)
    {
         G94.PosR=0;G94.PosX=0;G94.PosZ=0;
    }     
}

void ProcessG32(void)
{
	long	TempX;
	long 	LastX;
	FP32	Current_XPos,Current_ZPos;
	FP32	Temp_TargetX,Temp_TargetZ,Temp_Length;
	int 	sta1,sta2;
	FP32	Pos_KX,Pos_KZ;
	BOOLEAN b_Pause;
	if(b_XCode)      Target_PosX=Pos_X;
	else if(b_UCode) Target_PosX=Pos_U+Get_AbsPos(1);
	else             Target_PosX=Get_AbsPos(1);
	if(b_ZCode)      Target_PosZ=Pos_Z;
	else if(b_WCode) Target_PosZ=Pos_W+Get_AbsPos(3);
	else             Target_PosZ=Pos_Z;
	Current_XPos=Get_AbsPos(1);
	Current_ZPos=Get_AbsPos(3);
	Temp_TargetX=Target_PosX-Current_XPos;
	Temp_TargetZ=Target_PosZ-Current_ZPos;
	Temp_Length=sqrt(Temp_TargetX*Temp_TargetX+Temp_TargetZ*Temp_TargetZ);//斜边长度
			
	Pos_KX=fabs(Pos_G32*Temp_TargetX/Temp_Length);//螺距
	Pos_KZ=fabs(Pos_G32*Temp_TargetZ/Temp_Length);
	while(1)
	{
		if(read_bit(MAIN_HOME)==1)break;
 		if(read_bit(SCRAM_IO)==0)return;
	}
	while(1)
	{
		if(read_bit(MAIN_HOME)==0)break;
	}
	set_actual_pos(2,0);
	LastX=0;
	b_Pause=FALSE;
	
	while(1)
	{   
	    OSTimeDly(1);
 		if(g_Scram || read_bit(PAUSE_IO)==0 )return;
		get_actual_pos(2,&TempX);
		TempX=abs(TempX/4);
	 	if(TempX !=LastX)
	 	{
	 		if(Target_PosX>Current_XPos)
	 		{
	 			Temp_TargetX=Current_XPos+(FP32)TempX/g_Sysparam.MainCoderNum*Pos_KX;
	 			if(Temp_TargetX>Target_PosX)Temp_TargetX=Target_PosX;
	 		}
	 		else
	 		{
	 			Temp_TargetX=Current_XPos-(FP32)TempX/g_Sysparam.MainCoderNum*Pos_KX;
	 			if(Target_PosX>Temp_TargetX)Temp_TargetX=Target_PosX;
	 		}
	 		
	 		if(Target_PosZ>Current_ZPos)
	 		{
	 			Temp_TargetZ=Current_ZPos+(FP32)TempX/g_Sysparam.MainCoderNum*Pos_KZ;
	 			if(Temp_TargetZ>Target_PosZ)Temp_TargetZ=Target_PosZ;
	 		}
	 		else
	 		{
	 			Temp_TargetZ=Current_ZPos-(FP32)TempX/g_Sysparam.MainCoderNum*Pos_KZ;
	 			if(Target_PosZ>Temp_TargetZ)Temp_TargetZ=Target_PosZ;
	 		}
	 		//Set_LuoWenSpeed(2500);
	 		G01_AbsPos(Temp_TargetX,Temp_TargetZ);
	 		while(1)
	 		{
				get_status(1,&sta1);
				get_status(2,&sta2);
				if(sta1==0 && sta2==0)break;
	 		}
	 		LastX=TempX;
	 		if(Temp_TargetX==Target_PosX && Temp_TargetZ==Target_PosZ)break;
	 	}
	}

}

BOOLEAN ProcessG90(void)
{
	FP32	Current_XPos,Current_ZPos;
	INT8U   ret;
	
	Current_XPos=Get_AbsPos(1);
	Current_ZPos=Get_AbsPos(3);
	
    if(b_XCode)		 G90.PosX=Pos_X;
	else if(b_UCode) G90.PosX=Pos_U+Current_XPos;
	
	if(b_ZCode)		 G90.PosZ=Pos_Z;
	else if(b_WCode) G90.PosZ=Pos_W+Current_ZPos;
	
	if(b_RCode)G90.PosR=Pos_R;
	

	Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
	Target_PosX=G90.PosX;
    Target_PosX=Target_PosX+G90.PosR;
	while(1)
	{
		Move_AbsPos(1,Target_PosX);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	g_Current_ASpeed=g_Current_PSpeed*g_Current_Inp_Rate;
	if(g_Current_ASpeed>g_Sysparam.Max_FastSpeed)
		g_Current_ASpeed=g_Sysparam.Max_FastSpeed;
	Set_InpSpeed(g_Current_ASpeed);
	
	Target_PosX=G90.PosX;
    Target_PosZ=G90.PosZ;
	
	bRunInp=TRUE;
	while(1)
	{
		G01_AbsPos(Target_PosX,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	Target_PosX=Current_XPos;
	Target_PosZ=Get_AbsPos(3);
	bRunInp=TRUE;
	while(1)
	{
		G01_AbsPos(Target_PosX,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	Target_PosZ=Current_ZPos;
	Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
	bRunInp=FALSE;
	while(1)
	{
		Move_AbsPos(3,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	return FALSE;
}

BOOLEAN ProcessG92(void)		//螺纹切削循环
{
	FP32	Current_XPos,Current_ZPos;
	BOOLEAN b_Pause;
	INT8U	ret;
	
	if(b_XCode){G9X_PosX=Pos_X;G9X_XorU=TRUE;}
	if(b_UCode){G9X_PosU=Pos_U;G9X_XorU=FALSE;}
	
	if(b_ZCode){G9X_PosZ=Pos_Z;G9X_ZorW=TRUE;}
	if(b_WCode){G9X_PosW=Pos_W;G9X_ZorW=FALSE;}
	if(b_RCode)G9X_PosR=Pos_R;

	Current_XPos=Get_AbsPos(1);
	Current_ZPos=Get_AbsPos(3);
	b_Pause=FALSE;
	
	//快速进给到X轴位置
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
	if(b_XCode)		 Target_PosX=G9X_PosX;
	else if(b_UCode) Target_PosX=G9X_PosU+Current_XPos;
	else
	{
		if(G9X_XorU) Target_PosX=G9X_PosX;
		else         Target_PosX=G9X_PosU+Current_XPos;
	}
	Target_PosX=Target_PosX+G9X_PosR;
	
	while(1)
	{
		Move_AbsPos(1,Target_PosX);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	if(g_Scram)return TRUE;
	ProcessG32();
	if(g_Scram)return TRUE;
	Target_PosX=Current_XPos;
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
	
	while(1)
	{
		Move_AbsPos(1,Target_PosX);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	//Z轴快速回Z起点
	if(g_Scram)return TRUE;
	Target_PosZ=Current_ZPos;
	Set_SipSpeed(3,g_Sysparam.Z_FastSpeed);
	while(1)
	{
		Move_AbsPos(3,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	return FALSE;
}

BOOLEAN ProcessG94(void)
{
	FP32	Current_XPos,Current_ZPos;
	INT8U	ret;
	
	Current_XPos=Get_AbsPos(1);
	Current_ZPos=Get_AbsPos(3);
    if(b_XCode)		 G94.PosX=Pos_X;
	else if(b_UCode) G94.PosX=Pos_U+Current_XPos;
	
	if(b_ZCode)		 G94.PosZ=Pos_Z;
	else if(b_WCode) G94.PosZ=Pos_W+Current_ZPos;
	
	if(b_RCode)G94.PosR=Pos_R;
	
	
	Set_SipSpeed(3,g_Current_Fast_Rate*g_Sysparam.Z_FastSpeed);
	Target_PosZ=G94.PosZ;
	Target_PosZ=Target_PosZ+G94.PosR;
	
	while(1){
		Move_AbsPos(3,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	g_Current_ASpeed=g_Current_PSpeed*g_Current_Inp_Rate;
	if(g_Current_ASpeed>g_Sysparam.Max_FastSpeed)
		g_Current_ASpeed=g_Sysparam.Max_FastSpeed;
	Set_InpSpeed(g_Current_ASpeed);
	
	Target_PosZ=G94.PosZ;
	Target_PosX=G94.PosX;
	bRunInp=TRUE;
	while(1){
		G01_AbsPos(Target_PosX,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	if(g_Scram)return TRUE;
	Target_PosZ=Current_ZPos;
	Target_PosX=Get_AbsPos(1);
	bRunInp=TRUE;
	while(1){
		G01_AbsPos(Target_PosX,Target_PosZ);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	
	Target_PosX=Current_XPos;
	Set_SipSpeed(1,g_Current_Fast_Rate*g_Sysparam.X_FastSpeed);
	bRunInp=FALSE;
	while(1){
		Move_AbsPos(1,Target_PosX);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return TRUE;
		if(ret==0)break;
	}
	return FALSE;
}


BOOLEAN ProcessG92_1(FP32 Xpos,FP32 Zpos,FP32 DaoCheng)
{
	FP32	CurrentXPos,CurrentZPos;
	INT8U	ret;

    if(DaoCheng<=0)return FALSE;
	CurrentXPos=Get_AbsPos(1);
	CurrentZPos=Get_AbsPos(3);
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
	//运行到指定的X位置
	while(1){
		Move_AbsPos(1,Xpos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//横向加工螺纹
	if(g_Scram)return FALSE;
	Process(Xpos,Zpos,DaoCheng,0);
	//X轴快速回X起点
	if(g_Scram)return FALSE;
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
    while(1){
		Move_AbsPos(1,CurrentXPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//Z轴快速回Z起点
	if(g_Scram)return FALSE;
	Set_SipSpeed(3,g_Sysparam.Z_FastSpeed);
	while(1){
		Move_AbsPos(3,CurrentZPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	return TRUE;
}


BOOLEAN ProcessG92_2(FP32 Xpos,FP32 Zpos,FP32 DaoCheng,FP32 R)
{
	FP32	CurrentXPos,CurrentZPos;
	INT8U	ret;

    if(DaoCheng<=0)return FALSE;
	CurrentXPos=Get_AbsPos(1);
	CurrentZPos=Get_AbsPos(3);
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
	//运行到指定的X位置
	while(1){
		Move_AbsPos(1,Xpos+R);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//横向加工螺纹
	if(g_Scram)return FALSE;
	Process(Xpos,Zpos,DaoCheng,0);
	//X轴快速回X起点
	if(g_Scram)return FALSE;
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
    while(1){
		Move_AbsPos(1,CurrentXPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//Z轴快速回Z起点
	if(g_Scram)return FALSE;
	Set_SipSpeed(3,g_Sysparam.Z_FastSpeed);
	while(1){
		Move_AbsPos(3,CurrentZPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	return TRUE;
}


BOOLEAN ProcessG92_3(FP32 Xpos,FP32 Zpos,FP32 DaoCheng,INT16U DelayAngle)
{
	FP32	CurrentXPos,CurrentZPos;
	INT8U	ret;

    if(DaoCheng<=0)return FALSE;
    
	CurrentXPos=Get_AbsPos(1);
	CurrentZPos=Get_AbsPos(3);
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
	//运行到指定的X位置
	while(1){
		Move_AbsPos(1,Xpos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//横向加工螺纹
	if(g_Scram)return FALSE;
	Process(Xpos,Zpos,DaoCheng,DelayAngle);
	//X轴快速回X起点
	if(g_Scram)return FALSE;
	
	Set_SipSpeed(1,g_Sysparam.X_FastSpeed);
    while(1){
		Move_AbsPos(1,CurrentXPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	
	//Z轴快速回Z起点
	if(g_Scram)return FALSE;
	Set_SipSpeed(3,g_Sysparam.Z_FastSpeed);
	while(1){
		Move_AbsPos(3,CurrentZPos);
		g_MoveMode=1;
		ret=WaitMotionEnd();
		if(ret==255)return FALSE;
		if(ret==0)break;
	}
	return TRUE;
}

BOOLEAN ProcessG92_4(FP32 Xpos,FP32 Zpos,FP32 DaoCheng,INT16U LoopCount)
{
     INT16U  StartAngle=0;
     INT16U  PerAngle;
     
     if(DaoCheng<=0)return FALSE;
     
     PerAngle=360/LoopCount;
     while(1){
           if(ProcessG92_3(Xpos,Zpos,DaoCheng,StartAngle)==0)return FALSE; 
           StartAngle=StartAngle+PerAngle;
           if(StartAngle>=360)break;
           if(g_Scram)return FALSE;
     }
     return TRUE;   
}

FP32 ChangeValue(INT32U Value)
{
/*       
   if(Value>=200 || Value<=231)
   {
       return  g_Sysparam.Public[Value-200];       
   }
   else
   */
   {
       return 0xfffff;
   }      
}