.

     

MPI Application Template
template.c
 

anticol1.c -- Program Sequencer aborts axes prior to a collision (anti-collision protection)
/* anticol1.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.
 */

/*

:Program Sequencer aborts axes prior to a collision (anti-collision protection)

This sample program provides anti-collision protection for two axes that move
 along the same physical space.  The program creates a program sequencer that
 will continually monitor the relative actual positions of two axes.  When the
 relative positions get closer than a defined tolerance (defined at the top of
 the program as DIFFERENCE_TOLEARNCE) the axes will be aborted by the program
 sequencer.

The XMP program sequencer controls the execution of a single command or a
 series of commands on the XMP controller.  The program sequencer provides the
 ability to execute programs directly on the XMP controller without host
 intervention.  Examples of individual commands that can be executed by the
 program sequencer are motion, looping, conditional branching, computation,
 reading and writing of memory, time delays, waiting for conditions, setting
 inputs/outputs, and generation of events.  This rich command set provides
 capability similar to, and beyond, that provided by Programmable Logic
 Controller (PLC) programs.

From anticol1c.c (8/1/1999)

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    anticol1Main

argMainRENAME(main, anticol1)
#endif

#define AXIS_COUNT              (2)
#define DIFFERENCE_TOLERANCE    (7000)

/* Command line arguments and defaults */
long            axisNumber[AXIS_COUNT] = { 0, 1, };
long            motionNumber    = 0;
long            sequenceNumber  = 0;
long            differenceTolerance = DIFFERENCE_TOLERANCE;

Arg argList[] = {
    {   "-axis",        ArgTypeLONG,    &axisNumber[0],         },
    {   "-motion",      ArgTypeLONG,    &motionNumber,          },
    {   "-sequence",    ArgTypeLONG,    &sequenceNumber,        },
    {   "-tolerance",   ArgTypeLONG,    &differenceTolerance,   },

    {   NULL,       ArgTypeINVALID, NULL,   }
};


                        /* COMPUTE + BRANCH + BRANCH + ABORT = 4 */
MPICommand  CommandTable[4];
#define COMMAND_COUNT   (sizeof(CommandTable) / sizeof(MPICommand))

