specycle.cc

数控系统中的解释器源代码

#include "rs274ngc.hh"
#include "agl.hh"
#include "profdata.hh"
#include "canon.hh"
#include "rs274ngc_return.hh"
#define WORD unsigned short

int     ZBackDir[2][4] = {
			{1,1,1,1},
			{ 1,1, 1,-1}
			};


int     XBackDir[2][4] = {
			{ 1,1, 1,1},
			{-1,1,-1,-1}
			};

int  read_sequence_number(setup_pointer settings, Special_Cycle        *pSpeCycle)
{
   PointVar pos;
	 if (pSpeCycle->ParaAxisFlag == 1)
	{
	        pos.PosY=pSpeCycle->OrdValue  -pSpeCycle->CutStep;
	
	}
    else
	{
	       pos.PosX=pSpeCycle->AbsValue  -pSpeCycle->CutStep;
	
	
	//pMove->MoveAxis[0].EndPos = pSpeCycle->AbsValue;
	}
	while( 1 )
	{
	
	
	if (CNC_CheckPointInSegment(pSpeCycle,&pos) )
	    {
	     break;
	    }
	else
	{
	     if (pSpeCycle->BlockNum<=1)
		 	return NCE_PROF_POINT_INVIALD;
	    pSpeCycle->BlockNum --;
	    continue;
	}
	}
	return RS274NGC_OK;
}




/********************************************************************
 * Function:
 *  	This function checks if the relation between the tool
 *  	position and the profile's is reasonable.
 *  	and it return 1 if reasonable and 0 for illegal.
 *
 ********************************************************************/


BYTE  PToolPositionValid(Special_Cycle          *pSpeCycle,
				    float                  minvalue)
{
BYTE            ret = 1, block1;
// return(ret);
//    printf("direction is %d, Flag is %d\n",pSpeCycle->ProfDirection,pSpeCycle->ParaAxisFlag);
    block1 = pSpeCycle->BlockNum;
    block1 --;
    if (pSpeCycle->ParaAxisFlag == 1)
	switch(pSpeCycle->ProfDirection)
	    {
	    case 1:
		if ((pSpeCycle->AbsStartValue + minvalue)
			  > pSpeCycle->Block[0].Point.PosX ||
		    (pSpeCycle->OrdStartValue - minvalue)
			 < pSpeCycle->Block[block1].Point.PosY)
		    ret = 0;
		break;
	    case 2:
		if ((pSpeCycle->AbsStartValue - minvalue)
			 < pSpeCycle->Block[0].Point.PosX ||
		    (pSpeCycle->OrdStartValue - minvalue)
			 < pSpeCycle->Block[block1].Point.PosY)
		    ret = 0;
		break;
	    case 3:
		if ((pSpeCycle->AbsStartValue - minvalue)
			 < pSpeCycle->Block[0].Point.PosX ||
		    (pSpeCycle->OrdStartValue + minvalue)
			 > pSpeCycle->Block[block1].Point.PosY)
		    ret = 0;
		break;
	    case 4:
		 if ((pSpeCycle->AbsStartValue + minvalue)
			 > pSpeCycle->Block[0].Point.PosX ||
		     (pSpeCycle->OrdStartValue + minvalue)
			 > pSpeCycle->Block[block1].Point.PosY)
		    ret = 0;
		 break;
	}
    else
	switch(pSpeCycle->ProfDirection)
	    {
	    case 1:
		if ((pSpeCycle->AbsStartValue - minvalue)
			 < pSpeCycle->Block[block1].Point.PosX ||
		    (pSpeCycle->OrdStartValue + minvalue)
			 > pSpeCycle->Block[0].Point.PosY)
		    ret = 0;
		break;
	    case 2:
		if ((pSpeCycle->AbsStartValue + minvalue)
			 > pSpeCycle->Block[block1].Point.PosX ||
		    (pSpeCycle->OrdStartValue + minvalue)
			 > pSpeCycle->Block[0].Point.PosY)
		    ret = 0;
		break;
	    case 3:
		if ((pSpeCycle->AbsStartValue + minvalue)
			 > pSpeCycle->Block[block1].Point.PosX ||
		    (pSpeCycle->OrdStartValue - minvalue)
			 < pSpeCycle->Block[0].Point.PosY)
		    ret = 0;
		break;
	    case 4:
		 if ((pSpeCycle->AbsStartValue - minvalue)
			 < pSpeCycle->Block[block1].Point.PosX ||
		     (pSpeCycle->OrdStartValue - minvalue)
			 < pSpeCycle->Block[0].Point.PosY)
		    ret = 0;
		 break;
	}
	//DEBUG_PRINT("PToolPositionValid return %d\n",ret);
    return(ret);
}


/********************************************************************
 * Function:
 *	Add profile information and
 *	Check whether parameters is reasonable.
 *
 ********************************************************************/
