ptappend.c -- Simple motion path generation, using position/time
points with Motion Append
/* ptappend.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.
*/
/*
:Simple motion path generation, using position/time points with Motion Append
This sample code demonstrates the motion type, MPIMotionTypePT, with Motion
Append. A simple trapezoidal velocity profile points list is created, and
downloaded to the controller. The time delta between each position is
constant and the positions are spaced to generate an acceleration, constant
velocity, and deceleration profile to the final position.
Several motion parameters must be initialized for the PT motion type:
params.pt.pointCount - Specifies the number of points (position/time).
params.pt.position - Pointer to a position[...] array. There is one
position value per point, per axis. The length of the array must be
equal to pointCount multiplied by the number of axes. The positions
are interleaved in the array by the axis index.
For example, a three axis system would have:
position[0] = position for axis 0
position[1] = position for axis 1
position[2] = position for axis 2
position[3] = position for axis 0
position[4] = position for axis 1
etc.
params.pt.time - Pointer to a time[...] array. There is one time value
per point. The time specifies the number of seconds between the previous
position (point) and the specified position. The length of the
time array must be equal to the pointCount.
params.pt.point.retain - Specifies whether the MPI and XMP should buffer the
points after they have executed. If retain = 0, the buffer will destroy
the points after they have executed. If retain = 1, the buffer will keep the
points after they have executed. This parameter is useful for future backup
on path capability.
params.pt.point.final - Specifies if more points will be loaded. If final = 1,
no more points will be loaded. If final = 0, more points can be loaded
with mpiMotionModify(...) using the APPEND attribute.
params.pt.point.emptyCount - Specifies how many points the XMP-Series
controller must have in it's buffer. If the XMP's point buffer falls
below the emptyCount, an E-Stop event will be generated by the motion
supervisor, decelerating all the associated axes to a stop.
When the points are passed to mpiMotionStart(...), the MPI calculates
constant velocity segments to fit exactly through the specified positions at
the specified times. The MPI library automatically handles buffering points
lists that are larger than the XMP's buffer. Presently, the XMP-Series
controller can store 128 Frames per axis. Each point requires one Frame
per axis.
Special Considerations:
It is not necesary to specify the initial command position as the first
position point. The time[...] values can be constant or vary for each point.
Constant time[...] values make the programmer's calculations easier. For
optimum performance, the number of points can be reduced, with no loss of
resolution, by replacing the constant velocity sections with longer
time[...] values.
When creating points lists longer than 64 points, you must create an Event
Manager. The Event Manager handles point buffering between the MPI and the XMP
controller.
For Acceleration/Jerk fits through a points list, use the motion type,
MPIMotionTypePVT. In this case, position/velocity/time points are specified.
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 ptappendMain
argMainRENAME(main, ptappend)
#endif
#define AXIS_COUNT (1)
#define POINT_COUNT (36) /* Number of points in trajectory */
#define TIME (0.1) /* Time between points (seconds) */
#define POSITION_DELTA (10.0) /* counts for each point */
double position[AXIS_COUNT * POINT_COUNT];
double time[POINT_COUNT];
double position2[AXIS_COUNT * POINT_COUNT];
double time2[POINT_COUNT];
/* Simple trapezoidal profile velocity path */
double point[POINT_COUNT + 1];
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, };
long motionNumber = 0;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ NULL, ArgTypeINVALID, NULL, }
};
int
main(int argc,
char *argv[])
{
MPIControl control; /* Control object */
MPIAxis axis[MPIXmpMAX_Axes]; /* Array of axis objects */
MPIMotion motion; /* Motion object */
MPINotify notify; /* Event notification handle */
MPIEventMgr eventMgr; /* Event manager handle */
MPIMotionParams params; /* MPI motion parameters */
MPIMotionParams params2; /* MPI motion parameters */
Service service; /* Event manager service handle */
long returnValue;
MPIControlType controlType;
MPIControlAddress controlAddress;
long argIndex;
long pointIndex;
long axisIndex;
MPI_BOOL motionDone;
MPIEventMask eventMask;
double initialPosition[MPIXmpMAX_Axes];
double pos = 0.0;
/* 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 >= MPIXmpMAX_MSs)) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - AXIS_COUNT,
MPIXmpMAX_MSs - 1);
exit(MPIMessageARG_INVALID);
}
/* Obtain a Control handle */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize the controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
/* Create Axis objects */
for (axisIndex = 0; axisIndex < AXIS_COUNT; axisIndex++) {
axis[axisIndex] =
mpiAxisCreate(control,
axisNumber[axisIndex]);
msgCHECK(mpiAxisValidate(axis[axisIndex]));
}
/* Create motion object */
motion =
mpiMotionCreate(control,
motionNumber,
NULL);
msgCHECK(mpiMotionValidate(motion));
/* Append axis objects to motion object */
for (axisIndex = 0; axisIndex < AXIS_COUNT; axisIndex++) {
returnValue =
mpiMotionAxisAppend(motion,
axis[axisIndex]);
msgCHECK(returnValue);
/* Read initial command position */
returnValue =
mpiAxisCommandPositionGet(axis[axisIndex],
&initialPosition[axisIndex]);
msgCHECK(returnValue);
}
/* Request notification of all events from motion */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskALL(eventMask);
returnValue =
mpiMotionEventNotifySet(motion,
eventMask,
NULL);
msgCHECK(returnValue);
/* Create event notification object for motion */
notify =
mpiNotifyCreate(eventMask,
motion);
msgCHECK(mpiNotifyValidate(notify));
/* Create event manager object */
eventMgr = mpiEventMgrCreate(control);
msgCHECK(mpiEventMgrValidate(eventMgr));
/* Add notify to event manager's list */
returnValue =
mpiEventMgrNotifyAppend(eventMgr,
notify);
msgCHECK(returnValue);
/* Create service thread */
service =
serviceCreate(eventMgr,
-1, /* Default (max) priority */
-1); /* Default sleep (msec) */
mpiAssert(service != NULL);
/* Initialize motion params structure */
params.pt.pointCount = POINT_COUNT;
params.pt.position = position;
params.pt.time = time;
params.pt.point.retain = TRUE; /* Flush frame buffer after execution */
params.pt.point.final = 0; /* Last point */
params.pt.point.emptyCount = 2; /* Start E-Stop if frames left to execute
is less than this limit. -1 disables */
/* Initialize second motion params structure */
params2.pt.pointCount = POINT_COUNT;
params2.pt.position = position2;
params2.pt.time = time2;
params2.pt.point.retain = TRUE; /* Flush frame buffer after execution */
params2.pt.point.final = 1; /* Last point */
params2.pt.point.emptyCount = 5; /* Start E-Stop if frames left to execute
is less than this limit. -1 disables */
for(pointIndex = 0; pointIndex < (POINT_COUNT + 1); pointIndex++)
{
point[pointIndex] = pos;
pos += (double) POSITION_DELTA;
}
/* Create points */
for (pointIndex = 0; pointIndex < POINT_COUNT; pointIndex++) {
time[pointIndex] = TIME; /* Delta time between points */
for (axisIndex = 0; axisIndex < AXIS_COUNT; axisIndex++) {
/* Position at point */
position[(pointIndex * AXIS_COUNT) + axisIndex] =
point[(pointIndex + 1)] + initialPosition[axisIndex];
}
}
/* Create secondary points */
for (pointIndex = 0; pointIndex < POINT_COUNT; pointIndex++) {
time2[pointIndex] = TIME; /* Delta time between points */
for (axisIndex = 0; axisIndex < AXIS_COUNT; axisIndex++) {
/* Position at point */
position2[(pointIndex * AXIS_COUNT) + axisIndex] =
(point[(pointIndex + 1)] + point[POINT_COUNT]) + initialPosition[axisIndex];
}
}
/* Start motion */
returnValue =
mpiMotionStart(motion,
MPIMotionTypePT,
¶ms);
fprintf(stderr,
"mpiMotionStart returns 0x%x: %s\n",
returnValue,
mpiMessage(returnValue, NULL));
msgCHECK(returnValue);
/* Modify motion */
returnValue =
mpiMotionModify(motion,
MPIMotionTypePT | MPIMotionAttrMaskAPPEND,
¶ms2);
fprintf(stderr,
"mpiMotionModify returns 0x%x: %s\n",
returnValue,
mpiMessage(returnValue, NULL));
msgCHECK(returnValue);
/* Collect motion events */
motionDone = FALSE;
while (motionDone != TRUE) {
MPIEventStatus eventStatus;
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
switch(eventStatus.type) {
case MPIEventTypeMOTION_DONE: {
motionDone = TRUE;
break;
}
default: {
break;
}
}
fprintf(stderr,
"mpiNotifyEventWait() returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
msgCHECK(returnValue);
}
/* Delete Objects */
returnValue = serviceDelete(service);
msgCHECK(returnValue);
returnValue = mpiEventMgrDelete(eventMgr);
msgCHECK(returnValue);
returnValue = mpiNotifyDelete(notify);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
for (axisIndex = 0; axisIndex < AXIS_COUNT; axisIndex++) {
returnValue = mpiAxisDelete(axis[axisIndex]);
msgCHECK(returnValue);
}
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
|
