compensation.cc

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

					}
				}
				else
				{
					x2=x3;
					y2=k*x3+b;
				}
			}
			else
			{
				if(com.last_arc_center_vertical_pos-com.last_program_vertical_axis_pos)
				{
					k1=-(com.last_arc_center_horizontal_pos-com.last_program_horizontal_axis_pos)/(com.last_arc_center_vertical_pos-com.last_program_vertical_axis_pos);
					b1=com.temp_cutter_end_point_last_vertical-k1*com.temp_cutter_end_point_last_horizontal;

					x2=com.temp_cutter_end_point_last_horizontal;
					y2=k1*x2+b1;
				}
				else
				{
					if(last_move_type* _bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].ElementType != G_2*G_3)
					{
						printf("error when arc and arc is xiangqie!");
						ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);
					}
					else
					{
						x2=com.temp_cutter_end_point_last_horizontal;
						y2=com.temp_cutter_end_point_last_vertical;
					}
				}
			}

			temp_distance=sqrt((x2-x3)*(x2-x3)+(y2-y3)*(y2-y3));

			if(temp_distance<=fabs(settings->cutter_comp_radius))
			{

				_bufferQueue.Prim.Point.PosX=com.temp_cutter_end_point_last_horizontal;
				_bufferQueue.Prim.Point.PosY=com.temp_cutter_end_point_last_vertical;

				CHP(Execute_C_Compensation(settings,point));

							
				double x,y,z;
												
				if(settings->plane IS CANON_PLANE_XY)
				{
					x=x2;
					y=y2;
					z=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else if(settings->plane IS CANON_PLANE_YZ)
				{
					y=x2;
					z=y2;
					x=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else if(settings->plane IS CANON_PLANE_XZ)
				{
					z=x2;
					x=y2;
					y=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else
					ERM(NCE_BUG_PLANE_NOT_XY_YZ_OR_XZ);

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

				if(settings->plane IS CANON_PLANE_XY)
				{
					x=x3;
					y=y3;
						//check
					z=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else if(settings->plane IS CANON_PLANE_YZ)
				{
					y=x3;
					z=y3;
					x=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else if(settings->plane IS CANON_PLANE_XZ)
				{
					z=x3;
					x=y3;
					y=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];
				}
				else
					ERM(NCE_BUG_PLANE_NOT_XY_YZ_OR_XZ);

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

				updatePositionWhenTurn();
				//execute the status info if there have be some
				if(temp_list.len()>0)
				{
					settings->temp_queue_state=ON;
					//printf("settings->temp_queue_state=%d\n",settings->temp_queue_state);
				}

			}
			else
			{
				_bufferQueue.Prim.Point.PosX=com.temp_cutter_end_point_last_horizontal;
				_bufferQueue.Prim.Point.PosY=com.temp_cutter_end_point_last_vertical;
	 			CHP(Execute_C_Compensation(settings,point));

				CHP(Get2PointInZheXianFrom3Point(com.temp_cutter_end_point_last_horizontal, com.temp_cutter_end_point_last_vertical,
										&x11, &y11, x2, y2, &x22, &y22, x3, y3, settings));

				updatePositionWhenTurn();
				//execute the status info if there have be some
				if(temp_list.len()>0)
				{
					settings->temp_queue_state=ON;
					//printf("settings->temp_queue_state=%d\n",settings->temp_queue_state);
				}
			}

			_bufferQueue.Prim.Point.PosX=x3;
			_bufferQueue.Prim.Point.PosY=y3;

		}

		
	}
	//CHP(Execute_C_Compensation(settings));

	_bufferQueue.UsedQueueNum--;

  return RS274NGC_OK;
}

int Build_Arc_Element(      /* ARGUMENTS */
 const int movetype,
  const double radius,
  PointVar *point,
 setup_pointer settings)  /* may be NULL or a string to read */
{
	static char name[] SET_TO "Build_First_Line";
	int status;

	PointVar p0;
	
	//p0.PosX=com.last_program_horizontal_axis_pos;
    //p0.PosY=com.last_program_vertical_axis_pos;
	
	_bufferQueue.CurrentQueueNum=_bufferQueue.NextInQueueNo;
	_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].ElementType =movetype;
	_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].ElementType =movetype;
	_bufferQueue.UsedQueueNum++;
	
	_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.HeartX=com.arc_center_horizontal_pos;
	_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.HeartY=com.arc_center_vertical_pos;
	_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius=com.arc_radius;
