int InitNMCModule3AxisStatic (int & iModules, Stage & stage, Path & path)
{
iModules=stage.Initialize("COM4:");
if (iModules < 3)
{
MessageBox(NULL,"3 servos not found","",MB_TASKMODAL | MB_SETFOREGROUND);
return -1;
}
stage.Rotate(180.0);
//Set the group address for both controllers to 128
stage.SetGroupAddress(128, eXAxis);
SetScaling_3AxisStatic(stage);
SetPIDGain_3AxisStatic(stage);
ServoOnNMCModule3AxisStatic(stage);
//Set the required status items for the path control module
stage.SetPathStatus();
// ServoSetIoCtrl(1, IO1_IN | IO2_IN);
// ServoSetIoCtrl(2, IO1_IN | IO2_IN);
// ServoSetIoCtrl(3, IO1_IN | IO2_IN);
return 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;
}
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;
}
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;
}
int main(){
Stage stage;
stage.Initialize();
stage.PerformProfilerTest();
}