.

     

MPI Application Template
template.c
 

motion2.c -- Two-axis motion, with synchronized and coordinated S-Curve profiles.
/* motion2.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 motion, with synchronized and coordinated S-Curve profiles.

This sample demonstrates how to create a two-axis motion system, using
 a single motion object.  Multiple point to point motion is commanded
 using S-Curve profile until any key is pressed.

The MPIMotionAttrMaskSYNC_START and MPIMotionAttrMaskSYNC_END attribute is used. 
 The controller starts the motion profiles for both axes at the same time and 
 reaches their target positions at the same time.

During motion, the motion status is polled from the controller.  When both
 axes complete their motions (state = IDLE), the command and actual positions,
 and motion status information is displayed.

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 "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"

/* Defaults */
#define POINT_COUNT         (2)
#define AXIS_COUNT          (2)

#define MOTION_NUMBER       (0)
#define AXIS_X_NUMBER       (0)     


/* Perform basic command line parsing. (-control -server -port -trace) */
void parseCommandLine(int                 argc,
                      char               *argv[],
                      MPIControlType     *controlType,
                      MPIControlAddress  *controlAddress,
                      long                axisCount,
                      long               *axisX_Number,
                      long               *motionNumber)
{
    long    argIndex;
    Arg     argList[] =
    {
        /* Determining axisX, then axisY is axisX + 1 */
        {   "-axis",    ArgTypeLONG,    axisX_Number,    },
        /* Determining motion supervisor number for the axisX and axisY to relate */
        {   "-motion",  ArgTypeLONG,    motionNumber,    },
        /* End of argument list (null terminator) */
        {   NULL,       ArgTypeINVALID, NULL,            }
    };

    /* 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) ||
        (*axisX_Number > (MPIXmpMAX_Axes - axisCount)) ||
        (*motionNumber >= MPIXmpMAX_MSs))
    {
        mpiPlatformConsole("usage: %s %s\n"
                           "\t\t[-axis   # (0 .. %d)]\n"
                           "\t\t[-motion # (0 .. %d)]\n",
                            argv[0],
                            ArgUSAGE,
                            /*Max.#of axis = #of motors - 2*/
                            MPIXmpMAX_Axes - axisCount,
                            MPIMotionTypeLAST - 1);
        exit(MPIMessageARG_INVALID);
    }
}


/* Perform program initialization i.e. MPI object creation */
void programInit(MPIControl         *control,
                 MPIControlType      controlType,
                 MPIControlAddress  *controlAddress,
                 MPIMotion          *motion,
                 long                motionNumber,
                 MPIAxis            *axisX,
                 long                axisNumberX,
                 MPIAxis            *axisY,
                 long                axisNumberY)
{
    long         returnValue;

    /* Create motion controller object */
    *control =
        mpiControlCreate(controlType,
                         controlAddress);
    msgCHECK(mpiControlValidate(*control));

    /* Initialize motion controller */
    returnValue =
        mpiControlInit(*control);
    msgCHECK(returnValue);

    /* Create axis object */
    *axisX =
        mpiAxisCreate(*control,
                       axisNumberX);
    msgCHECK(mpiAxisValidate(*axisX));

    *axisY =
        mpiAxisCreate(*control,
                       axisNumberY);
    msgCHECK(mpiAxisValidate(*axisY));

    /* Create motion supervisor object and append axisX to it */
    *motion =
        mpiMotionCreate(*control,
                         motionNumber,
                        *axisX);
    msgCHECK(mpiMotionValidate(*motion));

    /* Append axisY to motion */
    returnValue =
        mpiMotionAxisAppend(*motion,
                            *axisY);
    msgCHECK(returnValue);
}


/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl      *control,
                    MPIMotion       *motion,
                    MPIAxis         *axisX,
                    MPIAxis         *axisY)
{
    long    returnValue;

    /* Delete motion supervisor object */
    returnValue =
        mpiMotionDelete(*motion);
    msgCHECK(returnValue);

    /* Delete axisX object */
    returnValue =
        mpiAxisDelete(*axisX);
    msgCHECK(returnValue);

    /* Delete axisY object */
    returnValue =
        mpiAxisDelete(*axisY);
    msgCHECK(returnValue);

    /* Delete motion controller object */
    returnValue =
        mpiControlDelete(*control);
    msgCHECK(returnValue);
}


/* Display actual position for each axis */
void displayMotionPosition(MPIMotion  motion,
                           MPIStatus *status,
                           long       axisCount)
{
    long    returnValue;
    long    index;
    double  *actual     = (double *) malloc(axisCount * sizeof(double));
    double  *command    = (double *) malloc(axisCount * sizeof(double));

    printf("MotionDone: status: state %d action %d eventMask 0x%x\n"
           "\tatTarget %d settled %d %s\n",
            status->state,
            status->action,
            status->eventMask,
            status->atTarget,
            status->settled,
            (status->settled == FALSE)
                    ? "=== NOT SETTLED ==="
                    : "");

    returnValue =
        mpiMotionPositionGet(motion,
                             actual,
                             command);
    msgCHECK(returnValue);

    /* Display axis positions */
    for (index = 0; index < axisCount; index++)
    {
        fprintf(stderr,"\taxis[%d]    position: command %11.3lf\tactual %11.3lf\n",
                index,
                command[index],
                actual[index]);
    }

    free(actual);
    free(command);
}