//printf("com.radius=%f ,real=%f\n",com.arc_radius,_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius);
	_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.HeartX=com.arc_center_horizontal_pos;
	_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.HeartY=com.arc_center_vertical_pos;

	if(movetype IS G_2)
	{
		_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius=-fabs(radius);
	}
	else if(movetype IS G_3)
	{
		_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius=fabs(radius);
	}

	_bufferQueue.third_axis_pos[_bufferQueue.CurrentQueueNum]=point->PosZ;
	
	//next time will use NextInQueueNo
	_bufferQueue.NextInQueueNo=(_bufferQueue.CurrentQueueNum+1)%2;
	//save the line number
	_bufferQueue.sequence_number[_bufferQueue.CurrentQueueNum]=settings->sequence_number;
	_bufferQueue.display_sequence_number[_bufferQueue.CurrentQueueNum]=settings->display_sequence_number;

//printf("Build_Arc_Element:program_radius=%f,real=%f\n",_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius,_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius);
	return RS274NGC_OK;
}

int  ComputeTempEndPoint(
	int move, 
	setup_pointer settings,
	double px, 
	double py,
	double end_z)
{

	static char name[] SET_TO "ComputeTempEndPoint";
	int status;
	
	if(com.program_horizontal_axis_pos IS UNKNOWN )
	{
  		double alpha;
 		double cx;       
  		double cy;       
  		double distance;
  		double radius;
  		int side;
  		double theta;

  		side SET_TO settings->cutter_comp_side;
  		
		if(settings->plane IS CANON_PLANE_XY)
		{
			cx SET_TO  settings->current_x;
  			cy SET_TO  settings->current_y;
		}
		else if(settings->plane IS CANON_PLANE_XZ)
		{
			cx SET_TO  settings->current_z;
  			cy SET_TO  settings->current_x;
		}
		else if(settings->plane IS CANON_PLANE_YZ)
		{
			cx SET_TO  settings->current_y;
  			cy SET_TO  settings->current_z;
		}

  		radius SET_TO settings->cutter_comp_radius; 

		//save the circle's radius, will be used 
		//settings->cutter_comp_radius_temp=radius;
	
  		distance SET_TO hypot((px - cx), (py -cy));
		
  		CHK(((side ISNT LEFT) AND (side ISNT RIGHT)),NCE_BUG_SIDE_NOT_RIGHT_OR_LEFT);
  		CHK((distance <= radius), NCE_CUTTER_GOUGING_WITH_CUTTER_RADIUS_COMP);
		

  		theta SET_TO acos(radius/distance);
  		alpha SET_TO (side IS LEFT) ? (atan2((cy - py), (cx - px)) - theta) :
                                (atan2((cy - py), (cx - px)) + theta);
  		cx SET_TO (px + (radius * cos(alpha))); 
  		cy SET_TO (py + (radius * sin(alpha)));

		if(settings->plane IS CANON_PLANE_XY)
		{
			com.last_horizontal_axis_pos=settings->current_x;
			com.last_vertical_axis_pos=settings->current_y;
			com.last_program_horizontal_axis_pos=settings->current_x;
			com.last_program_vertical_axis_pos=settings->current_y;

			com.temp_cutter_end_point_last_horizontal=settings->current_x;
			com.temp_cutter_end_point_last_vertical=settings->current_y;
		}
		else if(settings->plane IS CANON_PLANE_XZ)
		{
			com.last_horizontal_axis_pos=settings->current_z;
			com.last_vertical_axis_pos=settings->current_x;
			com.last_program_horizontal_axis_pos=settings->current_z;
			com.last_program_vertical_axis_pos=settings->current_x;

			com.temp_cutter_end_point_last_horizontal=settings->current_z;
			com.temp_cutter_end_point_last_vertical=settings->current_x;
		}
		else if(settings->plane IS CANON_PLANE_YZ)
		{
			com.last_horizontal_axis_pos=settings->current_y;
			com.last_vertical_axis_pos=settings->current_z;
			com.last_program_horizontal_axis_pos=settings->current_y;
			com.last_program_vertical_axis_pos=settings->current_z;

			com.temp_cutter_end_point_last_horizontal=settings->current_y;
			com.temp_cutter_end_point_last_vertical=settings->current_z;
		}


		//com.horizontal_axis_pos=cx;
		//com.vertical_axis_pos=cy;
		com.program_horizontal_axis_pos=px;
		com.program_vertical_axis_pos=py;

		com.temp_cutter_end_point_horizontal=cx;
		com.temp_cutter_end_point_vertical=cy;
//printf("cy=%f,py=%f \n",cy,py);
//	printf("721:first in ComputeTempEndPoint settings->horizontal_axis_pos=%f,settings->vertical_axis_pos=%f \n",settings->horizontal_axis_pos,settings->vertical_axis_pos);

	}
	else{
  		double alpha;
  		double beta;
 		double end_x;
  		double end_y; 
  		double gamma;
  		double mid_x; 
  		double mid_y; 
  		double radius;
  		int side;
  		double small SET_TO TOLERANCE_CONCAVE_CORNER; 
  		double start_x, start_y; 
  		double theta;

  		start_x SET_TO  com.program_horizontal_axis_pos; //settings->program_x;
  		start_y SET_TO  com.program_vertical_axis_pos;  //settings->program_y;	
		
	    side SET_TO settings->cutter_comp_side;
     	radius SET_TO settings->cutter_comp_radius;
      	theta SET_TO atan2(_bufferQueue.Prim.Point.PosY - start_y,
                         _bufferQueue.Prim.Point.PosX- start_x);
      	alpha SET_TO atan2(py - start_y, px - start_x);

      	if (side IS LEFT)
        {
        	if (theta < alpha)
        		theta SET_TO (theta + TWO_PI);
        	beta SET_TO ((theta - alpha) - PI2);
        	gamma SET_TO PI2;
       	}
      	else if (side IS RIGHT)
      	{
         	if (alpha < theta)
            	alpha SET_TO (alpha + TWO_PI);
          	beta SET_TO ((alpha - theta) - PI2);
          	gamma SET_TO -PI2;
        }
      	else
        	ERM(NCE_BUG_SIDE_NOT_RIGHT_OR_LEFT);
      	
      	end_x SET_TO (px + (radius * cos(alpha + gamma)));
      	end_y SET_TO (py + (radius * sin(alpha + gamma)));
      	mid_x SET_TO (start_x + (radius * cos(alpha + gamma)));
      	mid_y SET_TO (start_y + (radius * sin(alpha + gamma)));


		com.last_program_horizontal_axis_pos=com.program_horizontal_axis_pos;
		com.last_program_vertical_axis_pos=com.program_vertical_axis_pos;
		com.program_horizontal_axis_pos=px;
		com.program_vertical_axis_pos=py;

		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;

		com.temp_cutter_end_point_horizontal=end_x;
		com.temp_cutter_end_point_vertical=end_y;	
	//printf("721:in ComputeTempEndPoint (%f,%f) \n",com.temp_cutter_end_point_horizontal,com.temp_cutter_end_point_vertical);
		}

	 return RS274NGC_OK;
}