WORD   PStep0 (setup_pointer settings,Special_Cycle        *pSpeCycle)
{
//Special_Cycle        *pSpeCycle;
//Axis_Inf             *pAxisArg;
//char                 *ptr;
WORD                    ret = 0, block1, block2, block3;
double			x0,xn,y0;

   
	 
    block1=pSpeCycle->BlockNum-1;
	xn = pSpeCycle->Block[block1].Point.PosX;
	x0 = pSpeCycle->Block[0].Point.PosX;
	y0 =pSpeCycle->Block[0].Point.PosY;

	if (pSpeCycle->ParaAxisFlag == 1)
	    {
	    xn = pSpeCycle->Block[block1].Point.PosY;
	    x0 = pSpeCycle->Block[0].Point.PosY;
		y0 =pSpeCycle->Block[0].Point.PosX;
	    }
	pSpeCycle->CutNum=pSpeCycle->CutStepNum=(int)( (xn - x0) /pSpeCycle->CutStep+0.5);
		if (pSpeCycle->ParaAxisFlag == 1)
	    {
			pSpeCycle->OrdStartValue=x0+pSpeCycle->CutStepNum*pSpeCycle->CutStep;
			pSpeCycle->AbsStartValue=y0;
			}
		else
			{
				pSpeCycle->AbsStartValue=x0+pSpeCycle->CutStepNum*pSpeCycle->CutStep;
			   pSpeCycle->OrdStartValue=y0;
			}
	   
	
  //DEBUG_PRINT("8888step 0 STRAIGHT_TRAVERSE  X %f ,z %f \n", pSpeCycle->OrdStartValue,pSpeCycle->AbsStartValue);     
	 
	
    if ( PToolPositionValid(pSpeCycle,0) )
	{
	//block1 = (pSpeCycle->BlockNum)++;
	//block2 = block1-1;
	//block3 = block2-1;
	if (pSpeCycle->ParaAxisFlag ==0)
	    {
	  /*  if (pSpeCycle->CaviceExistFlag == 1)
		pSpeCycle->Block[block2].Point.PosY
		    = pSpeCycle->Block[block3].Point.PosY;*/
	    pSpeCycle->Pos.Element[0] = -1.0;
	    pSpeCycle->Pos.Element[1] = 0.0;
	    //pSpeCycle->Pos.Element[2] = pSpeCycle->AbsMachineValue2;
	   
		
	   
	    }
	else
	    {
	    /*if (pSpeCycle->CaviceExistFlag == 1)
		pSpeCycle->Block[block2].Point.PosX
		    = pSpeCycle->Block[block3].Point.PosX;*/
	    pSpeCycle->Pos.Element[0] = 0.0;
	    pSpeCycle->Pos.Element[1] = -1.0;
	    //pSpeCycle->Pos.Element[2] = pSpeCycle->OrdMachineValue2;
	   
	    }
	  
       STRAIGHT_TRAVERSE(pSpeCycle->OrdStartValue, settings->current_y, pSpeCycle->AbsStartValue
	#ifdef AA
	,settings->AA_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
#ifdef BB
,  settings->BB_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
#ifdef CC
,  settings->CC_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
);
      settings->current_x SET_TO pSpeCycle->OrdStartValue;
      settings->current_z SET_TO pSpeCycle->AbsStartValue;
 //DEBUG_PRINT("step 0 STRAIGHT_TRAVERSE  X %f ,z %f \n", pSpeCycle->OrdStartValue,pSpeCycle->AbsStartValue); 
       //pSpeCycle->Block[block1].sequence_number=_setup.sequence_number;
	   /*
	pProc->pProg_Inf->ContinuedFlag = 0x0A;

	ptr =
	 (char *) (&((pAxisArg = pProc->pAxis_Arg)->IntAxis[pSpeCycle->AbsAxisNo-1]));
	((AxisInf *)ptr)->MachineValueReserved = pSpeCycle->AbsMachineValue2;
	((AxisInf *)ptr)->MachineValue         = pSpeCycle->AbsMachineValue2;
	((AxisInf *)ptr)->UserValueReserved    = pSpeCycle->AbsUserValue2;
	((AxisInf *)ptr)->UserValue            = pSpeCycle->AbsUserValue2;
	ptr = (char *) (&pAxisArg->IntAxis[pSpeCycle->OrdAxisNo-1]);
	((AxisInf *)ptr)->MachineValueReserved = pSpeCycle->OrdMachineValue2;
	((AxisInf *)ptr)->MachineValue         = pSpeCycle->OrdMachineValue2;
	((AxisInf *)ptr)->UserValueReserved    = pSpeCycle->OrdUserValue2;
	((AxisInf *)ptr)->UserValue            = pSpeCycle->OrdUserValue2;
       */
	pSpeCycle->WorkStep = 1;
	ret = 0;
	}
    else
    	{
	ret = NCE_PROF_TOOL_POS_INVIALD;
	
    	}
    return(ret);
}


