.

     

MPI Application Template
template.c
 

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

/*
:n-axis motion, with synchronized and coordinated S-Curve profiles.

This sample demonstrates how to create a user specified number of axis motion
 system, using a single motion object. Multiple point to point motion is 
 commanded using S-Curve profile. 
 
Instruction:

All axes should be enabled and in an idle state before running the program.
  
User has to provide a text file specifying
 the number of axes and also position and trajectory table with the format:

 axis count:2
 #<-- for commented line
 #Pos_A Vel_A   Acc_A   Dec_A Jerk_A Pos_B  Vel_B   Acc_B   Dec_B Jerk_B
  10000 2000    4000    4000  0      20000  4000    8000    8000    0
  1000  100     200     200   50     1000   10000   20000   20000   0
 #more points ...

The MPIMotionAttrMaskSYNC_START and MPIMotionAttrMaskSYNC_END attribute are
 used as default value, the controller starts the motion profiles for all axes
 at the same time. Each axis uses it's own MPITrajectory, independently 
 reaching their target positions at the same time.With 
 MPIMotionAttrMaskSYNC_START and MPIMotionAttrMaskSYNC_END, the motion for 
 each axis will be scaled so that the motion of all axes will end at 
 approximately the same time. The time for this motion is based on the time 
 for the longest motion, so that the limits are not exceeded. The axes will be
 scaled to start and stop together, but the scaling may not be exact
 
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.
  
If an error condition occurs during motion, the program cleans up all the
 objects created and free all malloced arrays.

There are two ways to stop motion. To finish motion and stop normally, press 
 "q" or "Q". To abort and quit abruptly without finishing motion, press any 
 key. Either way, the program cleans up all the objects created and free all 
 malloced arrays.
 
The default motion supervisor where the axes are appended is motion supervisor  
 number 0, unless the user change it on the command line (-motion #).

When two or more axis are specified in the file, the first axis is defaulted
 to be axis number 0, unless user change the first axis number on the command
 line, then the second axis would be the first axis number + 1, third axis 
 would be first axis number + 2 and so on (-axis #). 

The number of points for is defaulted to be 0. Unless the user specify it 
 on the command line, the program will run through the number of points 
 (number of line) that is found in the file (-point).

Information print option is defaulted to be informational. If a minimum or 
 debug print option is preferred, user can choose this option on the command 
 line (-verbose).

The program will do a single loop through the points specified in the file,
 unless the multiple loop is specified in the command line.

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 <string.h>
#include <stdarg.h>

#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"

#define MOTION_NUMBER       (0)     /* Default motion supervisor number */
#define AXIS_NUMBER         (0)     /* Default initial axis number */
#define BUFFER_SIZE         (4096)  /* Buffer size for file reading */

/* Macro for checking errors */
#define messageCheck(msg, objs) \
    {if (msg != MPIMessageOK) reportError(msg, objs, __LINE__, __FILE__);}

typedef struct MPIObjects
{
    MPIControl          control;
    MPIControlType      controlType;
    MPIControlAddress   controlAddress;
    MPIMotion           motion;
    MPIAxis             axis[MEIXmpMAX_Axes];
    long                motionNumber;
    long                axisNumber;
    long                numberOfAxes;
    long                numberOfPoints;
} MPIObjects;

double          *position   = NULL;
MPITrajectory   *trajectory = NULL;
MPIMotionSCurve *sCurve     = NULL;

/* Information print option */
long verbosity;
enum
{
    VERBOSE_LEVEL_SILENT      = 0,
    VERBOSE_LEVEL_INFORMATION = 1,  /* Default verbosity level */
    VERBOSE_LEVEL_DEBUG       = 2,
} VERBOSE_LEVEL;


/* Error checking print routine */
void reportToUser(long verboseLevel, const char* format, ...)
{
    if (verbosity >= verboseLevel)
    {
        va_list printArgs;

        va_start(printArgs, format);
        vfprintf(stderr, format, printArgs);
        va_end(printArgs);
    }
}


