Exemplo n.º 1
0
int CreatePath1A_XOffset(double dX, double dY, double dZ, Path & vPath)
{
	int res;

//
//Clear the segment list and initialize the starting point for the path
//
	vPath.ClearSegListA(dX, dY, dZ);    //Clear the segment list and set the
									//  starting point for the path
									//  at X = 0, Y = 1, Z = 0

//
//Add line and arc segments to the path module's segment list for first move
//


	res = vPath.AddLineSegA(dX, dY+1.0, dZ+0.0);    //Add a segment to move to x=0, y=2, z=0
	if (res<0) return -1;

	res = vPath.AddArcSegA( dX+1.0, dY+2.0, dZ+0.0,     //end point of arc: x=1, y=3, z=0
							 dX+1.0, dY+1.0, dZ+0.0,     //center point of arc: x=1, y=2, z = 0
							 0.0, 0.0, -1.0 );   //normal vector to arc plane: x = 0, y = 0, z = -1
	if (res<0) return -1;

	res = vPath.AddLineSegA(dX+2.0, dY+2.0, dZ+0.0);    //line segment endpoint: x=4, y=3, z=0
	if (res<0) return -1;

//	ServoInf::m_pNMCServo->ClearSegList(4.0, 3.0, 0.0);    //Clear the segment list and set the
//	ExecutePath3Axis();

	res = vPath.AddArcSegA( dX+3.0, dY+1.0, dZ+0.0,     //end point of arc: x=5, y=2, z=0
							 dX+2.0, dY+1.0, dZ+0.0,     //center point of arc: x=4, y=2, z = 0
							 0.0, 0.0, -1.0 );   //normal vector to arc plane: x = 0, y = 0, z = -1
	if (res<0) return -1;

	res = vPath.AddLineSegA(dX+3.0, dY+0.0, dZ+0.0);    //line segment endpoint: x=5, y=0, z=0
	if (res<0) return -1;

	res = vPath.AddStartA(dX+3.0, dY+0.0, dZ+0.0);    //line segment endpoint: x=5, y=0, z=0
	if (res<0) return -1;

	res = vPath.AddLineSegA(dX+0.0, dY+0.0, dZ+0.0);    //line segment endpoint: x=5, y=0, z=0
	if (res<0) return -1;

}
Exemplo n.º 2
0
int TestCoordMotionShortPath()
{
	
	int iError;
	int res;
	int iModule;
	Path vPath;

	iError=InitNMCModule3Axis (iModule, vPath);
	if (iModule < 3)
		return -1;
	
	g_stage.Rotate(180.0);
	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(1.0, 1.0, 1.0);

	vPath.SetPathParams(P_60HZ,   //path frequency = 30 Hz
					75,       //Store a minimum of 45 points in the path point buffer
					6684.57447,  //X scale - 20000.0 counts per inch
					6684.57447,  //Y scale - 20000.0 counts per inch
					2*6684.57447,      //Z scale - 1.0 counts per inch - not used
					1.0);     //acceleration = 1.0 inch/second/second

	vPath.SetOrigin(0.0, 0.0, 0.0); //set the origin to X = 0, Y = 0, Z = 0
	vPath.SetFeedrate(1.0);         //feedrate = 1.0 inches/second
	vPath.SetTangentTolerance(10.0);  //continuous path tangent tolerence = 10 degrees	
	g_stage.InitPathMode(vPath);
	
	double x,y,z;
//	g_stage.GetPos(x,y,z);
//	InitPathParam(P_60HZ, 60, 
//		          6684.57447, 6684.57447, 2*6684.57447,
//				  1.0,
//				  1.0);

	g_stage.GetPos(x,y,z);
	g_stage.ResetPos();
//Clear the segment list and set the starting point for the path
//  at X = 0, Y = 1, Z = 0
	vPath.ClearSegListA(0,0,0);    

//Add a segment to move to x=0, y=2, z=0
	res = vPath.AddLineSegA(0.0, 1.0, 0.0);    
	if (res<0) return -1;
	
	res = vPath.AddArcSegA( 1.0, 2.0, 0.0,     //end point of arc: x=1, y=2, z=0
							 1.0, 1.0, 0.0,     //center point of arc: x=1, y=1, z = 0
							 0.0, 0.0, -1.0 );   //normal vector to arc plane: x = 0, y = 0, z = -1
	if (res<0) return -1;
	g_stage.ExecuteCoordMotion(vPath);
	
	vPath.ClearSegListA(1.0, 2.0, 0.0);    
	g_stage.GetPos(x,y,z);
	
	res = vPath.AddLineSegA(2.0, 2.0, 0.0);    //line segment endpoint: x=4, y=3, z=0
	if (res<0) return -1;
	
	res = vPath.AddArcSegA( 3.0, 1.0, 0.0,     //end point of arc: x=5, y=2, z=0
							 2.0, 1.0, 0.0,     //center point of arc: x=4, y=2, z = 0
							 0.0, 0.0, -1.0 );   //normal vector to arc plane: x = 0, y = 0, z = -1
	if (res<0) return -1;
	
//line segment endpoint: x=5, y=0, z=0
	res = vPath.AddLineSegA(3.0, 0.0, 0.0);    
	if (res<0) return -1;

//line segment endpoint: x=5, y=0, z=0
	g_stage.ExecuteCoordMotion(vPath);

	vPath.ClearSegListA(3.0, 0.0, 0.0);    
	if (res<0) return -1;

//line segment endpoint: x=5, y=0, z=0
	res = vPath.AddLineSegA(0.0, 0.0, 0.0);    
	if (res<0) return -1;
	g_stage.ExecuteCoordMotion(vPath);
	
	ResetModules(iModule);
	return 0;
};