compensation.cc

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

			com.arc_radius=-(fabs(com.arc_radius)+fabs(settings->cutter_comp_radius));
		else if(move IS G_3)
		{
			com.arc_radius=fabs(com.arc_radius)-fabs(settings->cutter_comp_radius);
			if(com.arc_radius<=0)
				ERM(C_COMPENSATION_ARC_RADIUS_IS_SMALLER_THAN_CUTTER_RADIUS_ERROR);
		}

	}

	
	
	if(com.isFirstArc)
	{
		com.last_arc_center_horizontal_pos=com.arc_center_horizontal_pos;
		com.last_arc_center_vertical_pos=com.arc_center_vertical_pos;
		com.last_arc_radius=com.arc_radius;
		com.last_arc_turn=com.arc_turn;
		com.isFirstArc=0;

		com.program_arc_radius=radius;
	}

//printf("center:last(%f,%f),current(%f,%f)\n",com.last_arc_center_horizontal_pos,com.last_arc_center_vertical_pos,com.arc_center_horizontal_pos,com.arc_center_vertical_pos);
	com.temp_cutter_end_point_last_horizontal=com.temp_cutter_end_point_horizontal;
	com.temp_cutter_end_point_last_vertical=com.temp_cutter_end_point_vertical;

	if(fabs(radius)<fabs(com.arc_radius))
    {
        //printf("big circle\n");
       CHP(GetAOutPointFrom2Point(&com.temp_cutter_end_point_horizontal, &com.temp_cutter_end_point_vertical,
					fabs(settings->cutter_comp_radius) ,com.program_horizontal_axis_pos,com.program_vertical_axis_pos,
                    fabs(radius), com.arc_center_horizontal_pos,com.arc_center_vertical_pos));
    }
    else
    {
            //printf("small circle\n");
		CHP(GetAPointFrom2Point(com.program_horizontal_axis_pos, com.program_vertical_axis_pos, 
					fabs(settings->cutter_comp_radius), &com.temp_cutter_end_point_horizontal, &com.temp_cutter_end_point_vertical, 
					fabs(radius)-fabs(settings->cutter_comp_radius), 
					com.arc_center_horizontal_pos,com.arc_center_vertical_pos));
    }

//printf("ComputeTempEndPointofArc:(%f,%f)\n",com.temp_cutter_end_point_horizontal,com.temp_cutter_end_point_vertical);
	return RS274NGC_OK;
}
void InitGTLQueuePtr()
{

	_bufferQueue.FirstQueueNo  = 0;
	_bufferQueue.SecondQueueNo = 1;
	//_bufferQueue.ThirdQueueNo  = 2;
	//_bufferQueue.isInitialized=1;
	_bufferQueue.NextInQueueNo=0;
	_bufferQueue.UsedQueueNum=0;
	_bufferQueue.CurrentQueueNum=0;
	_bufferQueue.Prim.Point.PosX=UNKNOWN;
	_bufferQueue.Prim.Point.PosY=UNKNOWN;
	_bufferQueue.Prim.Point.PosZ=UNKNOWN;
	
	
}


void ClearGTLQueuePtr()
{
	_bufferQueue.FirstQueueNo  = 0;
	_bufferQueue.SecondQueueNo = 0;
	//_bufferQueue.ThirdQueueNo  = 2;
	//_bufferQueue.isInitialized=0;
	_bufferQueue.NextInQueueNo=0;
	_bufferQueue.UsedQueueNum=0;
	//_bufferQueue.isLast=0;
	_bufferQueue.ProgramQueueElement[0].ElementType=0;
	_bufferQueue.ProgramQueueElement[1].ElementType=0;
	_bufferQueue.QueueElement[0].ElementType=0;
	_bufferQueue.QueueElement[1].ElementType=0;
	_bufferQueue.MoveType=0;
	_bufferQueue.sequence_number[0]=0;
	_bufferQueue.sequence_number[1]=0;
	_bufferQueue.display_sequence_number[0]=0;
	_bufferQueue.display_sequence_number[1]=0;

	_bufferQueue.speed[0]=0;
	_bufferQueue.speed[1]=0;
	_bufferQueue.third_axis_pos[0]=UNKNOWN;
	_bufferQueue.third_axis_pos[1]=UNKNOWN;

	_bufferQueue.Prim.Point.PosX=UNKNOWN;
	_bufferQueue.Prim.Point.PosY=UNKNOWN;
	_bufferQueue.Prim.Point.PosZ=UNKNOWN;
}