/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIObjects *objs)
{
    long    returnValue;
    long    mpiResult;
    int     index;

    /* Delete motion supervisor object */
    if (objs->motion != MPIHandleVOID)
    {
        returnValue =
            mpiMotionDelete(objs->motion);
        objs->motion = MPIHandleVOID;

        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "\nMotion object deleted.\n");
    }

    /* Delete axis object */
    for(index = objs->axisNumber;
        index < (objs->axisNumber + objs->numberOfAxes);
        index++)
    {
        if (objs->axis[index] != MPIHandleVOID)
        {
            mpiResult =
                mpiAxisDelete(objs->axis[index]);
            objs->axis[index] = MPIHandleVOID;

            reportToUser(VERBOSE_LEVEL_DEBUG,
                        "Axis[%ld] object deleted.\n", index);

            if (mpiResult != MPIMessageOK)
            {
                returnValue = mpiResult;
            }
        }
    }

    /* Delete motion controller object */
    if (objs->control != MPIHandleVOID)
    {
        mpiResult =
            mpiControlDelete(objs->control);
        objs->control = MPIHandleVOID;

        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "Controller object deleted.\n");

        if (mpiResult != MPIMessageOK)
        {
            returnValue = mpiResult;
        }
    }

    if (position != NULL)
    {
        free(position);
        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "Position table freed.\n");
    }
    if (trajectory != NULL)
    {
        free(trajectory);
        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "Trajectory table freed.\n");
    }
    if (sCurve != NULL)
    {
        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "SCurve table freed.\n");
        free(sCurve);
    }

    if (returnValue != MPIMessageOK)
    {
        /* report error */
        exit(returnValue);
    }
}


/* Reports application errors and quit */
void reportError(long           message,
                 MPIObjects     *objs,
                 long           lineNumber,
                 const char     *fileName)
{
    const char* msgText = mpiMessage(message, NULL);

    reportToUser(VERBOSE_LEVEL_SILENT,
                "\n\nError at line %d of %s:\n  %s\n",
                 lineNumber, fileName, msgText);

    programCleanup(objs);
    exit(message);
}


/* Get the number of axis and number of points from file */
long getInfoFromFile(char *fileName, MPIObjects *objs)
{
    FILE    *filePointer;
    char    *lower;
    char    lineBuffer[BUFFER_SIZE];
    long    returnValue                 = MPIMessageOK;
    int     lineBufferLen               = 0;

    if(fileName != NULL) {
        if ((filePointer = fopen(fileName, "r")) == NULL)
        {
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "Can't open %s.\n",fileName);
            exit(MPIMessageFILE_OPEN_ERROR);
        }

        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "\nFile name    is %s.", fileName);

        while(fgets(lineBuffer, sizeof(lineBuffer), filePointer) != NULL &&
              returnValue == MPIMessageOK)
        {
            /* Get lineBuffer length */
            if((lineBufferLen == 0) && (strncmp(lineBuffer,"#",1) != 0) &&
               (strncmp(lineBuffer,"\n",1) != 0))
            {
                lineBufferLen = strlen(lineBuffer);

                for(lower = lineBuffer; *lower != '\0'; lower++)
                {
                    *lower = tolower(*lower);
                }

                /* Get axis count */
                if (sscanf(lineBuffer,"axis count: %ld",&(objs->numberOfAxes)) < 0)
                {
                    reportToUser(VERBOSE_LEVEL_SILENT,
                                "Number of axes not found.", fileName);
                    returnValue = MPIMessageARG_INVALID;
                }
            }
            /* Count number of points */
            else if ((strncmp(lineBuffer,"#",1) != 0) &&
                     (strncmp(lineBuffer,"\n",1) != 0))
            {
                (objs->numberOfPoints)++;
            }
        }
        fclose(filePointer);
    }
    else
    {
        reportToUser(VERBOSE_LEVEL_SILENT,
                    "Please specify a filename, where trajectories and "
                    "points are specified.\n");
        returnValue = MPIMessageARG_INVALID;
    }
    return returnValue;
}