int
    main(int    argc,
         char   *argv[])
{
    MPIControl  control;            /* motion controller handle */
    MPIMotion   motion;             /* motion handle */
    MPIAxis     axis[AXIS_COUNT];   /* axis handles */
    MPISequence sequence;           /* sequence handle */

    MPICommandParams    commandParams;  /* command parameters */

    long    returnValue;    /* return value from library */

    long    commandIndex;   /* CommandTable[] index */
    long    index;

    MPIControlType      controlType;
    MPIControlAddress   controlAddress;
    MPIXmpData          *firmware;
    MPIPlatform         platform;   /* platform handle */
    MPIXmpBufferData    *xmpBufferData;
    long                *addressActPos1;
    long                *addressActPos2;
    long                address;
    long                *allocPointer;
    long                difference;
    long                *addressDifferenceStored;



    long    argIndex;

    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) ||
        (sequenceNumber >= MPIXmpMAX_PSs) ||
        (differenceTolerance < 0)){

        mpiPlatformConsole("usage: %s %s\n"
                           "\t\t[-axis # (0 .. %d)]\n"
                           "\t\t[-motion # (0 .. %d)]\n"
                           "\t\t[-sequence # (0 .. %d)]\n"
                           "\t\t[-tolerance # ( larger than 0)]\n",
                            argv[0],
                            ArgUSAGE,
                            MPIXmpMAX_Axes - AXIS_COUNT,
                            MPIXmpMAX_MSs - 1,
                            MPIXmpMAX_PSs - 1);

        exit(MPIMessageARG_INVALID);
    }

    axisNumber[1] = axisNumber[0] + 1;

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

    /* Initialize motion controller */
    returnValue = mpiControlInit(control);
    if (returnValue != MPIMessageOK) {
        fprintf(stderr, "%s: mpiControlInit() returns 0x%x: %s\n",
                        argv[0],
                        returnValue,
                        mpiMessage(returnValue, NULL));
        exit(1);
    }

    /* Get pointer to XMP firmware */
    returnValue =
        mpiControlMemory(control,
                         &firmware,
                         &xmpBufferData);
    msgCHECK(returnValue);

    platform =
        mpiControlPlatform(control);
    msgCHECK(mpiPlatformValidate(platform));

    /*Pointer into xmp memory space.  Pointer to the user buffer  */
    returnValue =
        mpiControlMemoryAlloc(control,
                              MPIControlMemoryTypeUSER,
                              sizeof(long),
                              &allocPointer);
    msgCHECK(returnValue);

    returnValue =
            mpiPlatformMemoryToFirmware(platform,
                                        allocPointer,
                                        (void **)&address);
    msgCHECK(returnValue);

    mpiPlatformConsole("address of allocPointer (user buffer) is 0x%x\n",address);

    /* Create motion object */
    motion =
        mpiMotionCreate(control,
                        motionNumber,
                        MPIHandleVOID);
    msgCHECK(mpiMotionValidate(motion));

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

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

    /* Create motion axis list */
    returnValue =
        mpiMotionAxisListSet(motion,
                             AXIS_COUNT,
                             axis);
    msgCHECK(returnValue);

    /* Write the axis list to the motion supervisor on the board */
    returnValue =
        mpiMotionAction(motion,
                        MPIActionMAP);

    msgCHECK(returnValue);

    /* Create Sequence */
    sequence =
        mpiSequenceCreate(control,
                          sequenceNumber,
                          -1);
    msgCHECK(mpiSequenceValidate(sequence));

    /* CommandTable[commandIndex] */
    commandIndex = 0;

    /* Get the address for the first axis's actual position register */
    addressActPos1 = &firmware->Axis[axisNumber[0]].ActPosition.l[1];

    /* Get the address for the second axis's actual position register */
    addressActPos2 = &firmware->Axis[axisNumber[1]].ActPosition.l[1];

    returnValue =
            mpiPlatformMemoryToFirmware(platform,
                                        addressActPos1,
                                        (void **)&address);
    msgCHECK(returnValue);

    mpiPlatformConsole("addressActPos1 is 0x%x\n",address);

    returnValue =
            mpiPlatformMemoryToFirmware(platform,
                                        addressActPos2,
                                        (void **)&address);
    msgCHECK(returnValue);
    mpiPlatformConsole("addressActPos2 is 0x%x\n",address);

    /* 1st. Command  */
    /* Take the difference of the two actual positions and store them in the user buffer */
    addressDifferenceStored = allocPointer;

    commandParams.compute.dst.l         = addressDifferenceStored;
    commandParams.compute.expr.address.l    = addressActPos1;
    commandParams.compute.expr.oper     = MPICommandOperatorSUBTRACT;
    commandParams.compute.expr.by.ref.l = addressActPos2;

    CommandTable[commandIndex] =
        mpiCommandCreate(MPICommandTypeCOMPUTE_REF,
                         &commandParams,
                         "First");
    msgCHECK(mpiCommandValidate(CommandTable[commandIndex]));

    commandIndex++;

    /*  2nd Command */
    /*  Compare the position differences to the position diference tolerance */
    /*  Perform abort if test is true */

    commandParams.branch.label              = "ABORT";
    commandParams.branch.expr.address.l     = addressDifferenceStored;
    commandParams.branch.expr.oper          = MPICommandOperatorLESS;
    commandParams.branch.expr.by.value.l    = differenceTolerance;

    CommandTable[commandIndex] =
        mpiCommandCreate(MPICommandTypeBRANCH,
                         &commandParams,
                         NULL);
    msgCHECK(mpiCommandValidate(CommandTable[commandIndex]));

    commandIndex++;

    /* 3rd. Command */
    /* Branch to the first command of the sequence */
    commandParams.branch.label      = "First";  /* First command */
    commandParams.branch.expr.oper  = MPICommandOperatorALWAYS;

    /* Create Command */
    CommandTable[commandIndex] =
        mpiCommandCreate(MPICommandTypeBRANCH,
                         &commandParams,
                         NULL);
    msgCHECK(mpiCommandValidate(CommandTable[commandIndex]));

    commandIndex++;

    /* 4th. command */
    /* Abort the axes that are part of motion supervisor motion */

    commandParams.motion.motionCommand  = MPICommandMotionABORT;
    commandParams.motion.motion     = motion;

    CommandTable[commandIndex] =
        mpiCommandCreate(MPICommandTypeMOVE,
                         &commandParams,
                         "ABORT");
    msgCHECK(mpiCommandValidate(CommandTable[commandIndex]));

    commandIndex++;


    /* Create sequence command list */
    returnValue =
        mpiSequenceCommandListSet(sequence,
                                  commandIndex,
                                  CommandTable);
    msgCHECK(returnValue);

    /* Start sequence */
    returnValue =
        mpiSequenceStart(sequence,
                         MPIHandleVOID);

    if (returnValue != MPIMessageOK) {
        fprintf(stderr, "%s: mpiSequenceStart() returns 0x%x: %s\n",
                        argv[0],
                        returnValue,
                        mpiMessage(returnValue, NULL));
        exit(2);
    }

    mpiPlatformConsole("Press any key to stop sequence\n");
    mpiPlatformKey(MPIWaitFOREVER);

    returnValue =
            mpiControlMemoryGet(control,
                                &difference,
                                allocPointer,
                                sizeof(long));
    msgCHECK(returnValue);

    mpiPlatformConsole("Axis Position difference is %ld\n",difference);

    /* Stop sequence */
    returnValue = mpiSequenceStop(sequence);
    msgCHECK(returnValue);

    returnValue = mpiSequenceDelete(sequence);
    msgCHECK(returnValue);

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

    for (index = 0; index < AXIS_COUNT; index++) {
        returnValue = mpiAxisDelete(axis[index]);
        msgCHECK(returnValue);
    }

    returnValue =
        mpiControlMemoryFree(control,
                             MPIControlMemoryTypeUSER,
                             sizeof(long),
                             allocPointer);
    msgCHECK(returnValue);


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

    return ((int)returnValue);
}


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