compensation.cc

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

					}
					
					_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));
		//printf("-=-=-=-=-=-=1(%f,%f);2(%f,%f);3(%f,%f)\n",settings->temp_cutter_end_point_last_horizontal,settings->temp_cutter_end_point_last_vertical,x2,y2,p1.PosX, p1.PosY);
					CHP(Get2PointInZheXianFrom3Point(com.temp_cutter_end_point_last_horizontal, com.temp_cutter_end_point_last_vertical,
													&x11, &y11, x2, y2, &x22, &y22, p1.PosX, p1.PosY, settings));

					_bufferQueue.Prim.Point.PosX=p1.PosX;
					_bufferQueue.Prim.Point.PosY=p1.PosY;

					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
			{
				//printf("not g0/g1/g2/g3\n");
				ERM(C_COMPENSATION_NOT_G0_G1_G2_G3_ERROR);
			}
		}

		//when last move is done, UsedQueueNum==0
		_bufferQueue.UsedQueueNum--;
	}
	//when the line is last move
	if(settings->compensation_flag==3)
	{
		_setup.compensation_end_flag=1;

		com.endPoint.PosX=point->PosX;
		com.endPoint.PosY=point->PosY;
		com.endPoint.PosZ=point->PosZ;
	}
	
  return RS274NGC_OK;
}

void CompensationInit()
{
	com.arc_radius=UNKNOWN;
	com.arc_center_horizontal_pos=UNKNOWN;
	com.arc_center_vertical_pos=UNKNOWN;
	com.arc_turn=0;
	
	com.last_arc_radius=UNKNOWN;
	com.last_arc_center_horizontal_pos=UNKNOWN;
	com.last_arc_center_vertical_pos=UNKNOWN;
	com.last_arc_turn=0;

	com.horizontal_axis_pos=UNKNOWN;
	com.vertical_axis_pos=UNKNOWN;
	com.last_horizontal_axis_pos=UNKNOWN;
	com.last_vertical_axis_pos=UNKNOWN;

	com.temp_cutter_end_point_horizontal=UNKNOWN;
	com.temp_cutter_end_point_vertical=UNKNOWN;
	com.temp_cutter_end_point_last_horizontal=UNKNOWN;
	com.temp_cutter_end_point_last_vertical=UNKNOWN;

	com.third_axis_pos=UNKNOWN;

	com.isFirstArc=0;

	//20051220
	com.isBeginCom=1;
	com.startPoint.PosX=UNKNOWN;
	com.startPoint.PosY=UNKNOWN;
	com.startPoint.PosZ=UNKNOWN;

	com.isSameCircle=0;

	_setup.compensation_end_flag=0;
	
}