/* Perform command line parsing. */
void parseCommandLine(int         argc,
                      char        *argv[],
                      MPIObjects  *objs,
                      char        **fileName,
                      long        *loopOption)
{
    long    argIndex;
    long    numPointsUserInput  = 0;
    long    returnValue         = MPIMessageOK;
    Arg     argList[]           =
    {
        /* Determining MS number for the axes to relate with            */
        {   "-motion",  ArgTypeLONG,    &objs->motionNumber,    },
        /* Determining axis1, then axis2=axis1+1, axis3=axis1+2, ...    */
        {   "-axis",    ArgTypeLONG,    &objs->axisNumber,      },
        /* Determining the number of points for position/points         */
        {   "-point",   ArgTypeLONG,    &numPointsUserInput,    },
        /* Determining filename where axis count and table are stored   */
        {   "-file",    ArgTypeTEXT,    fileName,               },
        /* Determining print option                                     */
        {   "-verbose", ArgTypeLONG,    &verbosity              },
        /* Determining print option                                     */
        {   "-loop",    ArgTypeLONG,    &(*loopOption)          },
        /* Determining end of argument list                             */
        {   NULL,       ArgTypeINVALID, NULL,                   }
    };

    /* Parse command line for Control type and address */
    argIndex =
        argControl(argc,
                   argv,
                   &objs->controlType,
                   &objs->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;
        }
    }

    /* Get number of axes and points */
    returnValue = getInfoFromFile(*fileName,
                                  objs);

    if(returnValue == MPIMessageOK)
    {
        /* Validate the number of points that the user requested */
        if(numPointsUserInput < 0)
        {
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "Number of points should be a positive number.\n");
            exit(MPIMessageARG_INVALID);
        }
        else if(numPointsUserInput > 0)
        {
            /* Use number of points found in file when requested is larger */
            if(objs->numberOfPoints < numPointsUserInput)
            {
                reportToUser(VERBOSE_LEVEL_SILENT,
                            "There are only %ld number of points found, "
                            "you requested %ld points.\n",
                             (objs->numberOfPoints),numPointsUserInput);
                exit(MPIMessageARG_INVALID);
            }
            else if(objs->numberOfPoints > numPointsUserInput)
            {
                objs->numberOfPoints = numPointsUserInput;
            }
        }
    }

    if(returnValue == MPIMessageOK)
    {
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "\nNumber of points   is %ld.\n", objs->numberOfPoints);
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "Motion number     is %ld.\n", objs->motionNumber);
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "Axis number    is %ld.\n",   objs->axisNumber);
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "Number of axes    is %ld.\n",    objs->numberOfAxes);
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "Loop option    is %ld.\n\n",  *loopOption);
    }

    /* Check for unknown/invalid command line arguments */
    if ((argIndex < argc)                       ||
        (objs->numberOfPoints <= 0)             ||
        (objs->axisNumber > (MEIXmpMAX_Axes - objs->numberOfAxes))  ||
        (objs->motionNumber  >= MEIXmpMAX_MSs)  ||
        (verbosity < VERBOSE_LEVEL_SILENT)      ||
        (verbosity > VERBOSE_LEVEL_DEBUG)       ||
        (returnValue != MPIMessageOK)           ||
        (*loopOption != TRUE) && (*loopOption != FALSE))
    {
        meiPlatformConsole("usage:  %s %s\n"
                           "\t\t[-motion  #(0 .. %d)]\n"
                           "\t\t[-axis    #(0 .. %d)]\n"
                           "\t\t[-point     #(1 .. n) ]\n"
                           "\t\t[-file    (text file where number of axis,\n"
                           "\t\t\t    points and trajectories are located)]\n"
                           "\t\t[-verbose #(0 for silent)\n"
                           "\t\t\t   (1 for informational)\n"
                           "\t\t\t   (2 for debug purposes)]\n"
                           "\t\t[-loop   #(0 for single loop)\n"
                           "\t\t\t   (1 for continuous loop)]",
                            argv[0],
                            ArgUSAGE,
                            MEIMotionTypeLAST - 1,
                            MEIXmpMAX_Axes - objs->numberOfAxes);
        exit(MPIMessageARG_INVALID);
    }
}


