.

     

MPI Application Template
template.c
 

settle2.c -- Configure settling criteria and exception event settling conditions.
/* settle2.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.
 */

/*

:Configure settling criteria and exception event settling conditions.

The MPI supports axis configurations for motion settling criteria to
 determine when motion is complete.  The XMP-Series controller supports
 event notification for Coarse, Fine, and Target positions and motion done.
 Generally, settling criteria is only applicable to normal host commanded
 motion profiles.  The XMP-Series controller can also be configured to
 support settling criteria with Stop and E-Stop events.

The settling criteria is configured set with mpiAxisConfigGet/Set(...),
 using the structures:

typedef struct MPIAxisInPosition {
    struct {
        float   positionFine;
        long    positionCoarse;
        float   velocity;
    } tolerance;
    float   settlingTime;
    long    settleOnStop;
    long    settleOnEstop;
} MPIAxisInPosition;

typedef struct MPIAxisConfig {
    MPIAxisInPosition   inPosition;
    MPIObjectMap    filterMap;
} MPIAxisConfig;


The settling criteria are:

 positionFine - in-position window between command and actual (counts)

 positionCoarse - in-position window between target and actual (counts)

 velocity - actual velocity (counts/second)

 settlingTime - required time for settling (seconds)


The Stop and E-Stop event settling conditions can be enabled/disabled:

 settleOnStop - TRUE or FALSE, enables/disables settling criteria for Stop

 settleOnEstop - TRUE or FALSE, enables/disables settling criteria for E-Stop


The XMP-Series controller sets the appropriate status bits and generates
 the following events based on the settling criteria and logic:

 IN_FINE_POSITION

    1) |command position - actual position| < positionFine tolerance
    2) |command velocity - actual velocity| < velocity tolerance
    3) 1,2 and 3 have been satisfied for the settlingTime tolerance

 IN_COARSE_POSITION

    1) |target position - actual position| < postionCoarse tolerance

 AT_TARGET

    1) The command position = target position.

 DONE

    1) The calculated trajectory is complete.
    2) IN_FINE_POSITION conditions have been satisfied.

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"

#if defined(ARG_MAIN_RENAME)
#define main    settle2Main

argMainRENAME(main, settle2)
#endif

#define MOTION_COUNT        (1)     /* Number of motion profiles */
#define AXIS_COUNT          (1)     /* Number of axes */


/* Command line arguments and defaults */
long            axisNumber[AXIS_COUNT] = { 0,   };
long            motionNumber    = 0;
MPIMotionType   motionType      = MPIMotionTypeS_CURVE;
float           fineTol         = (float)500.0; /* counts */
long            coarseTol       = 200;          /* counts */
float           velocityTol     = (float)1.0;   /* counts/sec */
float           settlingTime    = (float)0.2;   /* seconds */
long            eventDelay      = 1000;         /* milliseconds */

Arg argList[] = {
    {   "-axis",    ArgTypeLONG,    &axisNumber[0], },
    {   "-motion",  ArgTypeLONG,    &motionNumber,  },
    {   "-type",    ArgTypeLONG,    &motionType,    },
    {   "-fine",    ArgTypeFLOAT,   &fineTol,       },
    {   "-coarse",  ArgTypeLONG,    &coarseTol,     },
    {   "-velocity",ArgTypeFLOAT,   &velocityTol,   },
    {   "-settling",ArgTypeFLOAT,   &settlingTime,  },
    {   "-delay",   ArgTypeLONG,    &eventDelay,    },

    {   NULL,       ArgTypeINVALID, NULL,   }
};

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

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

/* Motion parameters */

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

MPIMotionTrapezoidal    trapezoidal[] = {
    {   &trajectory[0][0],  &position[0][0],    },
    {   &trajectory[1][0],  &position[1][0],    },
};

MPIMotionVelocity   velocity[] = {
    {   &trajectory[0][0],  },
    {   &trajectory[1][0],  },
};

#define SETTLE_EVENTS   (2) /* Number of settle configurations */

/* Settle on Stop/E-Stop Events */
long settleEvent[] = {
    FALSE,
    TRUE
};

#define ACTIONS         (5) /* Number of MPI actions */

/* Generate Actions */
MPIAction action[] = {
    MPIActionNONE,
    MPIActionSTOP,
    MPIActionE_STOP,
    MPIActionE_STOP_ABORT,
    MPIActionABORT
};

char * actionString[] = {
    "None",
    "Stop",
    "E-Stop",
    "E-Stop/Abort",
    "Abort"
};

void eventStatusDisplay(MPIEventStatus status);


