compensation.cc

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

		#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
		);


	}
	else
		 ERM(NCE_BUG_CODE_NOT_G0_OR_G1);

	settings->current_x=x;
	settings->current_y=y;
	settings->current_z=z;
/*
	//execute the status info if there have be some
	if(temp_list.len()>0)
	{
		settings->temp_queue_state=ON;
	}
	*/
	return RS274NGC_OK;
	
}



 int GetALineFrom2Point(PointVar         *p0,
				    PointVar        *p1,
				    LineVar          *l)
{
static char name[] SET_TO "GetALineFrom2Point";
double  xvar,yvar,distance;

        xvar = p1->PosX - p0->PosX;
        yvar = p1->PosY - p0->PosY;

        distance = xvar * xvar + yvar * yvar;
        distance = sqrt(distance);

        if (distance >0.001)   /* the same point ? */
          {
          l->MinusSin = -yvar / distance;
          l->PlusCos  = xvar / distance;
	    l->Distance = -(l->MinusSin * p0->PosX + l->PlusCos * p0->PosY);
	  }
	else
		ERM(C_COMPENSATION_GetALineFrom2Point);
	return RS274NGC_OK;
 }

int GetIntersectionNo(setup_pointer settings,int flag)
{
  	static char name[] SET_TO "GetIntersectionNo";
  	int status;
	int no;
	double distance1,distance2;
	double x,y;


	x=com.temp_cutter_end_point_last_horizontal;
	y=com.temp_cutter_end_point_last_vertical;

/*
	flag 1:line arc
	flag 2:arc arc

*/
//printf("!!!!!!!!!the last program point(%f,%f)\n",x,y);

if(flag IS 1)	
{	
	no=0;
	//printf("GetIntersectionNo:no=%d\n",no);
	CHP(GetIntersectionFromlineArc(&no,0));
	distance1=sqrt((x- _bufferQueue.Prim.Point.PosX)*(x- _bufferQueue.Prim.Point.PosX)+(y- _bufferQueue.Prim.Point.PosY)*(y- _bufferQueue.Prim.Point.PosY));
	
	no=1;
	//printf("GetIntersectionNo:no=%d\n",no);
	CHP(GetIntersectionFromlineArc(&no,0));
	distance2=sqrt((x- _bufferQueue.Prim.Point.PosX)*(x- _bufferQueue.Prim.Point.PosX)+(y- _bufferQueue.Prim.Point.PosY)*(y- _bufferQueue.Prim.Point.PosY));
}
else if(flag IS 2)
{

	no=0;
	CHP(GetIntersectionFrom2Arc(&no,0));
	distance1=sqrt((x- _bufferQueue.Prim.Point.PosX)*(x- _bufferQueue.Prim.Point.PosX)+(y- _bufferQueue.Prim.Point.PosY)*(y- _bufferQueue.Prim.Point.PosY));
//printf("##########\n");
//616
//printf("no=%d:(%f,%f);distance1:%f\n",no,_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY,distance1);

	no=1;
	CHP(GetIntersectionFrom2Arc(&no,0));
	distance2=sqrt((x- _bufferQueue.Prim.Point.PosX)*(x- _bufferQueue.Prim.Point.PosX)+(y- _bufferQueue.Prim.Point.PosY)*(y- _bufferQueue.Prim.Point.PosY));
//616
//printf("no=%d:(%f,%f);distance2:%f\n",no,_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY,distance2);
	
}else
	ERM(C_COMPENSATION_GetIntersectionNo);

	if(((distance1-distance2)<=0))//AND distance1<0.01)
		no=0;
	else if(((distance2-distance1)<=0))//AND distance2<0.01)
		no=1;

//ganxingming
//printf("GetIntersectionNo:return no:%d\n",no);

return no;
}

/*GetIntersectionFrom2Line

Returned Value: int

Side effects:
	get the intersection from given two lines.
	
Called by:
	GetIntersection

*/
 int GetIntersectionFrom2Line(setup_pointer settings)
 {
	static char name[] SET_TO "GetIntersectionFrom2Line";
	int  status;
	double    tmpvalue,distance_Off;
	LineVar *l0,*l1;
	PointVar *p;


	//past line
	l0=&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line;
	//current line
	l1=&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line;
	//get the return point
	p=&_bufferQueue.Prim.Point;

	tmpvalue = l1->PlusCos * l0->MinusSin - l1->MinusSin * l0->PlusCos;

	distance_Off=l1->Distance-l0->Distance;

// parellel and the same line
	if ((fabs(tmpvalue) <= 0.0001)&&(fabs(distance_Off)<0.0001) )
	{
	//printf("====the same line!\n");
		p->PosX=com.temp_cutter_end_point_last_horizontal;
		p->PosY=com.temp_cutter_end_point_last_vertical;

	}//parellel
	else if((fabs(tmpvalue) <= 0.0001)&&(fabs(distance_Off)>0.0001) )
	      {
	      //printf("====parellel!\n");
		 ERM(C_COMPENSATION_2LineParellel);

		}
	else
	  {
	  p->PosX = (l1->Distance*l0->PlusCos - l1->PlusCos*l0->Distance)/
                      tmpvalue;
          p->PosY = (l0->Distance*l1->MinusSin - l1->Distance*l0->MinusSin)/
                      tmpvalue;
	  }
	return RS274NGC_OK;
}

