path1.c -- Two-axis path motion, around a rectangle with rounded
corners.
/* path1.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.
*/
/*
:Two-axis path motion, around a rectangle with rounded corners.
This sample demonstrates how to create a two-axis path motion.
The following procedure is used to create the path motion.
- Create the path object (done in programInit).
- The motion trajectory parameters are configured (pathConfig2d)
- Line and Arc elements are appended to the path (pathLine, pathArc)
- The MotionParams are generated (generatePath)
- 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
(eventCollectUntilMotionDone).
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 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 <math.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#define AXIS_COUNT (2) /* Don't change, this is a 2d example */
/* Set the vector values for Vel, Accel, Decel */
#define VELOCITY (5000.0)
#define ACCEL (50000.0)
#define DECEL (50000.0)
/*
Time slice determine how far apart the points are in time.
The practical range for the time slice parameter is 10 msec (0.01 s)
and greater. The time slice determines the spacing of points for the
internal algorithm interpolation. A time slice value that is too small
will result in points needing to be loaded more frequently from the host
(and associated risk of the host being late loading the points). A time
slice that is too large will result in rough motion and can cause radii
to be distorted.
*/
#define TIME_SLICE (0.01) /* seconds */
/* There must be this many points in the buffer, or the motion will E_STOP */
#define EMPTY_COUNT (5)
/* Only BSPLINE and BLSPLINE2 are supported for Path motion */
#define MOTION_TYPE MPIMotionTypeBSPLINE
#define TIMEOUT (10000) /* ms to wait for MOTION_DONE */
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, 1};
long motionNumber = 0;
Arg argList[] = {
{ "-axis1", ArgTypeLONG, &axisNumber[0], },
{ "-axis2", ArgTypeLONG, &axisNumber[1], },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ NULL, ArgTypeINVALID, NULL, }
};
/* Perform command line parsing. (-control -server -port -trace) */
void parsing(int argc,
char *argv[],
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 >= MPIXmpMAX_MSs)) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis1 # (0 .. %d)]\n"
"\t\t[-axis2 # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - AXIS_COUNT,
MPIXmpMAX_MSs - 1);
exit(MPIMessageARG_INVALID);
}
}
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIAxis axis[AXIS_COUNT],
MPIMotion *motion,
MPIEventMask eventMask,
MPINotify *notify,
MPIEventMgr *eventMgr,
MPIPath *path,
Service *service)
{
long returnValue;
/* 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);
*path = mpiPathCreate();
msgCHECK(mpiPathValidate(*path));
/* Create service thread */
*service =
serviceCreate(*eventMgr,
-1, /* default (max) priority */
-1); /* default sleep (msec) */
mpiAssert(service != NULL);
}
void deleteObjects(MPIPath path, Service service, MPIEventMgr eventMgr,
MPINotify notify, MPIMotion motion, MPIAxis axis[AXIS_COUNT],
MPIControl control)
{
long returnValue;
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);
}
/*
Ensure that the E_STOP will finish before the points are empty.
Only shorten the EStop time, don't lengthen it.
This is important if the point list has an underflow and the path
tries to EStop in response. If the EStop takes longer than the
amount of motion left in the buffer, violent motion can occur.
*/
void setEStop(MPIMotion motion, double timeSlice, long emptyCount)
{
MPIMotionConfig motionConfig;
long returnValue;
/* Read current motion supervisor configuration */
returnValue =
mpiMotionConfigGet(motion,
&motionConfig,
NULL);
msgCHECK(returnValue);
if((timeSlice * (double)emptyCount) < motionConfig.decelTime.eStop)
{
/* Set new E_STOP deceleration rates */
motionConfig.decelTime.eStop = (float)(timeSlice * (double)emptyCount);
returnValue =
mpiMotionConfigSet(motion,
&motionConfig,
NULL);
msgCHECK(returnValue);
}
}
void getCommandPositions2d(MPIAxis axis[AXIS_COUNT], double start[2])
{
long returnValue;
returnValue = mpiAxisCommandPositionGet(axis[0], &start[0]);
msgCHECK(returnValue);
returnValue = mpiAxisCommandPositionGet(axis[1], &start[1]);
msgCHECK(returnValue);
}
/*
Sets the configuration needed to set up a path.
start is the starting point for the path.
is start == NULL, the path will start from 0,0.
TimeSlice in seconds. MotionType is typically MPIMotionTypeBSPLINE
*/
void pathConfig2d(MPIPath path, double start[2],
double velocity, double accel,
MPIMotionType motionType, double timeSlice)
{
MPIPathParams params;
long returnValue;
/*Allow the user to pass a null start position and get zeroes */
if(start == NULL)
{
start[0] = 0.0;
start[1] = 0.0;
}
returnValue =
mpiPathParamsGet(path,
¶ms,
NULL);
msgCHECK(returnValue);
params.dimension = 2;
MPIPathPointX(params.start) = start[0];
MPIPathPointY(params.start) = start[1];
params.velocity = velocity;
params.acceleration = accel;
params.deceleration = accel;
params.interpolation = motionType;
params.timeSlice = timeSlice;
returnValue =
mpiPathParamsSet(path,
¶ms,
NULL);
msgCHECK(returnValue);
}
/* Convenient function for appending an arc to a path */
void pathArc(MPIPath path, MPIPathElement *element,
double start, double included, double radius)
{
long returnValue;
element->type = MPIPathElementTypeARC;
element->params.arc.angle.start = start;
element->params.arc.angle.included = included;
element->params.arc.radius = radius;
returnValue = mpiPathAppend(path,
element);
msgCHECK(returnValue);
}
/*
Convenient function for appending a line to a path.
start is a starting point for the move and x and y
are relative from that point.
If start == NULL, the x and y points are absolute.
*/
void pathLine(MPIPath path, MPIPathElement *element,
double x, double y, double start[2])
{
long returnValue;
/* Make a default of 0,0 start point for null startpoint */
if(start == NULL)
{
start[0] = 0.0;
start[1] = 0.0;
}
element->type = MPIPathElementTypeLINE;
MPIPathPointX(element->params.line.point) = x + start[0];
MPIPathPointY(element->params.line.point) = y + start[1];
returnValue = mpiPathAppend(path,
element);
msgCHECK(returnValue);
}
/* Sets a few values needed for path generation and generates the path */
void generatePath(MPIPath path, MPIMotionParams *motionParams,
long retain, long emptyCount, long finalPoint)
{
long returnValue;
/* Keep the points in memory, in case we need to backup on path */
motionParams->bspline.point.retain = retain;
/* Specify the minimum number of points required in
the XMP buffer -- if there are less, motion will E_STOP */
motionParams->bspline.point.emptyCount = emptyCount;
/* This motion will not be appended */
motionParams->bspline.point.final = finalPoint;
/* Generate motion parameters from the path object */
returnValue = mpiPathMotionParamsGenerate(path,
motionParams);
msgCHECK(returnValue);
}
/*
Collects events and prints them out. Event collection
ends when a motion done event is found.
This function waits for an event for timeout milliseconds.
A timeout value of -1 will make the function wait forever
for and event.
A timeout value of 0 will make the function not wait for
an event (falls through).
*/
void eventCollectUntilMotionDone(MPINotify notify, long timeout)
{
MPIEventStatus eventStatus;
long returnValue = MPIMessageOK;
/* Collect motion events */
while (returnValue == MPIMessageOK)
{
/* 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]);
}
}
}
void main(int argc, char *argv[])
{
MPIControl control;
MPIAxis axis[AXIS_COUNT];
MPIMotion motion;
MPINotify notify;
MPIEventMgr eventMgr;
MPIPath path;
MPIPathElement element;
MPIMotionParams motionParams;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIEventMask eventMask;
Service service;
long returnValue;
double start[2];
parsing(argc,
argv,
&controlType,
&controlAddress);
programInit(&control,
controlType,
&controlAddress,
axis,
&motion,
eventMask,
¬ify,
&eventMgr,
&path,
&service);
/*
Ensure that the E_STOP will finish before the points are empty.
Only shorten the EStop time, don't lengthen it
*/
setEStop(motion, TIME_SLICE, EMPTY_COUNT);
/* Get the command positions to offset the move */
getCommandPositions2d(axis, start);
/* Set path configurations before appending points */
pathConfig2d(path, start,
VELOCITY, ACCEL,
MOTION_TYPE, TIME_SLICE);
/*
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;
/* Append all the path segements */
pathLine(path, &element, 1800, 0, start);
pathArc(path, &element, 270.0, 90.0, 200.0);
pathLine(path, &element, 2000, 1800.0, start);
pathArc(path, &element, 0.0, 90.0, 200.0);
pathLine(path, &element, -1800.0, 2000.0, start);
pathArc(path, &element, 90.0, 90.0, 200.0);
pathLine(path, &element, -2000.0, 200.0, start);
pathArc(path, &element, 180.0, 90.0, 200.0);
pathLine(path, &element, 0, 0, start);
/* After appending the segments, they must be converted for motion */
generatePath(path, &motionParams, TRUE, EMPTY_COUNT, TRUE);
/* Start motion */
returnValue =
mpiMotionStart(motion,
MOTION_TYPE,
&motionParams);
msgCHECK(returnValue);
/* Prints events to screen until a motion done */
eventCollectUntilMotionDone(notify, TIMEOUT);
/* Clean up objects */
deleteObjects(path,
service,
eventMgr,
notify,
motion,
axis,
control);
}
|