int
    main(int    argc,
         char   *argv[])
{
    MPIControl  control;    /* Motion controller handle */
   MPIMotor motor;
    MPIAxis     axis;       /* Axis object */
    MPIMotion   motion;     /* Motion object */
    MPINotify   notify;     /* Event notification object */
    MPIEventMgr eventMgr;   /* Event manager handle */

    MPIAxisConfig   axisConfig; /* Axis configuration object */

    MPIEventMask    eventMask;

    long    returnValue;    /* Return value from library */

    long    index;
    long    actionIndex;    /* Index to action[...] array */
    long    settleIndex;    /* Index to settleEvent[...] array */
    long    motionDone;     /* Flag when Done occurs */
    long    previousEvents; /* Flag to check for previously generated events */

    double  actual;         /* Axis position */
    double  origin;         /* Axis origin */

    Service service;

    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    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] > (MEIXmpMAX_Axes - AXIS_COUNT)) ||
        (motionNumber  >= MEIXmpMAX_MSs) ||
        (motionType <  MPIMotionTypeFIRST) ||
        (motionType >= MEIMotionTypeLAST) ||
        (fineTol < 0.0) ||
        (coarseTol < 0) ||
        (velocityTol < 0.0) ||
        (settlingTime < 0.0) ||
        (eventDelay < 0)) {
        printf("usage: %s %s\n"
               "\t\t[-axis # (0 .. %d)]\n"
               "\t\t[-motion # (0 .. %d)]\n"
               "\t\t[-type # (0 .. %d)]\n"
               "\t\t[-fine # (0.0 .. )]\n"
               "\t\t[-coarse # (0 .. )]\n"
               "\t\t[-velocity # (0.0 .. )]\n"
               "\t\t[-settling # (0.0 .. )]\n"
               "\t\t[-delay # (0 .. )]\n",
                argv[0],
                ArgUSAGE,
                MEIXmpMAX_Axes - AXIS_COUNT,
                MEIXmpMAX_MSs - 1,
                MEIMotionTypeLAST - 1);
        exit(MPIMessageARG_INVALID);
    }

    switch (motionType) {
        case MPIMotionTypeS_CURVE:
        case MPIMotionTypeTRAPEZOIDAL:
        case MPIMotionTypeVELOCITY: {
            break;
        }
        default: {
            meiPlatformConsole("%s: %d: motion type not available\n",
                                argv[0],
                                motionType);
            exit(MPIMessageUNSUPPORTED);
            break;
        }
    }

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

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

    /* Create motor object for axisNumber */
    motor =
        mpiMotorCreate(control,
                      axisNumber[0]);
    msgCHECK(mpiMotorValidate(motor));

    /* Create axis object for axisNumber */
    axis =
        mpiAxisCreate(control,
                      axisNumber[0]);
    msgCHECK(mpiAxisValidate(axis));

    /* Create motion object, appending the axis object */
    motion =
        mpiMotionCreate(control,
                        motionNumber,
                        axis);
    msgCHECK(mpiMotionValidate(motion));

    /* Request notification of ALL events from motion */
    mpiEventMaskCLEAR(eventMask);
    mpiEventMaskALL(eventMask);
    meiEventMaskALL(eventMask);

    returnValue =
        mpiMotionEventNotifySet(motion,
                                eventMask,
                                NULL);      /* ALL sources */
    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);  /* -1 => enable interrupts */
    meiAssert(service != NULL);

    meiPlatformConsole("Press any key to quit ...\n");

    /* Loop repeatedly */
    index       = 0;
    actionIndex = 0;
    settleIndex = 0;
    motionDone  = TRUE;
    while (meiPlatformKey(MPIWaitPOLL) <= 0) {
        MPIEventStatus      eventStatus;
        MPIStatus           status;

        if (motionDone) {
            MPIMotionParams     motionParams;       /* Motion parameters */

            /* Fill in the MPIMotionParams structure */
            switch (motionType) {
                case MPIMotionTypeS_CURVE: {
                    motionParams.sCurve = sCurve[index];
                    break;
                }
                case MPIMotionTypeTRAPEZOIDAL: {
                    motionParams.trapezoidal = trapezoidal[index];
                    break;
                }
                case MPIMotionTypeVELOCITY: {
                    motionParams.velocity = velocity[index];
                    break;
                }
                default: {
                    meiAssert(FALSE);
                    break;
                }
            }

            /* Check motion status for any error conditions */
            returnValue =
                mpiMotionStatus(motion,
                                &status,
                                NULL);
            msgCHECK(returnValue);

            if (status.state == MPIStateERROR) {
                returnValue =
                    mpiMotionAction(motion,
                                    MPIActionRESET);
                msgCHECK(returnValue);

                printf("  Error state cleared.\n");
            }

            /* Set origin to current actual position */
            returnValue =
                mpiAxisActualPositionGet(axis,
                                         &actual);
            msgCHECK(returnValue);

            returnValue =
                mpiAxisOriginGet(axis,
                                 &origin);
            msgCHECK(returnValue);

            returnValue =
                mpiAxisOriginSet(axis,
                                 (origin + actual));
            msgCHECK(returnValue);

            /* Configure axis settling criteria */
            returnValue =
                mpiAxisConfigGet(axis,
                                 &axisConfig,
                                 NULL);
            msgCHECK(returnValue);

            axisConfig.inPosition.tolerance.positionFine =
                fineTol;
            axisConfig.inPosition.tolerance.positionCoarse =
                coarseTol;
            axisConfig.inPosition.tolerance.velocity =
                velocityTol;
            axisConfig.inPosition.settlingTime =
                settlingTime;

            /* Configure settling criteria for Stop and E-Stop events
                TRUE = use settling criteria for Stop and E-Stop
                FALSE = do NOT use settling criteria for Stop and E-Stop
            */
            axisConfig.inPosition.settleOnStop = settleEvent[settleIndex];
            axisConfig.inPosition.settleOnEstop = settleEvent[settleIndex];

            returnValue =
                mpiAxisConfigSet(axis,
                                 &axisConfig,
                                 NULL);
            msgCHECK(returnValue);

            /* Check for any previous events (generated by RESET) */
            previousEvents = TRUE;
            while (previousEvents) {
                returnValue =
                    mpiNotifyEventWait(notify,
                                       &eventStatus,
                                       MPIWaitPOLL);
                if (returnValue == MPIMessageTIMEOUT) {
                    previousEvents = FALSE;
                }
                else {
                    msgCHECK(returnValue);
                    eventStatusDisplay(eventStatus);
                }
            }

            meiPlatformConsole("\nMotion Start, Target= %12.0lf  ",
                   position[index][axisNumber[0]]);

            meiPlatformConsole("  Settle on Event: %s",
                   settleEvent[settleIndex] ? "yes" : "no");


         returnValue = mpiMotorAmpEnableSet(motor,
                                    TRUE);
            msgCHECK(returnValue);



            /* Point to point motion (relative to new origin) */
            returnValue =
                mpiMotionStart(motion,
                               motionType,
                               &motionParams);
            msgCHECK(returnValue);

            /* Wait for motion profile to partially execute */
            meiPlatformSleep(eventDelay);

            /* Generate an event */
            returnValue =
                mpiMotionAction(motion,
                                action[actionIndex]);
            msgCHECK(returnValue);

            meiPlatformConsole("  Event:%s.\n", actionString[actionIndex]);

            /* Increment through settleEvent[...] and action[...] */
            if (++actionIndex >= ACTIONS) {
                actionIndex = 0;
                if (++settleIndex >= SETTLE_EVENTS) {
                    settleIndex = 0;
                }
            }
            motionDone = FALSE;
        }

        /* Wait for motion event */
        returnValue =
            mpiNotifyEventWait(notify,
                               &eventStatus,
                               MPIWaitFOREVER);
        msgCHECK(returnValue);

        /* Motion Done Event from motion source */
        if  (eventStatus.type == MPIEventTypeMOTION_DONE) {
            motionDone = TRUE;

            if (++index >= MOTION_COUNT) {
                index = 0;
            }
        }
        eventStatusDisplay(eventStatus);
    }

    printf("\n");

    returnValue = mpiMotionDelete(motion);
    msgCHECK(returnValue);

    returnValue = mpiAxisDelete(axis);
    msgCHECK(returnValue);

    returnValue = serviceDelete(service);
    msgCHECK(returnValue);

    returnValue = mpiEventMgrDelete(eventMgr);
    msgCHECK(returnValue);

    returnValue = mpiNotifyDelete(notify);
    msgCHECK(returnValue);

    returnValue = mpiControlDelete(control);
    msgCHECK(returnValue);

    return ((int)returnValue);
}