int Build_Line_Element(      /* ARGUMENTS */
 const int movetype,
 const double speed,
 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++;
	
	GetALineFrom2Point(&p0,point,&_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Line);
	GetALineFrom2Point(&p0,point,&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line);

	if(settings->cutter_comp_side IS RIGHT)
		_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.Distance+=fabs(settings->cutter_comp_radius);
	else if(settings->cutter_comp_side IS LEFT)
        _bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.Distance-=fabs(settings->cutter_comp_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_Line_Element real_d=%f,program_d=%f\n",_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.Distance,_bufferQueue.ProgramQueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.Distance);
	
	return RS274NGC_OK;
}

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

	//printf("====Add_Arc_to_Compensation_moveQueue:point(%f,%f,%f),Arccenter(%f,%f),radius=%f\n",point->PosX,point->PosY,point->PosZ,ArcCenter->PosX,ArcCenter->PosY,radius);
	//printf("====settings->compensation_flag=%d turn=%d\n",settings->compensation_flag,arc_turn);
	
	com.speed=speed;
	
	if(((movetype!=G_2) AND (movetype!=G_3)) OR (settings->compensation_flag!=2))
	{
		printf("add not an arc to moveQueue Error!\n");
	}

	//first line or the last two lines is parrallel
	CHP(ComputeTempEndPointofArc(movetype,settings,ArcCenter,radius,arc_turn,point));
	
	
    //build the current arc
	CHP(Build_Arc_Element(movetype,radius,point,settings));

    //get the intersection with the last line/arc
   	CHP(GetIntersection(settings));

    CHP(DoDamage());

	//
	CHP(GetIntersectAngle(movetype, settings->cutter_comp_side, com.last_program_horizontal_axis_pos,com.last_program_vertical_axis_pos,
					com.temp_cutter_end_point_last_horizontal, com.temp_cutter_end_point_last_vertical, 
					com.program_horizontal_axis_pos, com.program_vertical_axis_pos, &beta));
	

	//execute the previous move
	distance1=sqrt((com.last_program_horizontal_axis_pos-_bufferQueue.Prim.Point.PosX)*(com.last_program_horizontal_axis_pos-_bufferQueue.Prim.Point.PosX)+
			(com.last_program_vertical_axis_pos-_bufferQueue.Prim.Point.PosY)*(com.last_program_vertical_axis_pos-_bufferQueue.Prim.Point.PosY));
//printf("distance=%f\n",distance1);
//printf("last-pr(%f,%f),_buf(%f,%f)\n",com.last_program_horizontal_axis_pos,com.last_program_vertical_axis_pos,_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY);
	if((beta < -0.001) OR (beta > (PI + 0.001))OR(distance1<=1.5*fabs(settings->cutter_comp_radius)))
	{
		//20051220
		if(1==com.isBeginCom)
		{
			//compute the first cutter center point
			if(fabs(radius)<fabs(_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius))
				CHP(GetAOutPointFrom2Point(&_bufferQueue.Prim.Point.PosX, &_bufferQueue.Prim.Point.PosY,fabs(settings->cutter_comp_radius), com.last_program_horizontal_axis_pos, com.last_program_vertical_axis_pos, 
					fabs(radius),com.arc_center_horizontal_pos,com.arc_center_vertical_pos));
			else
				CHP(GetAPointFrom2Point(com.last_program_horizontal_axis_pos, com.last_program_vertical_axis_pos, 
					fabs(settings->cutter_comp_radius), &_bufferQueue.Prim.Point.PosX, &_bufferQueue.Prim.Point.PosY, fabs(radius)-fabs(settings->cutter_comp_radius), 
						com.arc_center_horizontal_pos,com.arc_center_vertical_pos));

			com.isBeginCom=0;
		}
		
	 	CHP(Execute_C_Compensation(settings,point));
	 	//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
	{
		int last_move_type=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].ElementType;
		double temp_distance, temp_x,temp_y;
		double x2,y2,x3,y3;
		double x11,y11,x22,y22;
					
		//compute (x3,y3)
		if(fabs(radius)<fabs(_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Circle.Radius))
			CHP(GetAOutPointFrom2Point(&x3, &y3,fabs(settings->cutter_comp_radius), com.last_program_horizontal_axis_pos, com.last_program_vertical_axis_pos, 
					fabs(radius),com.arc_center_horizontal_pos,com.arc_center_vertical_pos));
		else
			CHP(GetAPointFrom2Point(com.last_program_horizontal_axis_pos, com.last_program_vertical_axis_pos, 
					fabs(settings->cutter_comp_radius), &x3, &y3, fabs(radius)-fabs(settings->cutter_comp_radius), 
						com.arc_center_horizontal_pos,com.arc_center_vertical_pos));

		if((last_move_type IS G_1)OR(last_move_type IS G_0))
		{
			double k,b,k1,b1;

			//compute (x2,y2)
			if(com.arc_center_vertical_pos-com.last_program_vertical_axis_pos)
			{
				k=-(com.arc_center_horizontal_pos-com.last_program_horizontal_axis_pos)/(com.arc_center_vertical_pos-com.last_program_vertical_axis_pos);
				b=y3-k*x3;

				if(_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.PlusCos)
				{
					k1=-_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.MinusSin/_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.PlusCos;
					b1=com.temp_cutter_end_point_last_vertical-k1*com.temp_cutter_end_point_last_horizontal;

					if(k-k1)
					{
						x2=-(b1-b)/(k1-k);
						y2=k*x3+b;
					}
				}
				else
				{
							//20050815
					x2=com.temp_cutter_end_point_last_horizontal; //settings->last_program_horizontal_axis_pos;
					y2=k*x3+b;
				}
			}
			else
			{
				if(_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.PlusCos)
				{
					k1=-_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.MinusSin/_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line.PlusCos;
					b1=com.temp_cutter_end_point_last_vertical-k1*com.temp_cutter_end_point_last_horizontal;
					//printf("=======k1=%f,b1=%f,(%f,%f)\n",k1,b1,settings->temp_cutter_end_point_last_horizontal,settings->temp_cutter_end_point_last_vertical);
					x2=x3;
					y2=k1*x2+b1;
				}
				else
				{
					x2=x3;
					y2=com.temp_cutter_end_point_last_vertical;
				}
			}
			temp_distance=sqrt((x2-x3)*(x2-x3)+(y2-y3)*(y2-y3));
			//printf("2(%f,%f)\n",x2,y2);
			if(temp_distance<=fabs(settings->cutter_comp_radius))
			{
				//20051220
				if(1==com.isBeginCom)
				{
					PointVar endPoint,tempCutterEnd,pointQ;
					LineVar line;

					endPoint.PosX=com.last_program_horizontal_axis_pos;
					endPoint.PosY=com.last_program_vertical_axis_pos;

					tempCutterEnd.PosX=com.temp_cutter_end_point_last_horizontal;
					tempCutterEnd.PosY=com.temp_cutter_end_point_last_vertical;
						
					CHP(GetTangentPoint(com.startPoint,endPoint,tempCutterEnd,&pointQ));

					//CHP(GetALineFrom2Point(&com.startPoint, &pointQ, &line));

					double k,k1,b,b1;
					//double temp_pointS_x,temp_pointS_y;
					if((fabs(pointQ.PosX-com.startPoint.PosX)>0.001) )
					{
						k=(pointQ.PosY-com.startPoint.PosY)/(pointQ.PosX-com.startPoint.PosX);
						b=com.startPoint.PosY-com.startPoint.PosX*k;

						if(fabs(com.arc_center_vertical_pos-endPoint.PosY)>0.001)
						{
							k1=-(com.arc_center_horizontal_pos-endPoint.PosX)/(com.arc_center_vertical_pos-endPoint.PosY);
							b1=y3-k1*x3;
						
							x2=(b-b1)/(k1-k);
	                                		y2=k*x2+b;
						}
						else
						{
							x2=x3;
							y2=k*x2+b;
							//printf("temp_pintS(%f,%f),k=%f,b=%f\n\n",temp_pointS_x,temp_pointS_y,k,b);
						}
						
					}
					else
					{
						if(fabs(com.arc_center_vertical_pos-endPoint.PosY)>0.001)
						{
							k1=-(com.arc_center_horizontal_pos-endPoint.PosX)/(com.arc_center_vertical_pos-endPoint.PosY);
							b1=y3-k1*x3;
						
							x2=pointQ.PosX;
	                                		y2=k1*x2+b1;
						}
						else
						{
							printf("error in get qiexian of circle\n");
							//ERM(C_COMPENSATION_ERROR);
						}

					}

					com.isBeginCom=0;
				}
				
				_bufferQueue.Prim.Point.PosX=x2;
				_bufferQueue.Prim.Point.PosY=y2;
				
				//printf("last line\n");
	 			CHP(Execute_C_Compensation(settings,point));
							
				double x,y,z;
												
				if(settings->plane IS CANON_PLANE_XY)
				{
					x=x3;
					y=y3;
					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);
			//printf("append a  line\n");
						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
			{
				//20051220
				if(1==com.isBeginCom)
				{
					ERM(C_COMPENSATION_START_ANGLE_IS_LESS_THAN_90_ERROR);
					com.isBeginCom=0;
				}
				
				_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;
		}
		else if(((last_move_type IS G_2) OR (last_move_type IS G_3)))//AND(1!=com.isSameCircle))
		{
			double k,b,k1,b1;
			
			if(com.arc_center_vertical_pos-com.last_program_vertical_axis_pos)
			{
				k=-(com.arc_center_horizontal_pos-com.last_program_horizontal_axis_pos)/(com.arc_center_vertical_pos-com.last_program_vertical_axis_pos);
				b=y3-k*x3;

				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;

					if(k-k1)
					{
						x2=-(b1-b)/(k1-k);
						y2=k*x3+b;
					}
					else
					{
						printf("error (arc_arc ) when get qiexian of arc");
						ERM(C_COMPENSATION_GET_INTERSECTION_FROM_2ARC_ERROR);