motMap1.c -- Motion object and Motion Supervisor axis map configuration.
/* motMap1.c */
/* Copyright(c) 1991-2006 by Motion Engineering, Inc. All rights reserved.
*
* This software contains proprietary and confidential information of
* Motion Engineering Inc., and its suppliers. Except as may be set forth
* in the license agreement under which this software is supplied, use,
* disclosure, or reproduction is prohibited without the prior express
* written consent of Motion Engineering, Inc.
*/
/*
:Motion object and Motion Supervisor axis map configuration.
The MPI supports Motion objects and Axis objects which reside in the
host computer. Several Motion objects can be created and associated
with single or multiple Axis objects. Additionally, several Motion
objects can be configured to share one or more Axis objects.
The XMP controller has Motion Supervisors and Axes. The controller's
Motion Supervisors can be associated with a single axis or multiple axes.
Additionally, several Motion Supervisors can be configured to share
one or more axes.
When multiple axes are configured for a Motion Supervisor, the controller
automatically manages the status and error conditions. For example, if
a Motion Supervisor is configured with two axes, and one of the axes faults
with a position error limit, causing an Abort Event, the Motion Supervisor
will generate Abort Event on the other axis.
Some care must be used with Motion Supervisor axis map configurations when
axes are shared between Motion Supervisors. Since the Error conditions
are automatically proliferated across mapped axes, it is possible to create
Motion Supervisor axis maps that generate Error conditions that cannot be
cleared. For example, if MS#0 is mapped to axes 0 and 1, and MS#1 is mapped
to axis 0. MS#1 will NOT be able to clear Error conditions. Only MS#0 will
be able to clear Error conditions on axes 0 and 1.
Motion Supervisor axis maps are set with any of the following methods:
1) mpiMotionAction(...) with the MPIActionMAP or MPIActionRESET
2) mpiMotionStart(...)
3) mpiMotionModify(...)
4) mpiMotionEventNotifySet(...)
Warning! This is a sample program to assist in the integration of an
MEI motion controller with your application. It may not contain all
of the logic and safety features that your application requires.
*/
#include <stdlib.h>
#include <stdio.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#if defined(ARG_MAIN_RENAME)
#define main motMap1Main
argMainRENAME(main, motMap1)
#endif
#define MOTION_COUNT (2)
#define AXIS_COUNT (2)
#define MS_COUNT (4)
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, 1, };
long motionNumber[MS_COUNT] = { 0, 1, 2, 3, };
MPIMotionType motionType = MPIMotionTypeS_CURVE;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionNumber[0], },
{ "-type", ArgTypeLONG, &motionType, },
{ NULL, ArgTypeINVALID, NULL, }
};
double position[MOTION_COUNT][AXIS_COUNT] = {
{ 10000.0, 20000.0, },
{ 0.0, 0.0, },
};
MPITrajectory trajectory[MOTION_COUNT][AXIS_COUNT] = {
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
};
/* Motion parameters */
MPIMotionSCurve sCurve[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
};
MPIMotionTrapezoidal trapezoidal[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
};
MPIMotionVelocity velocity[MOTION_COUNT] = {
{ &trajectory[0][0], },
{ &trajectory[1][0], },
};
long
motionDoneWait(MPIMotion motion);
long
motionIdle(MPIMotion motion,
MPIStatus *status);
int
main(int argc,
char *argv[])
{
MPIControl control; /* Motion controller handle */
MPIAxis axisX; /* X axis */
MPIAxis axisY; /* Y axis */
MPIMotion motion; /* Motion object for no axes */
MPIMotion motionX; /* Motion object for axisX */
MPIMotion motionY; /* Motion object for axisY */
MPIMotion motionXY; /* Motion object for axisX and axisY */
enum {
Motion,
MotionX,
MotionY,
MotionXY,
};
long index;
long returnValue; /* Return value from library */
MPIControlType controlType;
MPIControlAddress controlAddress;
long argIndex;
/* Parse command line for Control type and address */
argIndex =
argControl(argc,
argv,
&controlType,
&controlAddress);
/* Parse command line for application-specific arguments */
while (argIndex < argc) {
long argIndexNew;
argIndexNew = argSet(argList, argIndex, argc, argv);
if (argIndexNew <= argIndex) {
argIndex = argIndexNew;
break;
}
else {
argIndex = argIndexNew;
}
}
/* Check for unknown/invalid command line arguments */
if ((argIndex < argc) ||
(axisNumber[0] > (MPIXmpMAX_Axes - AXIS_COUNT)) ||
(motionNumber[0] > (MPIXmpMAX_MSs - MS_COUNT)) ||
(motionType < MPIMotionTypeFIRST) ||
(motionType >= MPIMotionTypeLAST)) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n"
"\t\t[-type # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - AXIS_COUNT,
MPIXmpMAX_MSs - MS_COUNT,
MPIMotionTypeLAST - 1);
exit(MPIMessageARG_INVALID);
}
switch (motionType) {
case MPIMotionTypeS_CURVE:
case MPIMotionTypeTRAPEZOIDAL:
case MPIMotionTypeVELOCITY: {
break;
}
default: {
mpiPlatformConsole("%s: %d: motion type not available\n",
argv[0],
motionType);
exit(MPIMessageUNSUPPORTED);
break;
}
}
axisNumber[1] = axisNumber[0] + 1;
motionNumber[1] = motionNumber[0] + 1;
motionNumber[2] = motionNumber[1] + 1;
motionNumber[3] = motionNumber[2] + 1;
/* Create control object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize motion controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
/* Create X axis object using AXIS_X number on controller */
axisX =
mpiAxisCreate(control,
axisNumber[0]); /* Axis Number */
msgCHECK(mpiAxisValidate(axisX));
/* Create Y axis object using AXIS_Y number on controller */
axisY =
mpiAxisCreate(control,
axisNumber[1]); /* Axis Number */
msgCHECK(mpiAxisValidate(axisY));
/* Create Motion object with NO axes */
motion =
mpiMotionCreate(control,
motionNumber[MotionXY], /* Motion supervisor number */
NULL);
msgCHECK(mpiMotionValidate(motion));
/* Create Motion object for axisX */
motionX =
mpiMotionCreate(control,
motionNumber[MotionX], /* Motion supervisor number */
axisX);
msgCHECK(mpiMotionValidate(motionX));
/* Create Motion object for axisY */
motionY =
mpiMotionCreate(control,
motionNumber[MotionY], /* Motion supervisor number */
axisY);
msgCHECK(mpiMotionValidate(motionY));
/* Motion object for axisX and axisY */
motionXY =
mpiMotionCreate(control,
motionNumber[MotionXY], /* Motion supervisor number */
axisX);
msgCHECK(mpiMotionValidate(motionXY));
/* Append axisY to motionXY */
returnValue =
mpiMotionAxisAppend(motionXY,
axisY);
msgCHECK(returnValue);
/* Map X axis to Motion Supervisor */
returnValue =
mpiMotionAction(motionX, /* Axis X, using MotionX */
(MPIAction)MPIActionMAP); /* Map axis to Motion Supervisor */
msgCHECK(returnValue);
/* Map Y axis to Motion Supervisor */
returnValue =
mpiMotionAction(motionY, /* Axis Y, using MotionY */
(MPIAction)MPIActionMAP); /* Map axis to Motion Supervisor */
msgCHECK(returnValue);
while (mpiPlatformKey(MPIWaitPOLL) <= 0) {
MPIMotionParams motionParams;
/* Clear any error conditions on axisX */
returnValue =
mpiMotionAction(motionX,
(MPIAction)MPIActionRESET);
msgCHECK(returnValue);
index = 0;
/* Move axisX to position #1 */
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[index];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[index];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[index];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
printf("\nMoving X axis...\n");
returnValue =
mpiMotionStart(motionX,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motionX,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
returnValue = motionDoneWait(motionX);
msgCHECK(returnValue);
/* Clear any error conditions on axisY */
returnValue =
mpiMotionAction(motionY,
MPIActionRESET);
msgCHECK(returnValue);
/* Move axisY to position #1 */
printf("\nMoving Y axis...\n");
returnValue =
mpiMotionStart(motionY,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motionY,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
returnValue = motionDoneWait(motionY);
msgCHECK(returnValue);
/* Configure Motion Supervisor Map for X,Y */
returnValue =
mpiMotionAction(motionXY, /* Axes X and Y, using MotionXY */
(MPIAction)MPIActionMAP); /* Map axes to Motion Supervisor */
msgCHECK(returnValue);
/* Clear any error conditions on AxisX and AxisY */
returnValue =
mpiMotionAction(motionXY,
MPIActionRESET);
msgCHECK(returnValue);
index = 1;
/* Move axisX and axisY to position #2 */
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[index];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[index];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[index];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
printf("\nMoving X and Y axes...\n");
returnValue =
mpiMotionStart(motionXY,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motionXY,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
returnValue = motionDoneWait(motionXY);
msgCHECK(returnValue);
/* Un-configure the controller's XY Motion Supervisor */
returnValue =
mpiMotionAction(motion, /* Motion object has NO axes */
(MPIAction)MPIActionMAP); /* Set a Motion Supervisor map of no axes */
msgCHECK(returnValue);
}
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motionX);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motionY);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motionXY);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisY);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisX);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
long
motionDoneWait(MPIMotion motion)
{
MPIStatus status;
long returnValue;
long motionDone = FALSE;
/* Poll status until motion done */
while (motionDone == FALSE)
{
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state)
{
case MPIStateIDLE:
case MPIStateSTOPPED:
{
motionDone = TRUE;
returnValue =
motionIdle(motion,
&status);
mpiPlatformSleep(300); /* msec */
break;
}
case MPIStateERROR:
{
motionDone = TRUE;
printf("Motion State Error!\n");
break;
}
default:
{
break;
}
}
}
return (returnValue);
}
long
motionIdle(MPIMotion motion,
MPIStatus *status)
{
long returnValue;
double actual[AXIS_COUNT];
double command[AXIS_COUNT];
MPITrajectory trajectory[AXIS_COUNT];
long index;
printf("MotionDone: status: state %d action %d eventMask 0x%x\n"
"\tatTarget %d settled %d %s\n",
status->state,
status->action,
status->eventMask,
status->atTarget,
status->settled,
(status->settled == FALSE)
? "=== NOT SETTLED ==="
: "");
returnValue =
mpiMotionPositionGet(motion,
actual,
command);
msgCHECK(returnValue);
returnValue =
mpiMotionTrajectory(motion,
trajectory);
msgCHECK(returnValue);
/* Display axis positions */
for (index = 0; index < AXIS_COUNT; index++) {
printf("\taxis[%d] position: command %11.3lf\tactual %11.3lf\n",
index,
command[index],
actual[index]);
printf("\t\ttrajectory: velocity %11.3lf\taccel %11.3lf\tjerk %11.3lf\n",
trajectory[index].velocity,
trajectory[index].acceleration,
trajectory[index].jerkPercent);
}
return (returnValue);
}
|