/********************************************************************
 * Procedure:
 *	Rapid to locate before parallel axis machining.
 *
 ********************************************************************/
int  PStep1(setup_pointer settings,Special_Cycle        *pSpeCycle)
{
double end_x,end_z;
//IntMsgStruc          *pMove;

   // (pMove = &(pResult->Move))->AxisCount = 1;
   /* 
   if ((pSpeCycle->ParaAxisFlag == 1)
	pMove->MoveAxis[0].AxisNo = pSpeCycle->OrdAxisNo - 1;
    else
	pMove->MoveAxis[0].AxisNo = pSpeCycle->AbsAxisNo - 1;
  */
    if (pSpeCycle->CutStepNum == pSpeCycle->CutNum)
	{
	pSpeCycle->AbsValue = pSpeCycle->AbsStartValue;
	pSpeCycle->OrdValue = pSpeCycle->OrdStartValue;
	}
   /*  if (pSpeCycle->ParaAxisFlag == 1)
	          pSpeCycle->Pos.Element[2] = pSpeCycle->OrdValue;
	     else
		   pSpeCycle->Pos.Element[2] = pSpeCycle->AbsValue;*/
    if (pSpeCycle->ParaAxisFlag == 1)
	{
	pSpeCycle->OrdValue          -= pSpeCycle->CutStep;
	//pMove->MoveAxis[0].EndPos = pSpeCycle->OrdValue;
	end_z=settings->current_z;
	end_x= pSpeCycle->OrdValue;
	 pSpeCycle->Pos.Element[2] = pSpeCycle->OrdValue+pSpeCycle->retDistance;
	}
    else
	{
	pSpeCycle->AbsValue          -= pSpeCycle->CutStep;
	end_x=settings->current_x;
	end_z= pSpeCycle->AbsValue;
	pSpeCycle->Pos.Element[2] = pSpeCycle->AbsValue+pSpeCycle->retDistance;
	
	//pMove->MoveAxis[0].EndPos = pSpeCycle->AbsValue;
	}
	STRAIGHT_TRAVERSE(end_x, settings->current_y, end_z
	#ifdef AA
	,settings->AA_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
#ifdef BB
,  settings->BB_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
#ifdef CC
,  settings->CC_current
#else
#ifdef ALL_AXES
, 0
#endif
#endif
);
      settings->current_x SET_TO end_x;
      settings->current_z SET_TO end_z;
//DEBUG_PRINT("step 1 STRAIGHT_TRAVERSE  X %f ,z %f \n", end_x,end_z);
 
    /*
    if (!psGlobal->GCode.DyModeSub)
	CNC_SetG29Flag(psGlobal);
    else
	CNC_SetG27Flag(psGlobal);
    CNC_SetG01Flag(psGlobal);
    pMove->ProgramF = 1.0;
    */
    pSpeCycle->WorkStep = 2;
    return RS274NGC_OK;
}


/********************************************************************
 * Procedure:
 *	Parallel axis machining.
 *
 ********************************************************************/
int  PStep2(setup_pointer settings,Special_Cycle        *pSpeCycle)
{
//Special_Cycle        *pSpeCycle;
ProfBlockStruc       *pAGL;
ProfBlockStruc		Gtl;
int                  circleDirection,exception = 0;
int			returnCode;
double end_x,end_z;
 //   DEBUG_PRINT("pSpeCycle->BlockNum IS %d  1\n", pSpeCycle->BlockNum);
   //pSpeCycle->BlockNum ++;
    while( 1 )
	{
	
	
	if (CNC_CheckPointInSegment(pSpeCycle,
				    (PointVar *)&pSpeCycle->AbsValue) )
	    {
	    //DEBUG_PRINT("pSpeCycle->BlockNum IS %d  2\n", pSpeCycle->BlockNum);
	    if (pSpeCycle->ParaAxisFlag == 1)
		Gtl.Point.PosY = pSpeCycle->OrdValue;
	    else
		Gtl.Point.PosX = pSpeCycle->AbsValue;
          
	    CNC_GetLineOfParallelAxis(&Gtl,
				      pSpeCycle->ParaAxisFlag,
				      pSpeCycle->ProfDirection/*2*/);
	    pAGL = &pSpeCycle->Block[pSpeCycle->BlockNum-1];
	    if (pAGL->MoveType == 1 ||pAGL->MoveType == 0)
		CNC_AGLPointOf2Line((LineVar  *)&Gtl.CurveExpression,
				    (LineVar  *)&pAGL->CurveExpression,
				    (PointVar  *)&pSpeCycle->AbsValue,
				    TINY,
				    &exception);
	    else
		{
		circleDirection = pAGL->MoveType -3;
		CNC_AGLPointOfLineArc((LineVar  *)&Gtl.CurveExpression,
				      (CircleVar  *)&pAGL->CurveExpression,
				      &circleDirection,
				      (PointVar  *)&pSpeCycle->AbsValue,
				      TINY,