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] > (MPIXmpMAX_Axes - AXIS_COUNT)) ||
(motionNumber >= MPIXmpMAX_MSs) ||
(motionType < MPIMotionTypeFIRST) ||
(motionType >= MPIMotionTypeLAST) ||
(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,
MPIXmpMAX_Axes - AXIS_COUNT,
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;
}
}
/* 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);
mpiEventMaskALL(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 */
mpiAssert(service != NULL);
mpiPlatformConsole("Press any key to quit ...\n");
/* Loop repeatedly */
index = 0;
actionIndex = 0;
settleIndex = 0;
motionDone = TRUE;
while (mpiPlatformKey(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: {
mpiAssert(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);
}
}
mpiPlatformConsole("\nMotion Start, Target= %12.0lf ",
position[index][axisNumber[0]]);
mpiPlatformConsole(" 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 */
mpiPlatformSleep(eventDelay);
/* Generate an event */
returnValue =
mpiMotionAction(motion,
action[actionIndex]);
msgCHECK(returnValue);
mpiPlatformConsole(" 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)
{
MPIEventStatusInfo *info;
info = (MPIEventStatusInfo *)status.info;
switch (status.type) {
/* In Coarse Event from axis source */
case MPIEventTypeIN_POSITION_COARSE: {
mpiPlatformConsole(" InCoarse, actual:%ld, sample:0x%x\n",
info->data.axis.actualPosition,
info->data.axis.sampleCounter);
break;
}
/* In Fine Event from axis source */
case MPIEventTypeIN_POSITION_FINE: {
mpiPlatformConsole(" InFine, actual:%ld, sample:0x%x\n",
info->data.axis.actualPosition,
info->data.axis.sampleCounter);
break;
}
/* At Target Event from axis source */
case MPIEventTypeAT_TARGET: {
mpiPlatformConsole(" 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: {
mpiPlatformConsole(" Done, sample:0x%x\n",
info->data.motion.sampleCounter);
break;
}
default: {
mpiPlatformConsole("Event (unexpected): %ld (0x%x)",
status.type,
info->data.word[0]);
break;
}
}
}
|