/* Perform mpi program initialization i.e. MPI object creation */
void mpiProgramInit(MPIObjects  *objs)
{
    long    returnValue;
    int         index;

    /* Initialize all objects */
    objs->control   = MPIHandleVOID;
    reportToUser(VERBOSE_LEVEL_DEBUG,
                "\n\nSet controller object to MPIHandleVOID.\n");

    objs->motion    = MPIHandleVOID;
    reportToUser(VERBOSE_LEVEL_DEBUG,
                "Set motion supervisor object to MPIHandleVOID.\n");

    for (index=0; index<MEIXmpMAX_Axes; ++index)
    {
        objs->axis[index]   = MPIHandleVOID;
    }
    reportToUser(VERBOSE_LEVEL_DEBUG,
                "Set axes to MPIHandleVOID.\n", index);

    /* Create motion controller object */
    objs->control =
        mpiControlCreate(objs->controlType,
                         &objs->controlAddress);
    messageCheck(mpiControlValidate(objs->control), objs);
    reportToUser(VERBOSE_LEVEL_DEBUG,
                "Create controller object.\n");

    /* Initialize motion controller */
    returnValue =
        mpiControlInit(objs->control);
    messageCheck(returnValue, objs);

    /* Create axis object */
    for (index = objs->axisNumber;
         index < (objs->axisNumber + objs->numberOfAxes);
         index++)
    {
        objs->axis[index] =
            mpiAxisCreate(objs->control,
                          index);
        messageCheck(mpiAxisValidate(objs->axis[index]), objs);
        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "Create axis[%ld] object.\n", index);
    }

    /* Create motion supervisor object */
    objs->motion =
        mpiMotionCreate(objs->control,
                        objs->motionNumber,
                        NULL);
    messageCheck(mpiMotionValidate(objs->motion), objs);
    reportToUser(VERBOSE_LEVEL_DEBUG,
                "Create motion supervisor object with motion number %ld.\n",
                 objs->motionNumber);

    /* Append axes to motion supervisor object */
    for (index = objs->axisNumber;
         index < (objs->axisNumber + objs->numberOfAxes);
         index++)
    {
        returnValue =
            mpiMotionAxisAppend(objs->motion,
                                objs->axis[index]);
        messageCheck(returnValue, objs);
        reportToUser(VERBOSE_LEVEL_DEBUG,
                    "Append axis[%ld] to motion supervisor.\n", index);
    }
        reportToUser(VERBOSE_LEVEL_DEBUG,
                     "\n");

}


/* Get the positions/points and trajectories from file */
long readTableFromFile(char *fileName, MPIObjects *objs)
{
    int      row                        = 1;
    int      column                     = 0;
    int      index                      = 0;
    int      positionIndex              = 0;
    int      trajectoryIndex            = 0;
    int      skipOverhead               = 2;
    int      numberOfData               = 5;
    int      lineBufferLen              = 0;
    long     returnValue                = MPIMessageOK;
    FILE     *filePointer;
    char     *token;
    char     *lower;
    char     lineBuffer[BUFFER_SIZE];

    /* Open file for read only */
    if((filePointer = fopen(fileName, "r")) == NULL)
    {
        reportToUser(VERBOSE_LEVEL_SILENT,
                    "Can't open %s\n",fileName);
        exit(MPIMessageFILE_OPEN_ERROR);
    }

    reportToUser(VERBOSE_LEVEL_DEBUG,
                "\nReading information from file \"%s\"\n", fileName);

    /* Go through the lines */
    while((returnValue == MPIMessageOK) &&
          (row <= objs->numberOfPoints) &&
          (fgets(lineBuffer, sizeof(lineBuffer), filePointer) != NULL))
    {
        /* Skip comments and blank lines */
        if(strncmp(lineBuffer,"#",1) == 0 || strncmp(lineBuffer,"\n",1) == 0)
        {
            reportToUser(VERBOSE_LEVEL_DEBUG,
                        "Skipping line: %s",lineBuffer);
        }
        /* Skip the axis count line */
        else if(lineBufferLen == 0)
        {
            for(lower = lineBuffer; *lower != '\0'; lower++)
            {
                *lower = tolower(*lower);
            }

            /* If it is the axis count line, then change lineBufferLen */
            if (strncmp(lineBuffer,"axis count:",11) == 0)
            {
                lineBufferLen = strlen(lineBuffer);
                reportToUser(VERBOSE_LEVEL_DEBUG,
                            "Getting  line: %s",lineBuffer);
            }
        }
        else
        {
            row++;
            reportToUser(VERBOSE_LEVEL_DEBUG,
                        "Getting  line: %s",lineBuffer);

            /* Go through the columns */
            for(column = 0;
                (column < (objs->numberOfAxes * numberOfData)) &&
                (returnValue == MPIMessageOK);
                column++)
            {
                /* Get first token*/
                if(column == 0 &&
                   ((token = strtok(lineBuffer," ,\t\n")) == NULL))
                {
                    reportToUser(VERBOSE_LEVEL_SILENT,
                                "Check your file<%s> again!", fileName);
                    returnValue = MPIMessageFILE_READ_ERROR;
                }
                /* Get next token */
                else if(column != 0 &&
                        ((token = strtok(NULL," ,\t\n")) == NULL))
                {
                    reportToUser(VERBOSE_LEVEL_SILENT,
                                "Check your file<%s> again!", fileName);
                    returnValue = MPIMessageFILE_READ_ERROR;
                }
                /* Store trajectories data */
                else
                {
                    /* Get position for the other axes */
                    if(column%numberOfData == 0)
                    {
                        position[positionIndex] = atof(token);
                        positionIndex++;
                    }
                    /* Get velocity */
                    else if((column-1)%numberOfData == 0)
                    {
                        trajectory[trajectoryIndex].velocity = atof(token);
                    }
                    /* Get acceleration */
                    else if((column-2)%numberOfData == 0)
                    {
                        trajectory[trajectoryIndex].acceleration = atof(token);
                    }
                    /* Get decceleration */
                    else if((column-3)%numberOfData == 0)
                    {
                        trajectory[trajectoryIndex].deceleration = atof(token);
                    }
                    /* Get jerk percent */
                    else if((column-4)%numberOfData == 0)
                    {
                        trajectory[trajectoryIndex].jerkPercent = atof(token);
                        trajectoryIndex++;
                    }
                }
            }
        }
    }

    if(returnValue == MPIMessageOK)
    {
        /* Fill up the sCurve structure */
        for(index = 0; index < objs->numberOfPoints; index++)
        {
            sCurve[index].position   = &position[(objs->numberOfAxes)*index];
            sCurve[index].trajectory = &trajectory[(objs->numberOfAxes)*index];
        }
    }

    fclose(filePointer);
    return returnValue;
}


