コード例 #1
0
int TestPtToPtAndCoordOutPts()
{
	
	int iModule;
	int iError;
	double dX, dY, dZ;

	iError=InitNMCModule3Axis (iModule);

	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(1.0, 1.0, 1.0);

//	g_stage.EnableAmp();
//	g_stage.ResetPos();
//	g_stage.SetPathStatus();

//Initialize path control module parameters
	g_stage.SetPathFreq(P_60HZ);
	g_stage.SetNumberOfPoints(75);
	g_stage.SetScale(15000.0, 15000.0, 15000.0);
	g_stage.SetPathAcceleration(0.175);
	g_stage.SetFeedrate(0.175);         //feedrate = 1.0 inches/second
	g_stage.SetTangentTolerance(10.0);  //continuous path tangent tolerence = 10 degrees
	g_stage.SetPathParams();     //acceleration = 1.0 inch/second/second
	g_stage.SetOrigin( 0.0, 0.0, 0.0 ); //set the origin to X = 0, Y = 0, Z = 0
	
//	g_stage.ClosePointsOut();

	return 0;
};
コード例 #2
0
int TestPtToPtAndCoordStageMotion()
{
	
	int iModule;
	int iError;
	double dX, dY, dZ;

	Path vPath;

	iError=InitNMCModule3Axis (iModule, vPath);

	g_stage.Rotate(180.0);
	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(1.0, 1.0, 1.0);

//	g_stage.EnableAmp();
//	g_stage.ResetPos();
//	g_stage.SetPathStatus();

//	g_stage.EnableAmp();
//	g_stage.ResetPos();
//	g_stage.SetPathStatus();
	vPath.SetPathParams(P_60HZ,   //path frequency = 30 Hz
					75,       //Store a minimum of 45 points in the path point buffer
					15000.0,  //X scale - 20000.0 counts per inch
					15000.0,  //Y scale - 20000.0 counts per inch
					15000.0,      //Z scale - 1.0 counts per inch - not used
					0.175);     //acceleration = 1.0 inch/second/second
//	InitServoPathParams(P_30HZ,1, 2, 3);

	vPath.SetOrigin(0.0, 0.0, 0.0); //set the origin to X = 0, Y = 0, Z = 0
	vPath.SetFeedrate(0.175);         //feedrate = 1.0 inches/second
	vPath.SetTangentTolerance(10.0);  //continuous path tangent tolerence = 10 degrees	
	g_stage.InitPathMode(vPath);
	

	BOOL bMotion;
	iError=g_stage.MoveTo(0.0, 0.000, -0.5, true);
	g_stage.GetPos( dX, dY, dZ );
	CreatePath1A_XOffset( dX, dY, dZ, vPath);
	ExecutePath3Axis();
	
	iError=g_stage.MoveTo(1.0, 0.000, 0.000, true);
	iError=g_stage.MoveTo(1.0, 0.000, -0.5, true);

	g_stage.GetPos( dX, dY, dZ );
	CreatePath1A_XOffset( dX, dY, dZ, vPath );
	ExecutePath3Axis();
	
	iError=g_stage.MoveTo(-1.0, 1.000, 0.0, true);
	iError=g_stage.MoveTo(-1.0, 1.000, -0.5, true);
	g_stage.GetPos( dX, dY, dZ );
	CreatePath1A_XOffset( dX, dY, dZ, vPath );
	ExecutePath3Axis();
	
	iError=g_stage.MoveTo(0.0, 0.000, 0.000, true);

	return 0;
};
コード例 #3
0
int TestCoordMotionCircleAtOrigin()
{
	
	int iModule;
	int iError;
	double dX=0.0, dY=0.0, dZ=0.0;
	Path vPath;

	iError=InitNMCModule3Axis (iModule, vPath);

	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
					15000.0,  //X scale - 20000.0 counts per inch
					15000.0,  //Y scale - 20000.0 counts per inch
					15000.0,      //Z scale - 1.0 counts per inch - not used
					0.175);     //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(0.175);         //feedrate = 1.0 inches/second
	vPath.SetTangentTolerance(10.0);  //continuous path tangent tolerence = 10 degrees	
	g_stage.InitPathMode(vPath);

	

	
	BOOL bMotion;
	iError=g_stage.MoveTo(2.5, 0.0, 0.0, true);
	g_stage.GetPos( dX, dY, dZ );

	vPath.ClearSegListA(2.5, 0.0, 0.0);    //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
