mboard2.c -- Two point motion on multiple controllers with an event
manager in polling mode.
/* mboard2.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
*/
/*
:Two point motion on multiple controllers with an event manager in polling mode.
Creates a two point motion sequentially on multiple controller boards.
One event manager is used for all controllers. The event manager is polled
to retrieve MotionDone events from each motion.
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 mboard2Main
argMainRENAME(main, mboard2)
#endif
#define BOARD_COUNT (2)
#define MOTION_COUNT (1)
#define AXIS_COUNT (1)
/* Command line arguments and defaults */
long controlNumber = 0;
long axisNumber = 0;
long motionNumber = 0;
MPIMotionType motionType = MPIMotionTypeTRAPEZOIDAL;
Arg argList[] = {
{ "-control", ArgTypeLONG, &controlNumber, },
{ "-axis", ArgTypeLONG, &axisNumber, },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ "-type", ArgTypeLONG, &motionType, },
{ NULL, ArgTypeINVALID, NULL, }
};
double position[MOTION_COUNT][AXIS_COUNT] = {
{ 20000.0 },
};
MPITrajectory trajectory[MOTION_COUNT][AXIS_COUNT] = {
{ /* velocity accel decel */
{ 10000.0, 100000.0, 100000.0, },
},
};
/* motion parameters */
MPIMotionSCurve sCurve[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
};
MPIMotionTrapezoidal trapezoidal[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
};
MPIMotionVelocity velocity[MOTION_COUNT] = {
{ &trajectory[0][0], },
};
int
main(int argc,
char *argv[])
{
MPIControl controller[BOARD_COUNT];/* Array of control handles */
MPIMotion motion[BOARD_COUNT]; /* Array of motion handles */
MPIAxis axis[BOARD_COUNT]; /* Array of axis handles */
MPINotify notify[BOARD_COUNT]; /* Array of notify handles */
MPIEventMgr eventMgr; /* Array of event manager handles */
MPIEventMask eventMask;
MPIControlAddress address;
long returnValue;
long argIndex=0;
long index;
/* 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 - 1) || /* minus application name */
(axisNumber >= MPIXmpMAX_Axes) ||
(motionNumber >= MPIXmpMAX_MSs) ||
(motionType < MPIMotionTypeFIRST) ||
(motionType >= MPIMotionTypeLAST)) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n"
"\t\t[-type # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - 1,
MPIXmpMAX_MSs - 1,
MPIMotionTypeLAST - 1);
exit(MPIMessageARG_INVALID);
}
switch (motionType) {
case MPIMotionTypeS_CURVE:
case MPIMotionTypeTRAPEZOIDAL:
case MPIMotionTypeVELOCITY: {
break;
}
default: {
mpiPlatformConsole("%s: %d: motion type not available\n",
argv[0],
motionType);
exit(MPIMessageUNSUPPORTED);
break;
}
}
/*
Initialize each controller with default settings,
specifying the board's controller number.
*/
for (index = 0; index < BOARD_COUNT; index++) {
address.number = controlNumber + index; /* set controller number */
controller[index] =
mpiControlCreate(MPIControlTypeDEFAULT,
&address);
msgCHECK(mpiControlValidate(controller[index]));
}
/* Initialize the controllers */
for (index = 0; index < BOARD_COUNT; index++) {
returnValue = mpiControlInit(controller[index]);
if (returnValue != MPIMessageOK) {
fprintf(stderr,
"Controller #%d : mpiControlInit(0x%x) returns 0x%x: %s\n",
index,
controller[index],
returnValue,
mpiMessage(returnValue, NULL));
exit(1);
}
}
/* Obtain Axis and Motion handle for each board */
for (index = 0; index < BOARD_COUNT; index++) {
axis[index] =
mpiAxisCreate(controller[index],
axisNumber);
msgCHECK(mpiAxisValidate(axis[index]));
/* Create motion object, append axis */
motion[index] =
mpiMotionCreate(controller[index],
motionNumber,
axis[index]);
msgCHECK(mpiMotionValidate(motion[index]));
}
/* Setup one Event Manager for all boards */
/* Create event manager object */
eventMgr = mpiEventMgrCreate(controller[0]);
msgCHECK(mpiEventMgrValidate(eventMgr));
for (index = 1; index < BOARD_COUNT; index++) {
/* Append additional controllers to the event manager */
returnValue =
mpiEventMgrControlAppend(eventMgr,
controller[index]);
msgCHECK(returnValue);
}
/* Flush any existing events */
returnValue = mpiEventMgrFlush(eventMgr);
msgCHECK(returnValue);
/* Setup Notify Object */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskSET(eventMask, MPIEventTypeMOTION_DONE);
for (index = 0; index < BOARD_COUNT; index++) {
/* Request notification of Motion Done events from motion */
returnValue =
mpiMotionEventNotifySet(motion[index],
eventMask,
NULL);
msgCHECK(returnValue);
/* Create event notification object for motion */
notify[index] =
mpiNotifyCreate(eventMask,
motion[index]);
msgCHECK(mpiNotifyValidate(notify[index]));
/* Add notify to event manager's list */
returnValue =
mpiEventMgrNotifyAppend(eventMgr,
notify[index]);
msgCHECK(returnValue);
}
/* Clear the command and actual positions on each controller */
for (index = 0; index < BOARD_COUNT; index++) {
double command;
returnValue =
mpiAxisCommandPositionGet(axis[index],
&command);
msgCHECK(returnValue);
returnValue =
mpiAxisOriginSet(axis[index],
command);
msgCHECK(returnValue);
/* clear position error */
returnValue =
mpiAxisActualPositionSet(axis[index],
0.0);
msgCHECK(returnValue);
}
/* Sequentially start motion */
for (index = 0; index < BOARD_COUNT; index++) {
MPIMotionParams motionParams;
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[0];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[0];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[0];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
/* Start motion */
returnValue =
mpiMotionStart(motion[index],
motionType,
&motionParams);
fprintf(stderr,
"mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion[index],
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
switch (returnValue) {
case MPIMotionMessageERROR: {
returnValue =
mpiMotionAction(motion[index],
MPIActionRESET);
fprintf(stderr,
"mpiMotionAction(0x%x, RESET) returns 0x%x\n",
motion[index],
returnValue);
msgCHECK(returnValue);
/* FALL THROUGH */
}
case MPIMotionMessageMOVING: {
returnValue = MPIMessageOK;
break;
}
case MPIMessageOK:
default: {
break;
}
}
/* Collect motion events (Polling Event Manager)*/
while (TRUE) {
MPIEventStatus eventStatus;
/* Obtain firmware event(s) (if any) */
returnValue =
mpiEventMgrService(eventMgr,
MPIHandleVOID);
msgCHECK(returnValue);
/* Poll for motion event */
returnValue =
mpiNotifyEventWait(notify[index],
&eventStatus,
MPIWaitPOLL);
if (returnValue == MPIMessageTIMEOUT) {
/* no events have happend yet */
returnValue = MPIMessageOK;
}
if (returnValue == MPIMessageOK) {
if (eventStatus.type == MPIEventTypeMOTION_DONE) {
/* Caught the motion done event */
printf("Motion Done: type %d source 0x%x info 0x%x\n",
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
break;
}
}
else {
printf("%d\n",returnValue);
mpiAssert(FALSE);
}
}
}
/* Delete the EventMgr handle (must happen before removing Notify) */
returnValue = mpiEventMgrDelete(eventMgr);
msgCHECK(returnValue);
/* Delete the Notify handles */
for (index = 0; index < BOARD_COUNT ;index++) {
returnValue = mpiNotifyDelete(notify[index]);
msgCHECK(returnValue);
}
/* Delete the Motion handles */
for ( index = 0; index < BOARD_COUNT ;index++) {
returnValue = mpiMotionDelete(motion[index]);
msgCHECK(returnValue);
}
/* Delete the Axis handles */
for (index = 0; index < BOARD_COUNT; index++) {
returnValue = mpiAxisDelete(axis[index]);
msgCHECK(returnValue);
}
/* Delete the CONTROL handles */
for (index = 0; index < BOARD_COUNT; index++) {
returnValue = mpiControlDelete(controller[index]);
msgCHECK(returnValue);
}
return ((int)returnValue);
}
|