/* Perform program initialization */
long programInit(MPIObjects *objs, char *fileName)
{
    long    returnValue     = MPIMessageOK;

    if(returnValue == MPIMessageOK)
    {
        if((position = (double *)
                        malloc (objs->numberOfAxes * objs->numberOfPoints *
                                sizeof(double))) == NULL)
        {
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "Malloc for position unsuccessful.\n");
            exit(MPIMessageNO_MEMORY);
        }
    }

    if(returnValue == MPIMessageOK)
    {
        if((trajectory = (MPITrajectory *)
                          malloc (objs->numberOfAxes * objs->numberOfPoints *
                                  sizeof(MPITrajectory))) == NULL )
        {
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "Malloc for trajectory unsuccessful.\n");
            free(position);
            exit(MPIMessageNO_MEMORY);
        }
    }

    if(returnValue == MPIMessageOK)
    {
        if((sCurve = (MPIMotionSCurve *)
                      malloc (objs->numberOfPoints *
                              sizeof(MPIMotionSCurve))) == NULL )
        {
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "Malloc for sCurve unsuccessful.\n");
            free(position);
            free(trajectory);
            exit(MPIMessageNO_MEMORY);
        }
    }

    if(returnValue == MPIMessageOK)
    {
        returnValue = readTableFromFile(fileName,
                                        objs);
    }

    if(returnValue == MPIMessageOK)
    {
        mpiProgramInit(objs);
    }
    return returnValue;
}


/* Display actual position for each axis */
long displayMotionPosition(MPIObjects *objs, MPIStatus *status)
{
    long    returnValue = MPIMessageOK;
    long    index;
    double  *actual;
    double  *command;

    if((actual = (double *)
                  malloc(objs->numberOfAxes * sizeof(double))) == NULL)
    {
        reportToUser(VERBOSE_LEVEL_SILENT,
                    "Malloc for actual unsuccessful.\n");
        messageCheck(MPIMessageNO_MEMORY, objs);
    }
    if((command = (double *)
                   malloc(objs->numberOfAxes * sizeof(double))) == NULL)
    {
        reportToUser(VERBOSE_LEVEL_SILENT,
                    "Malloc for command unsuccessful.\n");
        free(actual);
        messageCheck(MPIMessageNO_MEMORY, objs)
    }

    reportToUser(VERBOSE_LEVEL_INFORMATION,
                "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(objs->motion,
                             actual,
                             command);
    messageCheck(returnValue, objs);

    /* Display axis positions */
    for (index = 0; index < objs->numberOfAxes; index++)
    {
        reportToUser(VERBOSE_LEVEL_INFORMATION,
                    "\taxis[%d]   position: command %11.3lf\tactual %11.3lf\n",
                     index+objs->axisNumber,
                     command[index],
                     actual[index]);
    }

    free(actual);
    free(command);
    return returnValue;
}