//
	int res;
	res = vPath.AddArcSegA( 0.0, 2.5, 0.0,     //end point of arc: x=1, y=3, z=0
							 0.0, 0.0, 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;
	g_stage.ExecuteCoordMotion(vPath);

	vPath.ClearSegListA(0.0, 2.5, 0.0);    //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.AddArcSegA( -2.5, 0.0, 0.0,     //end point of arc: x=1, y=3, z=0
							 0.0, 0.0, 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;
	g_stage.ExecuteCoordMotion(vPath);


//	ExecutePath3Axis();
	
//	iError=g_stage.MoveTo(-1.0, 1.000, 0.0, true);
//	iError=g_stage.MoveTo(-1.0, 1.000, -0.5, true);
//	g_stage.GetPos( dX, dY, dZ );
//	CreatePath1A_XOffset( dX, dY, dZ );
//	ExecutePath3Axis();
	
	iError=g_stage.MoveTo(0.0, 0.000, 0.000, true);
	return 0;
};
コード例 #4
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;
};
コード例 #5
0
int TestPtToPtMotionShort()
{

//	NMCSERVO Servo;
	int iModules;
	int iError;
	long lPosition;
	double XPos,YPos,ZPos;
	double CmXPos,CmdYPos,CmdZPos;
	double dCmdXSpeed,dCmdYSpeed,dCmdZSpeed;
	double dCmdXAccel,dCmdYAccel,dCmdZAccel;
	double dVelocity;
	double dXHome, dYHome, dZHome;
	byte byAD;

	iModules=g_stage.Initialize("COM4:");

	SetScaling_3Axis();
	SetPIDGain_3Axis();


	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(6.0, 6.0, 6.0);

	g_stage.EnableAmp();
	g_stage.ResetPos();
	g_stage.GetHome( dXHome, dYHome, dZHome);

	BOOL bMotion;
	iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
	iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
	iError=g_stage.MoveRel(0.00, 0.00, 0.15, true);
	iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
	iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
	iError=g_stage.MoveRel(0.15, 0.00, 0.00, true);
	iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
	iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
	iError=g_stage.MoveRel(0.00, 0.15, 0.00, true);
	iError=g_stage.MoveTo(1.00, 1.000, 1.000, true);
	g_stage.GetPos(XPos, YPos, ZPos );
	if ( !((XPos < 1.02) && (XPos > 0.98)) && 
		 !((YPos < 1.02) && (YPos > 0.98)) &&
		 !((ZPos < 1.02) && (ZPos > 0.98))   )
		 MessageBox(NULL, "Error with Position","Error Dialog", 1);

	g_stage.GetHome( dXHome, dYHome, dZHome);

	g_stage.GetCmdAccel(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
	g_stage.GetCmdAccel(dCmdXAccel, dCmdYAccel, dCmdZAccel );

	iError=g_stage.MoveTo(0.0, 0.0, 0.0, true);

	g_stage.GetPos(XPos, YPos, ZPos );
	if ( !((XPos < 0.02) && (XPos > -0.02)) && 
		 !((YPos < 0.02) && (YPos > -0.02)) &&
		 !((ZPos < 0.02) && (ZPos > -0.02))   )
		 MessageBox(NULL, "Error with Position","Error Dialog", 1);

	iError=g_stage.MoveTo(0.000, 0.000, 0.000, true);
	g_stage.GetPos(XPos, YPos, ZPos );

	g_stage.GetHome( dXHome, dYHome, dZHome);

	return 0;
}	
コード例 #6
0
int TestPtToPtMotionInPCSCommand()
{

//	NMCSERVO Servo;
	int iModules;
	int iError;
	long lPosition;
	double dXPosition,dYPosition,dZPosition;
	double dCmdXPosition,dCmdYPosition,dCmdZPosition;
	double dCmdXSpeed,dCmdYSpeed,dCmdZSpeed;
	double dCmdXAccel,dCmdYAccel,dCmdZAccel;
	double dVelocity;
	double dXHome, dYHome, dZHome;
	byte byAD;
	
	iModules=g_stage.Initialize("COM9:");

	SetScaling_3Axis();
	SetPIDGain_3Axis();

	g_stage.Rotate(180.0);

	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(1.0, 1.0, 1.0);

	g_stage.EnableAmp();
	g_stage.ResetPos();
	
	g_stage.GetCmdVel(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
	g_stage.GetSpeed(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
	g_stage.GetCmdAccel(dCmdXAccel, dCmdYAccel, dCmdZAccel );

	millCmd millctrl;
	BOOL bMotion;

//	iError=g_stage.MoveTo(0.0, 0.0, 0.0, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove1 = new stageMoveXYZ(g_stage, 0.0, 0.0, 0.0); 
	millctrl.SetCommand(stagemove1);

	stageGetPos *stagegetpos1 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos1);

	double x,y,z;
//	x=0.5;y= 0.0;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove2 = new stageMoveXYZ(g_stage,0.5, 0.0, 0.0); 
	millctrl.SetCommand(stagemove2);
	stageGetPos *stagegetpos2 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos2);
	
//	x=0.0;y= 0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove3 = new stageMoveXYZ(g_stage,0.0, 0.5, 0.0); 
	millctrl.SetCommand(stagemove3);
	stageGetPos *stagegetpos3 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos3);

//	x=-0.5;y= 0.0;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove4 = new stageMoveXYZ(g_stage,-0.5, 0.0, 0.0); 
	millctrl.SetCommand(stagemove4);
	stageGetPos *stagegetpos4 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos4);