int GetIntersection(setup_pointer settings)
{
  	static char name[] SET_TO "GetIntersection";
  	int status;

	int last_element_type,current_element_type;
	int intersectionNo;

	last_element_type=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].ElementType;
	current_element_type=_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].ElementType;

	if(settings->compensation_flag>1)
	{
		//20051220
		if(1==com.isBeginCom)
		{
			com.last_horizontal_axis_pos=com.startPoint.PosX;
			com.last_vertical_axis_pos=com.startPoint.PosY;
		}
		else
		{
			com.last_horizontal_axis_pos=_bufferQueue.Prim.Point.PosX;
			com.last_vertical_axis_pos=_bufferQueue.Prim.Point.PosY;
		}
	}

	
/*
	if((_bufferQueue.UsedQueueNum >0)AND(_bufferQueue.isLast ISNT 1))
	{
*/
		if((last_element_type IS G_0) OR(last_element_type IS G_1) )
		{
		//judge the current element type:line
			if((current_element_type IS G_0) OR(current_element_type IS G_1))
				CHP(GetIntersectionFrom2Line(settings));
			//judge the current element type:arc
			else if((current_element_type IS G_2) OR(current_element_type IS G_3))
			{
				intersectionNo=GetIntersectionNo(settings,1);
				//printf("g0/1_g2/3 intersectionNo=%d\n",intersectionNo);
				CHP(GetIntersectionFromlineArc(&intersectionNo,1));
				
			}else
				{
					//printf("element type error!\n");
					ERM(C_COMPENSATION_NOT_G0_G1_G2_G3_ERROR);
				}
				

		}
		else if((last_element_type IS G_2) OR(last_element_type IS G_3) )
		{

			if((current_element_type IS G_0) OR(current_element_type IS G_1))
			{

				//settings->last_arc_center_horizontal_pos=settings->arc_center_horizontal_pos;
				//settings->last_arc_center_vertical_pos=settings->arc_center_vertical_pos;
				//settings->last_arc_radius=settings->arc_radius;
				//settings->last_arc_turn=settings->arc_turn;
				
				intersectionNo=GetIntersectionNo(settings,1);
				if((intersectionNo!=0)AND(intersectionNo!=1))
					ERM(C_COMPENSATION_GetIntersectFromLineArc_ERROR);
				//printf("===:arc_line,intersectionNo=%d\n",intersectionNo);
				CHP(GetIntersectionFromlineArc(&intersectionNo,1));
			}
			else if((current_element_type IS G_2) OR(current_element_type IS G_3))
			{
					//printf("714:arc_arc\n");
            	double arc_center_distance;
                double x1,y1,x2,y2;
                CircleVar c0,c1;

                c0=_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle;
                c1=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Circle;
                
                x1=com.last_arc_center_horizontal_pos;
                y1=com.last_arc_center_vertical_pos;
                x2=com.arc_center_horizontal_pos;
                y2=com.arc_center_vertical_pos;
                  
                arc_center_distance=sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
                  
				//printf("arc_center_distance=%f;1ast(%f,%f,%f)current(%f,%f,%f)\n",arc_center_distance,x1,y1,c0.Radius,x2,y2,c1.Radius);
				if(arc_center_distance==(fabs(com.last_program_arc_radius)+fabs(com.program_arc_radius)))
                {
                	PointVar p1,p2;
                    LineVar l;
                    int no;
                    double dist1,dist2;

					if((last_element_type*current_element_type!=600 ))
					{
						printf("ERROR GetIntersection\n");
						ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
					}
					else if(last_element_type*current_element_type IS 600)
					{
						p1.PosX=x1;
                        p1.PosY=y1;
                        p2.PosX=x2;
                        p2.PosY=y2;

                        CHP(GetALineFrom2Point(&p1, &p2, &l));

						no=0;
						//printf("zhexian:no=%d\n",no);
                        CHP(GetIntersectionFromlineArcSimple(&l,&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle,&no,&p1));
                        dist1=sqrt(com.last_program_horizontal_axis_pos-p1.PosX)*(com.last_program_horizontal_axis_pos-p1.PosX)+(com.last_program_vertical_axis_pos-p1.PosY)*(com.last_program_vertical_axis_pos-p1.PosY);

                        no=1;
                        //printf("zhexian:no=%d\n",no);
                        CHP(GetIntersectionFromlineArcSimple(&l,&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Circle,&no,&p2));
                        dist2=sqrt((com.last_program_horizontal_axis_pos-p2.PosX)*(com.last_program_horizontal_axis_pos-p2.PosX)+(com.last_program_vertical_axis_pos-p2.PosY)*(com.last_program_vertical_axis_pos-p2.PosY));

                        if(dist1<=dist2)
                        {
                        	_bufferQueue.Prim.Point.PosX=p1.PosX;
                            _bufferQueue.Prim.Point.PosY=p1.PosY;
                        }
                        else
                        {
                        	_bufferQueue.Prim.Point.PosX=p2.PosX;
                            _bufferQueue.Prim.Point.PosY=p2.PosY;
                        }
					}
				}
				else if((arc_center_distance<0.01) AND(fabs(c0.Radius-c1.Radius)<0.01))
				{
				printf("same circle!\n");
					//the two circles are the same!
					com.isSameCircle=1;
					
					_bufferQueue.Prim.Point.PosX=com.temp_cutter_end_point_last_horizontal;
                    _bufferQueue.Prim.Point.PosY=com.temp_cutter_end_point_last_vertical;

                    return RS274NGC_OK;
				}
				else if(arc_center_distance<(fabs(com.last_arc_radius)+fabs(com.arc_radius)));
				{
					intersectionNo=GetIntersectionNo(settings,2);
					
					if((intersectionNo!=0)AND(intersectionNo!=1))
					{
						printf("tag---\n");
						ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
					}
					//printf("714:the intersectionNo=%d\n",intersectionNo);
					CHP(GetIntersectionFrom2Arc(&intersectionNo,1));
				}

			}
			else
			{
					//printf("element type error!\n");
					ERM(C_COMPENSATION_NOT_G0_G1_G2_G3_ERROR);
			}
		}
