.

     

MPI Application Template
template.c
 

path1_3d.c -- Three-axis path motion with velocities, acceleration and timeSlices
/* path1_3D.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.
 */

/*

:Three-axis path motion with velocities, aceleration and timeSlices
defined for each element.

This sample demonstrates how to create a three-axis path motion together wth
different velocities, aceleration and timeSlices for different elements.

The code draws 2 rectangles. The first, larger rectangle has different velocities
and timeSlices, and different z elevations. The second, smaller one is simply
in the xy plane (z = 0) and has one corner with the direction of the arc
value = -1 which means that the motion will be around 270 degrees and not 90.
Even though the second rectangle is 2D, it must have values for z as it is inadvisable
to mix 2D and 3D paths.

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
    MPIPathParams{} -- path Params structure
    MPIPathArc{} -- arc from start, angle and radius
    MPIPathLine{} -- point in space

    mpiPathCreate() -- create a path
    mpiPathDelete() -- delete a path
    mpiPathValidate() -- validate a path
    mpiPathParamsGet() -- get the current path parameters
    mpiPathParamsSet() -- set the path parameters
    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          (3)

/* 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             (100000) /* ms to wait for MOTION_DONE */

/* Command line arguments and defaults */
long            axisNumber[AXIS_COUNT] = { 0, 1, 2};
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;
    MPIPathParams       pathParams;
    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;
    double              z_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;
    axisNumber[2] = axisNumber[1] + 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]));

    axis[2] =
        mpiAxisCreate(control,
                      axisNumber[2]);
    msgCHECK(mpiAxisValidate(axis[2]));

    /* 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 Configurationuration */
    returnValue =
        mpiMotionConfigGet(motion,
                           &motionConfig,
                           NULL);
    msgCHECK(returnValue);

    /* Set new E_STOP deceleration rates */
    motionConfig.decelTime.eStop = (float)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);
    returnValue = mpiAxisCommandPositionGet(axis[2], &z_start);
    msgCHECK(returnValue);

    path = mpiPathCreate();
    msgCHECK(mpiPathValidate(path));

    returnValue = mpiPathParamsGet(path,
                                    &pathParams,
                                    NULL);
    msgCHECK(returnValue);

    pathParams.dimension = AXIS_COUNT;
    MPIPathPointX(pathParams.start) = x_start;
    MPIPathPointY(pathParams.start) = y_start;
    MPIPathPointZ(pathParams.start) = z_start;
    pathParams.velocity = VELOCITY;
    pathParams.acceleration = ACCEL;
    pathParams.deceleration = DECEL;
    pathParams.interpolation = MOTION_TYPE;
    pathParams.timeSlice = TIME_SLICE;

    returnValue = mpiPathParamsSet(path,
                                    &pathParams,
                                    NULL);
    msgCHECK(returnValue);


    /* Start appending elements to the path */

    /* In this element, the path parameters will be used */
    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;
    MPIPathPointZ(element.params.line.point) = 0.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    /*
        The "| MPIPathElementAttrMaskVELOCITY" in the next line states that
        the element has it's own velocity.
    */
    element.type = MPIPathElementTypeARC_END_POINT | MPIPathElementAttrMaskVELOCITY;

    MPIPathPointX(element.params.arcEndPoint.center) = 1800;
    MPIPathPointY(element.params.arcEndPoint.center) = 200;
    MPIPathPointZ(element.params.arcEndPoint.center) = 50;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = 2000;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = 200;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 100;

    element.attributes.velocity = 10 * VELOCITY;

    /*
        The 'direction' is used to describe whether the motion should be around the large,
        or small angle between the start and stop points. direction = 1 means that
        the motion will be through less than 180 degrees, while direction = -1 means that
        the motion will be through more than 180 degrees.
    */
    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    /*
        The "| MPIPathElementAttrMaskTIMESLICE" in the next line states that
        the element has it's own velocity.
    */
    element.type = MPIPathElementTypeLINE | MPIPathElementAttrMaskTIMESLICE;
    MPIPathPointX(element.params.line.point) = 2000.0;
    MPIPathPointY(element.params.line.point) = 1800.0;
    MPIPathPointZ(element.params.line.point) = 100.0;

    element.attributes.timeSlice = 0.1 * TIME_SLICE;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    /* In this element, the path parameters will be used */
    element.type = MPIPathElementTypeARC_END_POINT;
    MPIPathPointX(element.params.arcEndPoint.center) = 1800;
    MPIPathPointY(element.params.arcEndPoint.center) = 1800;
    MPIPathPointZ(element.params.arcEndPoint.center) = 50;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = 1800;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = 2000;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 0;


    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    element.type = MPIPathElementTypeLINE ;
    MPIPathPointX(element.params.line.point) = -1800.0;
    MPIPathPointY(element.params.line.point) = 2000.0;
    MPIPathPointZ(element.params.line.point) = 0.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    element.type = MPIPathElementTypeARC_END_POINT;
    MPIPathPointX(element.params.arcEndPoint.center) = -1800;
    MPIPathPointY(element.params.arcEndPoint.center) = 1800;
    MPIPathPointZ(element.params.arcEndPoint.center) = -100;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = -2000;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = 1800;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = -200;

    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    element.type = MPIPathElementTypeLINE;
    MPIPathPointX(element.params.line.point) = -2000.0;
    MPIPathPointY(element.params.line.point) = 200.0;
    MPIPathPointZ(element.params.line.point) = -200.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    element.type = MPIPathElementTypeARC_END_POINT;
    MPIPathPointX(element.params.arcEndPoint.center) = -1800;
    MPIPathPointY(element.params.arcEndPoint.center) = 200;
    MPIPathPointZ(element.params.arcEndPoint.center) = -100;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = -1800;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = 0;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 0;

    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    element.type = MPIPathElementTypeLINE;
    MPIPathPointX(element.params.line.point) = 0.0;
    MPIPathPointY(element.params.line.point) = 0.0;
    MPIPathPointZ(element.params.line.point) = 0.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    element.type = MPIPathElementTypeLINE ;
    MPIPathPointX(element.params.line.point) = 0.0;
    MPIPathPointY(element.params.line.point) = -1800.0;
    MPIPathPointZ(element.params.line.point) = 0.0;


    returnValue = mpiPathAppend(path,
                                &element);

    element.type = MPIPathElementTypeARC_END_POINT ;
    MPIPathPointX(element.params.arcEndPoint.center) = -200;
    MPIPathPointY(element.params.arcEndPoint.center) = -1800;
    MPIPathPointZ(element.params.arcEndPoint.center) = 0;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = -200;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = -2000;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 0;

    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    msgCHECK(returnValue);
        element.type = MPIPathElementTypeLINE ;
    MPIPathPointX(element.params.line.point) = -1800.0;
    MPIPathPointY(element.params.line.point) = -2000.0;
    MPIPathPointZ(element.params.line.point) = 0.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    element.type = MPIPathElementTypeARC_END_POINT ;
    MPIPathPointX(element.params.arcEndPoint.center) = -1800;
    MPIPathPointY(element.params.arcEndPoint.center) = -1800;
    MPIPathPointZ(element.params.arcEndPoint.center) = 0;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = -2000;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = -1800;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 0;

    element.params.arcEndPoint.direction = -1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    element.type = MPIPathElementTypeLINE ;
    MPIPathPointX(element.params.line.point) = -2000.0;
    MPIPathPointY(element.params.line.point) = -200.0;
    MPIPathPointZ(element.params.line.point) = 0.0;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);

    element.type = MPIPathElementTypeARC_END_POINT;
    MPIPathPointX(element.params.arcEndPoint.center) = -1800;
    MPIPathPointY(element.params.arcEndPoint.center) = -200;
    MPIPathPointZ(element.params.arcEndPoint.center) = 0;

    MPIPathPointX(element.params.arcEndPoint.endPoint) = -1800;
    MPIPathPointY(element.params.arcEndPoint.endPoint) = 0;
    MPIPathPointZ(element.params.arcEndPoint.endPoint) = 0;

    element.params.arcEndPoint.direction = 1;

    returnValue = mpiPathAppend(path,
                                &element);
    msgCHECK(returnValue);


    element.type = MPIPathElementTypeLINE;
    MPIPathPointX(element.params.line.point) = 0.0;
    MPIPathPointY(element.params.line.point) = 0.0;
    MPIPathPointZ(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);
}


 
      
       Legal Notice  |  Tech Email  |  Feedback
      
Copyright ©
2001-2009 Motion Engineering