//	x=0.0;y= -0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove5 = new stageMoveXYZ(g_stage,0.0, -0.5, 0.0); 
	millctrl.SetCommand(stagemove5);
	stageGetPos *stagegetpos5 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos5);

//	x=0.5;y= 0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove6 = new stageMoveXYZ(g_stage,0.5, 0.5, 0.0); 
	millctrl.SetCommand(stagemove6);
	stageGetPos *stagegetpos6 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos6);

//	x=-0.5;y= -0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove7 = new stageMoveXYZ(g_stage,-0.5, 0.5, 0.0); 
	millctrl.SetCommand(stagemove7);
	stageGetPos *stagegetpos7 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos7);
	
//	x=0.5;y= -0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove8 = new stageMoveXYZ(g_stage,0.5, -0.5, 0.0); 
	millctrl.SetCommand(stagemove8);
	stageGetPos *stagegetpos8 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos8);
	
//	x=-0.5;y= 0.5;z= 0.0;
//	iError=g_stage.MoveTo(x,y,z, true);
//	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	stageMoveXYZ *stagemove9 = new stageMoveXYZ(g_stage,-0.5, 0.5, 0.0); 
	millctrl.SetCommand(stagemove9);
	stageGetPos *stagegetpos9 = new stageGetPos(g_stage);
	millctrl.SetCommand(stagegetpos9);
	int numberOfCommands =millctrl.GetNumberOfCommands(); 
	for (int i=0; i <= numberOfCommands-1;i++)
	{
		millctrl.execute(i);

	}
//	iError=Servo.move(-8000,2);
//	do{
//		bMotion=Servo.IsInMotion();
//	}
//	while(bMotion);
//	g_stage.GetHome( dXHome, dYHome, dZHome);

	return 0;
}
コード例 #7
0
int TestPtToPtMotionInPCSWithStage()
{

//	NMCSERVO Servo;
	int iModules;
	int iError;
	long lPosition;
	double dXPosition,dYPosition,dZPosition;
	double dCmdXPosition,dCmdYPosition,dCmdZPosition;
	double dCmdXSpeed,dCmdYSpeed,dCmdZSpeed;
	double dCmdXAccel,dCmdYAccel,dCmdZAccel;
	double dVelocity;
	double dXHome, dYHome, dZHome;
	byte byAD;
	
	iModules=g_stage.Initialize("COM3:");

	SetScaling_3Axis();
	SetPIDGain_3Axis();

	g_stage.Rotate(180.0);

	g_stage.SetVel(3.0, 3.0, 3.0);
	g_stage.SetAccel(1.0, 1.0, 1.0);

	g_stage.EnableAmp();
	g_stage.ResetPos();
	
	g_stage.GetCmdVel(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
	g_stage.GetSpeed(dCmdXSpeed, dCmdYSpeed, dCmdZSpeed);
	g_stage.GetCmdAccel(dCmdXAccel, dCmdYAccel, dCmdZAccel );

	
	BOOL bMotion;
	iError=g_stage.MoveTo(0.0, 0.0, 0.0, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

	double x,y,z;
	x=0.5;y= 0.0;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	
	x=0.0;y= 0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

	x=-0.5;y= 0.0;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

	x=0.0;y= -0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

	x=0.5;y= 0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

	x=-0.5;y= -0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	
	x=0.5;y= -0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );
	
	x=-0.5;y= 0.5;z= 0.0;
	iError=g_stage.MoveTo(x,y,z, true);
	g_stage.GetPos(dXPosition, dYPosition, dZPosition );

//	iError=Servo.move(-8000,2);
//	do{
//		bMotion=Servo.IsInMotion();
//	}
//	while(bMotion);
//	g_stage.GetHome( dXHome, dYHome, dZHome);

	return 0;
}