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);
}
|
