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