/* Wait and check for motion status */
void waitForMotionDone(MPIMotion motion,
                       long      axisCount)
{
    MPIStatus   status;
    long        motionDone  = FALSE;
    long        returnValue = MPIMessageOK;

    /* Poll status until motion done */
    while (motionDone == FALSE)
    {
        /* Get the motion supervisor status */
        returnValue =
            mpiMotionStatus(motion,
                            &status,
                            NULL);
        msgCHECK(returnValue);

        switch (status.state)
        {
            case MPIStateSTOPPING:
            case MPIStateMOVING:
            {
                /* Sleep for 10ms and give up control to other threads */
                mpiPlatformSleep(10);
                break;
            }
            case MPIStateIDLE:
            case MPIStateSTOPPED:
                motionDone = TRUE;
                /* Display axis position */
                displayMotionPosition(motion,
                                      &status,
                                      axisCount);
                /* Wait for the motor to settle */
                mpiPlatformSleep(300);  /* msec */
                break;
            case MPIStateERROR:
            case MPIStateSTOPPING_ERROR:
            {
                fprintf(stderr, "ERROR: Axis in error state. Cannot continue motion\n");
                exit(-1);
            }
            default:
            {
                /* Unknown State */
                fprintf(stderr, "Unknown state from mpiMotionStatus.\n");
                msgCHECK(MPIMessageFATAL_ERROR);
                break;
            }
        }
    }
}


/* Command multiple SCurve motion until a key is pressed*/
long doMultipleSCurveMove(MPIMotion   motion,
                          long        pointCount,
                          long        axisCount)
{
    long                index         = 0;
    long                returnValue;        /* MPI library return value */
    MPIMotionParams     params;             /* Motion parameters  */
    MPIMotionType       motionType    = MPIMotionTypeS_CURVE | MPIMotionAttrMaskSYNC_START | MPIMotionAttrMaskSYNC_END;

    double position[POINT_COUNT][AXIS_COUNT] =
    {
        { 2000.0,   20000.0,    },
        { 0.0,      0.0,        },
    };

    MPITrajectory trajectory[POINT_COUNT][AXIS_COUNT] =
    {
        { /*  velocity     accel       decel       jerkPercent */
            { 10000.0,      1000000.0,  1000000.0,  0.0,    },
            { 10000.0,      1000000.0,  1000000.0,  0.0,    },
        },
        { /*  velocity     accel       decel       jerkPercent */
            { 10000.0,      1000000.0,  1000000.0,  0.0,    },
            { 10000.0,      1000000.0,  1000000.0,  0.0,    },
        },
    };

    MPIMotionSCurve sCurve[POINT_COUNT] =
    {
        {   &trajectory[0][0],  &position[0][0],    },
        {   &trajectory[1][0],  &position[1][0],    },
    };

    /* Return immediately when an input character is available */
    while (mpiPlatformKey(MPIWaitPOLL) <= 0)
    {
        params.sCurve = sCurve[index];

        /* Start motion */
        returnValue =
            mpiMotionStart(motion,
                           motionType,
                           &params);
            msgCHECK(returnValue);

        /* Wait until the motion is completed */
        waitForMotionDone(motion, axisCount);

        if (++index >= pointCount)
        {
            index = 0;
        }
    }

    return returnValue;
}


int main(int    argc,
         char   *argv[])
{
    MPIControl          control;    /* motion controller object handle */
    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    MPIMotion           motion;     /* motion object handle */
    MPIAxis             axisX;      /* axisX object handle */
    MPIAxis             axisY;      /* axisY object handle */

    long                motionNumber    = MOTION_NUMBER;
    long                axisX_Number    = AXIS_X_NUMBER;
    long                axisY_Number;

    /* Perform basic command line parsing. (-control -server -port -trace) */
    parseCommandLine(argc,
                     argv,
                     &controlType,
                     &controlAddress,
                     AXIS_COUNT,
                     &axisX_Number,
                     &motionNumber);
    axisY_Number = axisX_Number +1;

    /* Create and initialize MPI objects */
    programInit(&control,
                controlType,
                &controlAddress,
                &motion,
                motionNumber,
                &axisX,
                axisX_Number,
                &axisY,
                axisY_Number);

    /* Command simple SCurve motion */
    doMultipleSCurveMove(motion,
                         POINT_COUNT,
                         AXIS_COUNT);

    /* Perform certain cleanup actions and delete MPI objects */
    programCleanup(&control,
                   &motion,
                   &axisX,
                   &axisY);

    return MPIMessageOK;
}


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