void eventStatusDisplay(MPIEventStatus status)
{
    MEIEventStatusInfo  *info;

    info = (MEIEventStatusInfo *)status.info;

    switch (status.type) {
        /* In Coarse Event from axis source */
        case MEIEventTypeIN_POSITION_COARSE: {
            meiPlatformConsole("  InCoarse, actual:%ld, sample:0x%x\n",
                   info->data.axis.actualPosition,
                   info->data.axis.sampleCounter);
            break;
        }
        /* In Fine Event from axis source */
        case MEIEventTypeIN_POSITION_FINE: {
            meiPlatformConsole("  InFine, actual:%ld, sample:0x%x\n",
                   info->data.axis.actualPosition,
                   info->data.axis.sampleCounter);
            break;
        }
        /* At Target Event from axis source */
        case MEIEventTypeAT_TARGET: {
            meiPlatformConsole("  AtTarget, actual:%ld, sample:0x%x\n",
                   info->data.axis.actualPosition,
                   info->data.axis.sampleCounter);
            break;
        }
        /* Motion Done Event from motion source */
        case MPIEventTypeMOTION_DONE: {
            meiPlatformConsole("  Done, sample:0x%x\n",
                   info->data.motion.sampleCounter);
            break;
        }
        default: {
            meiPlatformConsole("Event (unexpected): %ld (0x%x)",
                   status.type,
                   info->data.word[0]);
            break;
        }
    }
}


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