/*		
	}
	else if(_bufferQueue.isLast IS 1)
	{
		//compute the last intersection!
		_bufferQueue.Prim.Point.PosX=com.last_horizontal_axis_pos;
		_bufferQueue.Prim.Point.PosY=com.last_vertical_axis_pos;
	}
	else
		CHP(C_COMPENSATION_ERROR);
*/
	//printf("GetIntersection:(%f,%f)\n",_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY);
	return RS274NGC_OK;
}

/*Execute_C_Compensation


*/
int Execute_C_Compensation(setup_pointer settings,PointVar *point)
{
  	static char name[] SET_TO "Execute_C_Compensation";
  	int status;
	double x,y,z;
	int movetype,current_move;
	int turn;
	double center_horizontal,center_vertical;
	
	
	if(settings->plane IS CANON_PLANE_XY)
	 {
		x=_bufferQueue.Prim.Point.PosX;
		y=_bufferQueue.Prim.Point.PosY;
//check
		z=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
	 }
	 else if(settings->plane IS CANON_PLANE_YZ)
	 {
		y=_bufferQueue.Prim.Point.PosX;
		z=_bufferQueue.Prim.Point.PosY;
		x=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
	 }
	 else if(settings->plane IS CANON_PLANE_XZ)
	 {
		z=_bufferQueue.Prim.Point.PosX;
		x=_bufferQueue.Prim.Point.PosY;
		y=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
	 }
	 else
		ERM(NCE_BUG_PLANE_NOT_XY_YZ_OR_XZ);

	movetype=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].ElementType;
	current_move=_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].ElementType;

//printf("movetype=%d, Execute_C_Compensation (%f,%f,%f)\n",movetype,x,y,z);

	if((movetype IS G_0))
	STRAIGHT_TRAVERSE(x,y,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
		);
	else if(movetype IS G_1)
	{

		STRAIGHT_FEED(x,y,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
		);

		
	}
	else if((movetype IS G_2) OR (movetype IS G_3))
	{
     	if((current_move==G_0)||(current_move==G_1))
     	{
     		turn=com.arc_turn;
     		center_horizontal=com.arc_center_horizontal_pos;
     		center_vertical=com.arc_center_vertical_pos;
     	}
     	else
     	{
     		turn=com.last_arc_turn;
     		center_horizontal=com.last_arc_center_horizontal_pos;
     		center_vertical=com.last_arc_center_vertical_pos;
     	}
			
		if(settings->plane IS CANON_PLANE_XY)
		{
		//printf("point(%f,%f,%f),center(%f,%f),turn=%d\n",x,y,z,com.last_arc_center_horizontal_pos,com.last_arc_center_vertical_pos,turn);
		ARC_FEED(x,y,center_horizontal,center_vertical,turn,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
		);

		}
		else if(settings->plane IS CANON_PLANE_YZ)
			ARC_FEED(y,z,center_horizontal,center_vertical,turn,x
		#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
		);
		else if(settings->plane IS CANON_PLANE_XZ)
			ARC_FEED(z,x,center_horizontal,center_vertical,turn,y
		#ifdef AA
			,settings->AA_current
		#else