frame1.c -- Simple multi-axis frame generated motion profile
/* frame1.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 multi-axis frame generated motion profile
The frame is the basic building block to generate trajectory profiles. Frames
can be calculated in the host computer or in the controller. Once downloaded
into the controller's memory, they are executed sequentially from a FIFO
buffer.
This sample demonstrates how to generate a simple multi-axis motion profile
using frames.
The frame structure consists of the following elements:
MPIXmpMotionType Mode; /* MPIXmpMotionTypePATH_OPEN will be used.
float t; /* Time in samples
long Position; /* Position in counts
float Velocity; /* Units are counts per second
float Accel; /* Units are counts per second^2
float Jerk;
long Control;
long Reserved;
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)
#define DISTANCE (10000)
#define SAMPLE_TIME (0.005)
#define TIME_SLICE (0.12) /* seconds */
#define ACCEL (2000)
#define POINT_COUNT (1000)
#define MAX_VEL (100)
/* 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, }
};
int
main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
MPIAxis axis[AXIS_COUNT]; /* axis handle(s) */
MPIMotion motion; /* motion handle */
MPIMotionParams params; /* MPI motion parameters */
MPIControlType controlType;
MPIControlAddress controlAddress;
MPINotify notify; /* Event notification handle */
MPIEventMgr eventMgr; /* Event manager handle */
MPIEventMask eventMask;
Service service; /* Event manager service handle */
MPIXmpFrame frame[AXIS_COUNT*POINT_COUNT];
long returnValue; /* return value from library */
long point_index;
double x, v, a;
double x_start[AXIS_COUNT];
long index;
long motionDone;
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[-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 object(s) */
for (index = 0; index < AXIS_COUNT; index++){
axis[index] =
mpiAxisCreate(control,
axisNumber[index]);
msgCHECK(mpiAxisValidate(axis[index]));
}
/* Create motion object */
motion =
mpiMotionCreate(control,
motionNumber,
MPIHandleVOID);
msgCHECK(mpiMotionValidate(motion));
returnValue =
mpiMotionAxisListSet(motion,
AXIS_COUNT,
axis);
msgCHECK(returnValue);
/* Get initial command positions */
for (index = 0; index < AXIS_COUNT; index++) {
returnValue =
mpiAxisCommandPositionGet(axis[index],
&x_start[index]);
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 parameters */
params.frame.frame = frame;
params.frame.point.retain = 1; /* don't flush frame buffer. */
params.frame.point.emptyCount = -1; /* start E-Stop if number of frames left
to execute is less than this limit. -1 disables */
/* first frame */
point_index = 0;
for (index = 0; index < AXIS_COUNT; index++){
frame[point_index].Type = MPIXmpFrameTypeSTART;
frame[point_index].t = 0.0;
frame[point_index].Position = 0;
frame[point_index].Velocity = 0.0;
frame[point_index].Accel = 0.0;
frame[point_index].Jerk = 0.0;
frame[point_index].Control = 0;
point_index++;
}
/* initial trajectory values */
x = 0.0;
v = 0.0;
a = ACCEL;
/* calculate the motion profile frames */
while (x < DISTANCE) {
for (index = 0; index < AXIS_COUNT; index++){
frame[point_index].Type = MPIXmpFrameTypeS_CURVE;
frame[point_index].t = TIME_SLICE / SAMPLE_TIME; /* samples */
frame[point_index].Position = (long)(x + x_start[index]);
frame[point_index].Velocity = (float)(v * SAMPLE_TIME);
frame[point_index].Accel = (float)(a * SAMPLE_TIME * SAMPLE_TIME);
frame[point_index].Jerk = 0.0;
frame[point_index].Control = 0;
point_index++;
}
if (v < MAX_VEL) {
a = ACCEL;
}
else {
a = 0.0;
}
x += v * TIME_SLICE + 0.5 * a * TIME_SLICE * TIME_SLICE;
v += a * TIME_SLICE;
}
/* calculate last frame to set final target conditions exactly */
for (index = 0; index < AXIS_COUNT; index++){
frame[point_index].Type = MPIXmpFrameTypeS_CURVE;
frame[point_index].t = 2; /* 2 time samples are usually put in this last frame */
frame[point_index].Position = (long)(x + x_start[index]);
frame[point_index].Velocity = 0.0;
frame[point_index].Accel = 0.0;
frame[point_index].Jerk = 0.0;
frame[point_index].Control = 0;
point_index++;
}
/* null frame (placeholder) */
for (index = 0; index < AXIS_COUNT; index++){
frame[point_index].Type = MPIXmpFrameTypeNONE;
frame[point_index].t = 0.0;
frame[point_index].Position = (long)(x + x_start[index]);
frame[point_index].Velocity = 0.0;
frame[point_index].Accel = 0.0;
frame[point_index].Jerk = 0.0;
frame[point_index].Control = 0;
point_index++;
}
params.frame.point.final = TRUE;
params.frame.pointCount = point_index; /* point_index is the total number of frames for all axes */
/* Start motion */
returnValue =
mpiMotionStart(motion,
(MPIMotionType)(MPIMotionTypeFRAME),
(MPIMotionParams*)(¶ms));
fprintf(stderr,
"mpiMotionStart 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);
msgCHECK(returnValue);
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 (index = 0; index < AXIS_COUNT; index++){
returnValue = mpiAxisDelete(axis[index]);
msgCHECK(returnValue);
}
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
|
