path1.c -- Two-axis path motion, around a rectangle with rounded corners.
/* path1.c */
/* Copyright(c) 1991-2002 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.
*/
/*
:Two-axis path motion, around a rectangle with rounded corners.
This sample demonstrates how to create a two-axis path motion.
After the Path object has been created, Line and Arc elements are
appended to the path. After the path elements have been appended,
the MotionParams are generated, and the motion is started using
mpiMotionStart(...).
When the motion has started, the program will wait for a MOTION_DONE event
to be distributed by the event manager in a separate service thread.
A summary of Path object structures and methods:
MPIPath -- path object handle
MPIPathConfig{} -- path config structure
MPIPathArc{} -- arc from start, angle and radius
MPIPathLine{} -- point in space
mpiPathCreate() -- create a path
mpiPathDelete() -- delete a path
mpiPathValidate() -- validate a path
mpiPathConfigGet() -- get the current path configuration
mpiPathConfigSet() -- set the path configuration
mpiPathAppend() -- add a path element to the path
mpiPathMotionParamsGenerate() -- generate the motion parameters
Note: When multiple axes are associated with a motion supervisor, the
controller automatically combines the individual axis and motor status
into the motion status. Thus, if a Stop, E-Stop or Abort action occurs
on one axis, the event will be propogated automatically to the other axes.
Warning! This is a sample program to assist in the integration of the
XMP 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 <math.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#define AXIS_COUNT (2)
/* Set the vector values for Vel, Accel, Decel */
#define VELOCITY (5000.0)
#define ACCEL (50000.0)
#define DECEL (50000.0)
/* The practical range for the time slice parameter is from
10 msec (0.01) to 100 msec (0.1). The time slice determines
the spacing of points for the internal algorithm interpolation */
#define TIME_SLICE (0.050) /* seconds */
/* There must be this many points in the buffer, or the motion
will E_STOP */
#define EMPTY_COUNT (5)
/* Ensure that the E_STOP will finish before the points are empty */
#define E_STOP_RATE (TIME_SLICE * EMPTY_COUNT)
/* Only BSPLINE and BLSPLINE2 are supported for Path motion */
#define MOTION_TYPE MPIMotionTypeBSPLINE
#define TIMEOUT (10000) /* ms to wait for MOTION_DONE */
#if defined(ARG_MAIN_RENAME)
#define main path1Main
argMainRENAME(main, path1)
#endif
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, 1, };
long motionNumber = 0;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ NULL, ArgTypeINVALID, NULL, }
};
void main(int argc, char *argv[])
{
MPIControl control; /* motion controller handle */
MPIAxis axis[AXIS_COUNT]; /* axis handle(s) */
MPIMotion motion; /* motion handle */
MPINotify notify; /* event notification handle */
MPIEventMgr eventMgr; /* event manager handle */
MPIMotionConfig motionConfig;
MPIPath path;
MPIPathConfig pathConfig;
MPIPathElement element;
MPIMotionParams motionParams;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIEventMask eventMask;
Service service; /* service handle */
long returnValue; /* return value from library */
long argIndex;
double x_start;
double y_start;
/* 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] > (MEIXmpMAX_Axes - AXIS_COUNT)) ||
(motionNumber >= MEIXmpMAX_MSs)) {
meiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MEIXmpMAX_Axes - AXIS_COUNT,
MEIXmpMAX_MSs - 1);
exit(MPIMessageARG_INVALID);
}
axisNumber[1] = axisNumber[0] + 1;
/* Create motion controller object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize the controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
/* Create axis objects */
axis[0] =
mpiAxisCreate(control,
axisNumber[0]);
msgCHECK(mpiAxisValidate(axis[0]));
axis[1] =
mpiAxisCreate(control,
axisNumber[1]);
msgCHECK(mpiAxisValidate(axis[1]));
/* Create motion supervisor object using MS number 0 */
motion =
mpiMotionCreate(control,
motionNumber,
MPIHandleVOID);
msgCHECK(mpiMotionValidate(motion));
/* Create 2-axis motion coordinate system */
returnValue =
mpiMotionAxisListSet(motion,
AXIS_COUNT,
axis);
msgCHECK(returnValue);
/* Request notification of all MPI events from motion */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskALL(eventMask);
/* Request notification of all events from motion */
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) */
meiASSERT(service != NULL);
/* Read current motion supervisor configuration */
returnValue =
mpiMotionConfigGet(motion,
&motionConfig,
NULL);
msgCHECK(returnValue);
/* Set new E_STOP deceleration rates */
motionConfig.decelTime.eStop = E_STOP_RATE;
returnValue =
mpiMotionConfigSet(motion,
&motionConfig,
NULL);
msgCHECK(returnValue);
returnValue = mpiAxisCommandPositionGet(axis[0], &x_start);
msgCHECK(returnValue);
returnValue = mpiAxisCommandPositionGet(axis[1], &y_start);
msgCHECK(returnValue);
path = mpiPathCreate();
msgCHECK(mpiPathValidate(path));
returnValue = mpiPathConfigGet(path,
&pathConfig,
NULL);
msgCHECK(returnValue);
pathConfig.dimension = AXIS_COUNT;
MPIPathPointX(pathConfig.start) = x_start;
MPIPathPointY(pathConfig.start) = y_start;
pathConfig.velocity = VELOCITY;
pathConfig.acceleration = ACCEL;
pathConfig.deceleration = DECEL;
pathConfig.interpolation = MOTION_TYPE;
pathConfig.timeSlice = TIME_SLICE;
returnValue = mpiPathConfigSet(path,
&pathConfig,
NULL);
msgCHECK(returnValue);
/* Start appending elements to the path */
element.type = MPIPathElementTypeLINE;
/* Set blending to TRUE if the corners of the path
should be rounded. Set to FALSE if the corners
should be sharp. The motion will stop at corners when
blending is FALSE */
element.blending = TRUE;
MPIPathPointX(element.params.line.point) = 1800.0;
MPIPathPointY(element.params.line.point) = 0.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeARC;
element.params.arc.angle.start = 270.0;
/* Included angle is CCW when positive, CW when negative */
element.params.arc.angle.included = 90.0;
element.params.arc.radius = 200.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeLINE;
MPIPathPointX(element.params.line.point) = 2000.0;
MPIPathPointY(element.params.line.point) = 1800.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeARC;
element.params.arc.angle.start = 0.0;
element.params.arc.angle.included = 90.0;
element.params.arc.radius = 200.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeLINE;
MPIPathPointX(element.params.line.point) = -1800.0;
MPIPathPointY(element.params.line.point) = 2000.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeARC;
element.params.arc.angle.start = 90.0;
element.params.arc.angle.included = 90.0;
element.params.arc.radius = 200.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeLINE;
MPIPathPointX(element.params.line.point) = -2000.0;
MPIPathPointY(element.params.line.point) = 200.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeARC;
element.params.arc.angle.start = 180.0;
element.params.arc.angle.included = 90.0;
element.params.arc.radius = 200.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
element.type = MPIPathElementTypeLINE;
MPIPathPointX(element.params.line.point) = 0.0;
MPIPathPointY(element.params.line.point) = 0.0;
returnValue = mpiPathAppend(path,
&element);
msgCHECK(returnValue);
/* Finished appending elements to the path */
/* Generate motion parameters from the path object */
returnValue = mpiPathMotionParamsGenerate(path,
&motionParams);
msgCHECK(returnValue);
/* Keep the points in memory, in case we need to backup on path */
motionParams.bspline.point.retain = TRUE;
/* Specify the minimum number of points required in
the XMP buffer -- if there are less, motion will E_STOP */
motionParams.bspline.point.emptyCount = EMPTY_COUNT;
/* This motion will not be appended */
motionParams.bspline.point.final = TRUE;
/* Start motion */
returnValue =
mpiMotionStart(motion,
MOTION_TYPE,
&motionParams);
fprintf(stderr,
"mpiMotionStart returns 0x%x: %s\n",
returnValue,
mpiMessage(returnValue, NULL));
/* Collect motion events */
while (returnValue == MPIMessageOK) {
MPIEventStatus eventStatus;
/* Wait for event */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
TIMEOUT);
msgCHECK(returnValue);
if (eventStatus.type == MPIEventTypeMOTION_DONE) {
printf("Motion Done\n");
break;
}
else {
fprintf(stderr,
"mpiNotifyEventWait(0x%x, 0x%x, %d) returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
notify,
&eventStatus,
MPIWaitFOREVER,
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
}
}
fprintf(stderr,
"%s exiting: returnValue 0x%x: %s\n",
argv[0],
returnValue,
mpiMessage(returnValue, NULL));
returnValue = mpiPathDelete(path);
msgCHECK(returnValue);
returnValue = serviceDelete(service);
msgCHECK(returnValue);
returnValue = mpiEventMgrDelete(eventMgr);
msgCHECK(returnValue);
returnValue = mpiNotifyDelete(notify);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis[0]);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis[1]);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
}