/* Wait and check for motion status */
long waitForMotionDone(MPIObjects *objs, long *quit)
{
    MPIStatus   status;
    long        motionDone      = FALSE;
    long        key;
    long        returnValue     = MPIMessageOK;

    *quit = FALSE;

    /* Poll status until motion done */
    while (motionDone == FALSE)
    {
        key = meiPlatformKey(MPIWaitPOLL);

        if ((key == (long)'q') || (key == (long)'Q'))
        {
            *quit = TRUE;
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "\nQuitting: Q had been pressed. Normal QUIT.\n\n");
        }
        else if (key != -1)
        {
            returnValue =
                mpiMotionAction(objs->motion,
                                MPIActionE_STOP_ABORT);
            reportToUser(VERBOSE_LEVEL_SILENT,
                        "\n\nQuitting: ABORT. Stopping axes.\n");
            programCleanup(objs);
            exit(-1);
        }

        /* Get the motion supervisor status */
        returnValue =
            mpiMotionStatus(objs->motion,
                            &status,
                            NULL);
        messageCheck(returnValue, objs);

        switch (status.state)
        {
            case MPIStateSTOPPING:
            case MPIStateMOVING:
            {
                /* Sleep for 10ms and give up control to other threads */
                meiPlatformSleep(10);
                break;
            }
            case MPIStateIDLE:
            case MPIStateSTOPPED:
            {
                motionDone = TRUE;

                /* Display axis position */
                displayMotionPosition(objs, &status);

                /* Wait for the motor to settle */
                meiPlatformSleep(300);  /* msec */
                break;
            }
            case MPIStateERROR:
            case MPIStateSTOPPING_ERROR:
            {
                reportToUser(VERBOSE_LEVEL_SILENT,
                            "ERROR: Axis in error state. "
                            "Cannot continue motion.\n");
                messageCheck(MPIMotionMessageERROR, objs);
            }
            default:
            {
                /* Unknown State */
                reportToUser(VERBOSE_LEVEL_SILENT,
                            "Unknown state from mpiMotionStatus.\n");
                messageCheck(MPIMessageFATAL_ERROR, objs);
                break;
            }
        }
    }
    return returnValue;
}


/* Command simple trapezoidal motion */
long simpleSCurveMove(MPIObjects *objs, long loopOption)
{

    MPIMotionParams params;
    MPIMotionType   motionType      = MPIMotionTypeS_CURVE |
                                      MPIMotionAttrMaskSYNC_START |
                                      MPIMotionAttrMaskSYNC_END;

    long            index           = 0;
    long            returnValue     = MPIMessageOK;
    long            quit            = FALSE;
    long            multipleLoop    = TRUE;

    /* Return immediately when an input character is available */
    while ((quit == FALSE) && (returnValue == MPIMessageOK) && multipleLoop)
    {
        params.sCurve = sCurve[index];

        /* Start motion */
        returnValue =
            mpiMotionStart(objs->motion,
                           motionType,
                           &params);

        if(returnValue == MPIMessageOK)
        {
            /* Wait until the motion is completed */
           returnValue = waitForMotionDone(objs, &quit);

           if (++index >= objs->numberOfPoints)
           {
               if(loopOption == FALSE)
               {
                   multipleLoop = FALSE;
               }
               else {
                   index = 0;
               }
           }
        }
    }

    return returnValue;
}


/* MAIN ROUTINE */
int main(int argc, char *argv[])
{
    MPIObjects      objs;
    char            *fileName       = NULL;
    long            loopOption      = FALSE; /* Default to single loop */
    long            returnValue     = MPIMessageOK;

    /* Set default values */
    verbosity                       = VERBOSE_LEVEL_INFORMATION;
    objs.motionNumber               = MOTION_NUMBER;
    objs.axisNumber                 = AXIS_NUMBER;
    objs.numberOfAxes               = 0;
    objs.numberOfPoints             = 0;


    /* Perform command line parsing. */
    parseCommandLine(argc,
                     argv,
                     &objs,
                     &fileName,
                     &loopOption);

    /* Create and initialize MPI objects */
    returnValue = programInit(&objs,
                              fileName);
    messageCheck(returnValue, &objs);

    /* Command simple SCurve motion */
    returnValue = simpleSCurveMove(&objs,
                                   loopOption);
    messageCheck(returnValue, &objs);

    /* Perform certain cleanup actions and delete MPI objects */
    programCleanup(&objs);

    return MPIMessageOK;
}


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