compensation.cc

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

/*
	added by ganxm to realize c compensation on 2005-06-30

*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "rcs.hh"  
#include "interpl.hh"
#include "canon.hh"
//#include "rs274ngc_errors.cc"
#include "rs274ngc_return.hh"
#include "rs274ngc.hh"

#include "Compensation.hh"

//#include "GTL_Struct.hh"




//int settings->compensation_flag=0;//定义全局变量not here goto rs274_file!!!!!!!!!!!
/*
0:no
1:first_start
2:second_start
3:during
4:end
*/
static GTLBuffer _bufferQueue;
static compensation com;




int Add_Line_to_Compensation_moveQueue(      /* 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 "Add_Line_to_Compensation_moveQueue";
	int status;
	int last_move_type;

	//com.speed=speed;
	//printf("---Line:point(%f,%f,%f)\n",point->PosX,point->PosY,point->PosZ);
	//printf("---settings->compensation_flag=%d\n",settings->compensation_flag);
	
	if(((movetype!=G_0) AND (movetype!=G_1) ) OR (settings->compensation_flag==0))
	{
		printf("add not a line to moveQueue!\n");
	}

	if(settings->compensation_flag==1)
	{
		CompensationInit();
		//20051220
		/*
		com.startPoint.PosX=point->PosX;
		com.startPoint.PosY=point->PosY;
		*/
		CHP(SetStartPoint());
	}
	//first line or the last two lines is parrallel
	CHP(ComputeTempEndPoint(movetype,settings,point->PosX,point->PosY,point->PosZ));//ok
	
    //build the current line
	CHP(Build_Line_Element(movetype,speed,point,settings));

	if(settings->compensation_flag>1)
	{	
		double beta;
		PointVar p0,p1;
		double dis;

		p0.PosX=com.last_program_horizontal_axis_pos;
		p0.PosY=com.last_program_vertical_axis_pos;
		
    	//get the intersection with the last line/arc
   	 	CHP(GetIntersection(settings));

    	CHP(DoDamage());

		//printf("0(%f,%f),last(%f,%f),no(%f,%f)\n",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);
		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);

		last_move_type=_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].ElementType;
		//execute the previous move
		//printf("----beta=%f\n",beta);
		dis=sqrt((p0.PosX-_bufferQueue.Prim.Point.PosX)*(p0.PosX-_bufferQueue.Prim.Point.PosX)+(p0.PosY-_bufferQueue.Prim.Point.PosY)*(p0.PosY-_bufferQueue.Prim.Point.PosY));
		//printf("dis-1.5r=%f\n",dis-1.5*fabs(settings->cutter_comp_radius));
		//printf("com.isBeginCom=%d,settings->compensation_flag=%d\n",com.isBeginCom,settings->compensation_flag);
		if((beta < -0.001) OR (beta > (PI + 0.001))OR((dis<1.5*fabs(settings->cutter_comp_radius))AND(1!=com.isBeginCom)AND(3!=settings->compensation_flag)))
		{
			//20051220
			//printf("simple\n");
			if(1==com.isBeginCom)
			{
				PointVar startP,endP;

				startP.PosX=com.program_horizontal_axis_pos;
				startP.PosY=com.program_vertical_axis_pos;

				endP.PosX=com.last_program_horizontal_axis_pos;
				endP.PosY=com.last_program_vertical_axis_pos;
				
				//CHP(GetIntersectionFromPointVerticalToline(&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line, &p0,&_bufferQueue.Prim.Point,settings));

				CHP(GetVerticalPointFromTwoPoint(startP,endP,&_bufferQueue.Prim.Point));
				
				com.isBeginCom=0;
			}

			if(3==settings->compensation_flag)//end compensation
			{
				_bufferQueue.Prim.Point.PosX=com.temp_cutter_end_point_last_horizontal;
				_bufferQueue.Prim.Point.PosY=com.temp_cutter_end_point_last_vertical;

				//printf("the last point(%f,%f)\n",_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY);
			}
			
			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;
			}
		}
		else
		{
			
			CHP(GetIntersectionFromPointVerticalToline(&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line, &p0,&p1,settings));
			//printf("=====p1(%f,%f) \n",p1.PosX,p1.PosY);
			
			double temp_distance, temp_x,temp_y;
			temp_x=com.temp_cutter_end_point_last_horizontal;
			temp_y=com.temp_cutter_end_point_last_vertical;
			temp_distance=sqrt((temp_x-_bufferQueue.Prim.Point.PosX)*(temp_x-_bufferQueue.Prim.Point.PosX)+(temp_y-_bufferQueue.Prim.Point.PosY)*(temp_y-_bufferQueue.Prim.Point.PosY));

			double x2,y2;
			double x11,y11,x22,y22;

			//printf("not simple:temp(%f,%f) buffer(%f,%f) distance=%f\n",temp_x,temp_y,_bufferQueue.Prim.Point.PosX,_bufferQueue.Prim.Point.PosY,temp_distance);
			//printf("last_move_type=%d\n",last_move_type);
			
			if((last_move_type IS G_1)OR(last_move_type IS G_0))
			{
				
				if(temp_distance<=fabs(settings->cutter_comp_radius))
				{

					//20051220
					if(1==com.isBeginCom)
					{
						PointVar tempCutterEnd,pointQ;
						LineVar line;

						tempCutterEnd.PosX=com.temp_cutter_end_point_last_horizontal;
						tempCutterEnd.PosY=com.temp_cutter_end_point_last_vertical;
						//printf("startPoint(%f,%f)\n",com.startPoint.PosX,com.startPoint.PosY);
						CHP(GetTangentPoint(com.startPoint,p0,tempCutterEnd,&pointQ));

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

						//get the needed intersaction from two line:l_am and L'
						CHP(GetIntersectionFrom2LineSimple(&line, &_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line, &_bufferQueue.Prim.Point,settings));

						com.isBeginCom=0;
					}

					if(3==settings->compensation_flag)//end compensation
					{
						PointVar startP,endP,tempCutterEnd,pointQ;
						LineVar line;

						startP=*point;
						endP.PosX=com.last_program_horizontal_axis_pos;
						endP.PosY=com.last_program_vertical_axis_pos;
						
						CHP(GetIntersectionFromPointVerticalToline(&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line, &endP,&tempCutterEnd,settings));

						CHP(GetTangentPoint(startP,endP,tempCutterEnd,&pointQ));

						CHP(GetALineFrom2Point(&pointQ,&startP, &line));

						//get the needed intersaction from two line:l_am and L'
						CHP(GetIntersectionFrom2LineSimple(&_bufferQueue.QueueElement[_bufferQueue.NextInQueueNo].Prim.Line, &line, &_bufferQueue.Prim.Point,settings));

					}
					
	 				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
				{

					//20051220
					if((1==com.isBeginCom)OR(3==settings->compensation_flag))
					{
						ERM(C_COMPENSATION_START_ANGLE_IS_LESS_THAN_90_ERROR);
						com.isBeginCom=0;
					}
					
					x2=_bufferQueue.Prim.Point.PosX;
					y2=_bufferQueue.Prim.Point.PosY;
					_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));

					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 if((last_move_type IS G_2) OR (last_move_type IS G_3))
			{
				double point_t_x,point_t_y;
				double k,b,k1,b1;
				double arc_end_x,arc_end_y;
				
				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=com.temp_cutter_end_point_last_vertical-k*com.temp_cutter_end_point_last_horizontal;

					if(_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.PlusCos)
					{
						k1=-_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.MinusSin/_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.PlusCos;
						b1=com.temp_cutter_end_point_vertical-k1*com.temp_cutter_end_point_horizontal;

						if(k-k1)
						{
							x2=-(b1-b)/(k1-k);
							y2=k*x2+b;
						}
						else
						{
							//printf("error when get qiexian of circle");
							ERM(C_COMPENSATION_START_ERROR);
						}
					}
					else
					{
					//20050815
						x2=com.temp_cutter_end_point_horizontal; //settings->last_program_horizontal_axis_pos;
						y2=k*x2+b;
					}
				}
				else
				{
					if(_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.PlusCos)
					{
                        			k1=-_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.MinusSin/_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line.PlusCos;
                       			b1=com.temp_cutter_end_point_vertical-k1*com.temp_cutter_end_point_horizontal;

						x2=com.temp_cutter_end_point_last_horizontal;
						y2=k1*x2+b1;
					}
					else
					{
						x2=com.temp_cutter_end_point_last_horizontal;
						y2=com.temp_cutter_end_point_last_vertical;
					}
				}
				/*
				_bufferQueue.Prim.Point.PosX=com.temp_cutter_end_point_last_horizontal;
				_bufferQueue.Prim.Point.PosY=com.temp_cutter_end_point_last_vertical;
				*/
				
				//temp_distance=sqrt((x2-_bufferQueue.Prim.Point.PosX)*(x2-_bufferQueue.Prim.Point.PosX)+(y2-_bufferQueue.Prim.Point.PosY)*(y2-_bufferQueue.Prim.Point.PosY));
				arc_end_x=com.temp_cutter_end_point_last_horizontal;
				arc_end_y=com.temp_cutter_end_point_last_vertical;
				temp_distance=sqrt((x2-arc_end_x)*(x2-arc_end_x)+(y2-arc_end_y)*(y2-arc_end_y));
		//printf("arcEnd(%f,%f) 2(%f,%f) temp_distance=%f\n",arc_end_x,arc_end_y,x2,y2,temp_distance);
				if(temp_distance<=fabs(settings->cutter_comp_radius))
				{
					//20051220
					if(3==settings->compensation_flag)//end compensation
					{
						PointVar startP,endP,tempCutterEnd,pointQ;
						LineVar line;

						startP=*point;
						endP.PosX=com.last_program_horizontal_axis_pos;
						endP.PosY=com.last_program_vertical_axis_pos;
						
						CHP(GetIntersectionFromPointVerticalToline(&_bufferQueue.QueueElement[_bufferQueue.CurrentQueueNum].Prim.Line, &endP,&tempCutterEnd,settings));
												
						CHP(GetTangentPoint(startP,endP,tempCutterEnd,&pointQ));

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

							if(fabs(com.arc_center_vertical_pos-endP.PosY)>0.001)
							{
								k1=-(com.arc_center_horizontal_pos-endP.PosX)/(com.arc_center_vertical_pos-endP.PosY);
								b1=com.temp_cutter_end_point_last_vertical-k1*com.temp_cutter_end_point_last_horizontal;
						
								x2=(b-b1)/(k1-k);
	                                			y2=k*x2+b;
							}	
							else
							{
								x2=com.temp_cutter_end_point_last_horizontal;
								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-endP.PosY)>0.001)
							{
								k1=-(com.arc_center_horizontal_pos-endP.PosX)/(com.arc_center_vertical_pos-endP.PosY);
								b1=com.temp_cutter_end_point_last_vertical-k1*com.temp_cutter_end_point_last_horizontal;
							
								x2=pointQ.PosX;
		                                		y2=k1*x2+b1;
							}
							else
							{
								printf("error in get qiexian of circle\n");
								//ERM(C_COMPENSATION_ERROR);
							}

						}
					}
					
					//printf("============first  arc !!\n");
					_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];

						settings->current_x=x;
						settings->current_y=y;
						settings->current_z=z;
					 }
					 else if(settings->plane IS CANON_PLANE_YZ)
					 {
						y=x2;
						z=y2;
						x=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];

						settings->current_x=x;
						settings->current_y=y;
						settings->current_z=z;
					 }
					 else if(settings->plane IS CANON_PLANE_XZ)
					 {
						z=x2;
						x=y2;
						y=_bufferQueue.third_axis_pos[_bufferQueue.NextInQueueNo];

						settings->current_x=x;
						settings->current_y=y;
						settings->current_z=z;
					 }
					 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
						);

					_bufferQueue.Prim.Point.PosX=x2;
					_bufferQueue.Prim.Point.PosY=y2;	

					//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((3==settings->compensation_flag))
					{
						ERM(C_COMPENSATION_START_ANGLE_IS_LESS_THAN_90_ERROR);