/*GetIntersectionFromlineArc

Returned Value: int

Side effects:
	get the intersection from given  a line and an arc.
	
Called by:
	GetIntersection


*/
int GetIntersectionFromlineArc(int *s,int flag)
{
	static char name[] SET_TO "GetIntersectionFromlineArc";
	int  status;
	int     sign;
	double  tmp0,tmp1,tmp2;
	LineVar   *l0;
	CircleVar    *c0;
	PointVar   *p;
	int pre_element_type,current_element_type;
/*
	flag	1:compute the real element intersection;
	flag  0:compute the program element intersection;
*/


if(flag==1)
{
	pre_element_type=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].ElementType;
	current_element_type=_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].ElementType;

	if(((pre_element_type IS G_0)OR(pre_element_type IS G_1))AND((current_element_type IS G_2)OR(current_element_type IS G_3)))
	{
		l0=&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line;
		c0=&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle;
	}
	else if(((pre_element_type IS G_2)OR(pre_element_type IS G_3))AND((current_element_type IS G_0)OR(current_element_type IS G_1)))
		{
			c0=&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Circle;
			l0=&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line;
		}else
			ERM(C_COMPENSATION_GetIntersectFromLineArc_ERROR);
}
else if(flag==0)
{
	pre_element_type=_bufferQueue.ProgramQueueElement[_bufferQueue.NextInQueueNo].ElementType;
	current_element_type=_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].ElementType;

	if(((pre_element_type IS G_0)OR(pre_element_type IS G_1))AND((current_element_type IS G_2)OR(current_element_type IS G_3)))
	{
		l0=&_bufferQueue.ProgramQueueElement[_bufferQueue.NextInQueueNo].Prim.Line;
		c0=&_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle;
	}
	else if(((pre_element_type IS G_2)OR(pre_element_type IS G_3))AND((current_element_type IS G_0)OR(current_element_type IS G_1)))
	{
		c0=&_bufferQueue.ProgramQueueElement[_bufferQueue.NextInQueueNo].Prim.Circle;
		l0=&_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Line;
	}else
		{
			//printf("get the line and arc for program is error!\n");
			ERM(C_COMPENSATION_GetIntersectFromLineArc_ERROR);
		}

}

	p=&_bufferQueue.Prim.Point;
	
	if (*s)
	  sign = -1;
    else
      sign =  1;

        // compute the distance (circle heart to the line)

     tmp0 = c0->HeartX*l0->MinusSin + c0->HeartY*l0->PlusCos + l0->Distance;
     tmp1 = fabs(c0->Radius) - fabs(tmp0);
     
		//printf("GetIntersectionFromlineArc l.d=%f;circle(%f,%f,r=%f)\n",l0->Distance,c0->HeartX,c0->HeartY,c0->Radius);
		
		//printf("GetIntersectionFromlineArc(numNo=%d):temp1=%f,temp0=%f\n",*s,tmp1,tmp0);
	
     if (fabs(tmp1) <= 0.0001)
     {
          p->PosX = c0->HeartX - l0->MinusSin * tmp0;
          p->PosY = c0->HeartY - l0->PlusCos   * tmp0;
     }
     else if (tmp1 > 0.0)
     {
	  tmp2 = c0->Radius * c0->Radius - tmp0 * tmp0;
	  
	  tmp2 =sqrt(tmp2) * sign;
	  p->PosX = c0->HeartX - l0->MinusSin * tmp0 - l0->PlusCos * tmp2;
	  p->PosY = c0->HeartY - l0->PlusCos  * tmp0 + l0->MinusSin * tmp2;

	}
	else
	{
		printf("C_COMPENSATION_GetIntersectFromLineArc_ERROR\n");
	   	ERM(C_COMPENSATION_GetIntersectFromLineArc_ERROR);
	}
	
//ganxingming
//printf("the GetIntersectionFromlineArc(numNo=%d):%f,%f\n",*s,p->PosX,p->PosY);

	return RS274NGC_OK;
}

