motMap2.c -- Motion object and Motion Supervisor Axis mapping with
events
/* motMap2.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.
*/
/*
:Motion object and Motion Supervisor Axis mapping with events
This sample code performs the following operation:
-Map axis 0 to motion object 'motion'.
-Move axis 0 to position.
-Wait for an event based upon motion done on axis 0
-Move axis 1 to position
-Wait for an event based upon motion done on axis 1
-Map axis 0 and axis 1 to 'motion'
-Coordinated move on axis 0 and axis 1 back to position (0,0).
Only one Motion Supervisor and Motion object is used and remapped after
each move.
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"
#define AXIS_COUNT (2)
#define MS_COUNT (1)
#define MOTION_COUNT (3)
#define COUNT (100)
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, 1, };
long motionSupNumber[MS_COUNT] = { 0,};
MPIMotionType motionType = MPIMotionTypeS_CURVE;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionSupNumber[0], },
{ "-type", ArgTypeLONG, &motionType, },
{ NULL, ArgTypeINVALID, NULL, }
};
double position[MOTION_COUNT][AXIS_COUNT] = {
{ 10000.0,},
{ 10000.0,},
{ 0.0,},
};
MPITrajectory trajectory[MOTION_COUNT][AXIS_COUNT] = {
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
};
/* Motion parameters */
MPIMotionSCurve sCurve[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
{ &trajectory[2][0], &position[2][0], },
};
MPIMotionTrapezoidal trapezoidal[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
{ &trajectory[2][0], &position[2][0], },
};
MPIMotionVelocity velocity[MOTION_COUNT] = {
{ &trajectory[0][0], },
{ &trajectory[1][0], },
{ &trajectory[2][0], },
};
int
main(int argc,
char *argv[])
{
MPIControl control; /* Motion controller handle */
MPIAxis axisList[AXIS_COUNT]; /* Axis handle(s) */
MPIMotion motion; /* Motion object */
MPIMotionParams motionParams;
MPINotify notify; /* event notification object */
MPIEventMgr eventMgr; /* event manager handle */
MPIEventMask eventMask;
MPIEventStatus eventStatus;
long returnValue; /* Return value from library */
long argIndex;
Service service;
MPIControlType controlType;
MPIControlAddress controlAddress;
/* 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] > (MPIXmpMAX_Axes - AXIS_COUNT)) ||
(motionSupNumber[0] > (MPIXmpMAX_MSs - MS_COUNT)) ||
(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 - AXIS_COUNT,
MPIXmpMAX_MSs - MS_COUNT,
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;
}
}
/* Create control object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize motion controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
axisNumber[1] = axisNumber[0] + 1;
/* Create axis object using axisNumber on controller */
axisList[0] =
mpiAxisCreate(control,
axisNumber[0]);
msgCHECK(mpiAxisValidate(axisList[0]));
/* Create axis object using axisNumber on controller */
axisList[1] =
mpiAxisCreate(control,
axisNumber[1]);
msgCHECK(mpiAxisValidate(axisList[1]));
/* Create motion object motion for axisList[0] */
motion =
mpiMotionCreate(control,
0, /* Motion supervisor number */
axisList[0]);
msgCHECK(mpiMotionValidate(motion));
/* Create event notification object for events from all sources */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskALL(eventMask);
mpiEventMaskALL(eventMask);
notify =
mpiNotifyCreate(eventMask,
NULL);
msgCHECK(mpiNotifyValidate(notify));
/* Create event manager object */
eventMgr =
mpiEventMgrCreate(control);
msgCHECK(mpiEventMgrValidate(eventMgr));
/* Flush any existing events */
returnValue =
mpiEventMgrFlush(eventMgr);
msgCHECK(returnValue);
/* Add notify to event manager's list */
returnValue =
mpiEventMgrNotifyAppend(eventMgr,
notify);
msgCHECK(returnValue);
/* Request notification of events from motion */
returnValue =
mpiMotionEventNotifySet(motion,
eventMask,
NULL);
msgCHECK(returnValue);
/* Create service thread */
service =
serviceCreate(eventMgr,
-1, /* default (max) priority */
-1); /* -1 => enable interrupts */
mpiAssert(service != NULL);
/* Clear any error conditions on motion */
returnValue =
mpiMotionAction(motion,
MPIActionRESET);
msgCHECK(returnValue);
/* Move axis 0 */
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;
}
}
printf("\nMoving axis 0...\n");
returnValue =
mpiMotionStart(motion,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
while (returnValue == MPIMessageOK) {
/* Wait for axis and motion event */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
fprintf(stderr,
"mpiNotifyEventWait(0x%x, 0x%x, %d) returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
notify,
&eventStatus,
MPIWaitFOREVER,
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
if (eventStatus.type == MPIEventTypeMOTION_DONE) {
printf("Finished axis 0 move.\n");
break;
}
else {
continue;
}
}
/* Map axisList[1] to motion object */
returnValue =
mpiMotionAxisListSet(motion,
1,
&axisList[1]);
msgCHECK(returnValue);
/* Request notification of events from motion */
returnValue =
mpiMotionEventNotifySet(motion,
eventMask,
NULL);
msgCHECK(returnValue);
/* Move axis 1 */
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[1];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[1];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[1];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
printf("\nMoving Axis 1...\n");
returnValue =
mpiMotionStart(motion,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
while (returnValue == MPIMessageOK) {
/* Wait for axis and motion event */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
fprintf(stderr,
"mpiNotifyEventWait(0x%x, 0x%x, %d) returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
notify,
&eventStatus,
MPIWaitFOREVER,
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
if (eventStatus.type == MPIEventTypeMOTION_DONE) {
printf("Finished axis 1 move.\n");
break;
}
else {
continue;
}
}
/* Map motion for axis 0 and axis 1 */
returnValue =
mpiMotionAxisListSet(motion,
2,
axisList);
msgCHECK(returnValue);
/* Request notification of events from motion */
returnValue =
mpiMotionEventNotifySet(motion,
eventMask,
NULL);
msgCHECK(returnValue);
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[2];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[2];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[2];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
printf("\nMoving Axes 0 and 1...\n");
returnValue =
mpiMotionStart(motion,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
/* Wait for motion to complete */
while (returnValue == MPIMessageOK) {
/* Wait for axis and motion event */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
fprintf(stderr,
"mpiNotifyEventWait(0x%x, 0x%x, %d) returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
notify,
&eventStatus,
MPIWaitFOREVER,
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
if (eventStatus.type == MPIEventTypeMOTION_DONE) {
printf("Finished axis 0 and 1 move.\n");
break;
}
else {
continue;
}
}
/* Delete Objects */
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisList[0]);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisList[1]);
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);
}
|