int  ComputeTempEndPointofArc(
	int move, 
	setup_pointer settings,
	PointVar *ArcCenter,
 	const double radius,
 	int arc_turn,
 	PointVar *point
	)
{
	static char name[] SET_TO "ComputeTempEndPoint";
	int status;


//printf("ComputeTempEndPointofArc:center:(%f,%f)\n",ArcCenter->PosX,ArcCenter->PosY);	
		
	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.last_program_arc_radius=com.program_arc_radius;
		com.program_arc_radius=radius;
	}
	com.arc_center_horizontal_pos=ArcCenter->PosX;
	com.arc_center_vertical_pos=ArcCenter->PosY;
	com.arc_turn=arc_turn;
	com.arc_radius=radius;
	
	com.last_program_horizontal_axis_pos=com.program_horizontal_axis_pos;
	com.last_program_vertical_axis_pos=com.program_vertical_axis_pos;
	com.program_horizontal_axis_pos=point->PosX;
	com.program_vertical_axis_pos=point->PosY;

	if(settings->cutter_comp_side IS RIGHT)
	{	
		if(move IS G_2)
		{
			com.arc_radius=-(fabs(com.arc_radius)-fabs(settings->cutter_comp_radius));
			//compare the radius to cutter_radius when small circle
			if(com.arc_radius>=0)
				ERM(C_COMPENSATION_ARC_RADIUS_IS_SMALLER_THAN_CUTTER_RADIUS_ERROR);
		}
		else if(move IS G_3)
			com.arc_radius=fabs(com.arc_radius)+fabs(settings->cutter_comp_radius);
	}
	else if(settings->cutter_comp_side IS LEFT)
	{
		if(move IS G_2)