.

     

MPI Application Template
template.c
 

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*)(&params));
    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);
}


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