/*GetIntersectionFromlineArcSimple

Returned Value: int

Side effects:
	get the intersection from given a line and an arc.
	
Called by:
	GetIntersection
*/
int GetIntersectionFromlineArcSimple(LineVar  *l0,
				      CircleVar    *c0,
				      int     *s,
				      PointVar    *p )
{
	static char name[] SET_TO "GetIntersectionFromlineArcSimple";
	int  status;
	int     sign;
	double  tmp0,tmp1,tmp2;

		if (*s)
	  		sign = -1;
        else
          sign =  1;
        
        tmp0 = c0->HeartX*l0->MinusSin + c0->HeartY*l0->PlusCos + l0->Distance;
        tmp1 = fabs(c0->Radius) - fabs(tmp0);
			//printf("715:tmp1=%f\n",tmp1);
        if (fabs(tmp1) <= 0.0001)
        {
          p->PosX = c0->HeartX - l0->MinusSin * tmp0;
          p->PosY = c0->HeartY - l0->PlusCos   * tmp0;
        }
        else if (tmp1 > 0.0)
        {
	  		tmp2 = c0->Radius * c0->Radius - tmp0 * tmp0;
	  		tmp2 =sqrt(tmp2) * sign;
			//fprintf(stderr,"center(=%f,=%f)\n",c0->HeartX,c0->HeartY);
			//fprintf(stderr,"%x\n", p );
	  		p->PosX = c0->HeartX - l0->MinusSin * tmp0 - l0->PlusCos * tmp2;
	  		p->PosY = c0->HeartY - l0->PlusCos  * tmp0 + l0->MinusSin * tmp2;
	  	}
		else
		{
			ERM(C_COMPENSATION_GetIntersectFromLineArc_ERROR);
		}

		//	printf("614:the GetIntersectionFromlineArcSimple(numNo=%d):%f,%f\n",*s,p->PosX,p->PosY);
	return RS274NGC_OK;
}

/*GetIntersectionFrom2Arc

Returned Value: int

Side effects:
	get the intersection from given two arcs.
	
Called by:
	GetIntersection
	GetIntersectionNo


*/
int GetIntersectionFrom2Arc(int *s, int flag)
{
	static char name[] SET_TO "GetIntersectionFromlineArc";
	int  status;

	CircleVar *c0,*c1;
	LineVar     l;
	double      yvar,xvar,distance,rsum,rvar,xm,ym,rm1,rm2;
	int         internum;
	
	int     sign;
	double  tmp0,tmp1,tmp2;
	LineVar   *l0;
	PointVar   p;
	int pre_element_type,current_element_type;
/*
	flag	1:compute the real element intersection;
	flag  0:compute the program element intersection;
*/
//printf("in GetIntersectionFrom2Arc:flag=%d(0-program)\n",flag);

if(flag==0)
{
	c0=&_bufferQueue.ProgramQueueElement[_bufferQueue.NextInQueueNo].Prim.Circle;
	c1=&_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle;
	//printf("program circle:c0(%f,%f,r=%f),c1(%f,%f,r=%f)\n",c0->HeartX,c0->HeartY,c0->Radius,c1->HeartX,c1->HeartY,c1->Radius);

}else if(flag==1)	
{
	c0=&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Circle;
	c1=&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle;

	//printf("real circle:c0(%f,%f,r=%f),c1(%f,%f,r=%f)\n",c0->HeartX,c0->HeartY,c0->Radius,c1->HeartX,c1->HeartY,c1->Radius);
}

	 xvar = c1->HeartX - c0->HeartX;
        yvar = c1->HeartY - c0->HeartY;

        distance = xvar * xvar + yvar * yvar;

        distance =sqrt(distance); /* get the distance between 2 heart */

	if (distance <= 0.01)    /* is the same circle ? */
	  {
          	printf("the same ciicle!\n");
		ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);		
          }

	rsum = fabs(c0->Radius) + fabs(c1->Radius); /* sum of radius */
        rvar = distance - rsum;
        internum = 0;
        if (rvar >0.01)   /* no intersection ? */
          {
	         printf("615:no intersection \n");
		ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
	  }

        if (rvar >= - 0.01)  /* two circles are tangent ? */
          {
          printf("615:two circles are tangent\n");
		ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
          }
	else if (c1->Radius > c0->Radius) /* who is the bigger circle */
	  rvar =fabs(c1->Radius) - (distance + fabs(c0->Radius)) ;
        else
          rvar = fabs(c0->Radius) - (distance + fabs(c1->Radius)) ;

        if (!internum && rvar > 0.01)  /* small within bigger and no inter */
	{	
		printf("615:small within bigger and no inter\n");
		ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
        }
        else
          {
          l.MinusSin = xvar / distance;
          l.PlusCos  = yvar / distance;
	  xm  = (c0->HeartX + c1->HeartX) / 2;