Watchdog3.c -- Set up a watchdog by using user buffer fields, user limits, and a sequencer
/* Watchdog3.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.
*/
/*
: Set up a watchdog by using user buffer fields, user limits, and a sequencer
Sometimes there is a need to stop or abort motion on all axes if an application
dies. The way to set this up is via an application watchdog. This is where
an application sets a value to a specific controller field. The controller
monitors that field and will stop and/or abort all motion if the application
fails to set the value of the watchdog field.
This application sets up a watchdog by using 2 user buffer fields, 2 user limits
for watchdog monitoring, and 1 user limit per motor to abort motion.
User two user buffer fields:
User Buffer Watchdog Field: The application must periodically set this field
to zero.
User Buffer Abort Field: When this field is set to a non-zero value, axes
will abort.
Set up 2 user limits:
Limit A: Monitor user buffer watchdog field. If the watchdog field is
non-zero for more than a certain period of time, set the abort field
to a non-zero value.
Limit B: Must follow Limit A -- prefferably the immediate next user limit.
Always evaluates TRUE, and sets the watchdog field to a non-zero
value.
Shutdown procedure implemented by:
A sequence that will perform watchdog action (usually abort) on all motions if
the user buffer abort field is set to a non-zero value
Please see the following sample applications for more information on the
techniques used within this sample application:
User Limits: usrlim1.c, usrlim4.c
Sequencer: seqkill.c
Watchdog3.c is one of three sample applications that implement a watchdog. Each
is identical, except for the method of the shutdown procedure. Here is a
comparison of the three sample applications:
Watchdog1.c
Shutdown procedure implemented by one user limit on each motor that will
perform the watchdog action if user buffer field B is set to a non-zero
value.
Advantages: Does not use the SynqNet Node user faults
Evaluated in one background cycle
Disadvantages: Uses one user limit per motor. However the limits are
plentiful -- 16 per motor.
Watchdog2.c
Shutdown procedure implemented by having all SynqNet Node user fault
settings to watch user buffer field B. Then set all motor user fault
actions to desired watchdog action (usually abort).
Advantages: Only uses two user limits total -- no additional user limits
Evaluated in one background cycle
Disadvantages: Does the SynqNet Node user fault for every node. The user
faults cannot be used for any other functionality.
Watchdog3.c
Shutdown procedure implemented by a sequencer that will perform the
watchdog action on all motions if user buffer field B is set to a non-zero
value
Advantages: Does not use the SynqNet Node user faults
Only uses two user limits total -- no additional user limits
Disadvantages: Comparitively slow compared to other watchdog implementation
Evaluated in (number of motion supervisors + 1) background
cycles.
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.
The msgCHECK(...) macros used in the following sample code are intended
to convey our strong belief that ALL error return codes should be checked.
Actual application code should use specific error handling techniques (other
than msgCHECKs) best suited to your internal error recovery methods.
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
/* Watchdog time in seconds */
#define WATCHDOG_TIME (0.2)
/* User buffer addresses */
#define USER_BUFFER_INDEX_WATCHDOG (0)
#define USER_BUFFER_INDEX_ABORT (1)
/* Action to take place when watchdog fails */
#define WATCHDOG_ACTION MPIActionE_STOP_ABORT
/* User Limits needed to implement watchdog */
/* Used only by helper motor */
#define WATCHDOG_MONITOR_LIMIT MEIEventTypeLIMIT_USER1
/* Used only by helper motor */
#define WATCHDOG_RESET_LIMIT MEIEventTypeLIMIT_USER2
/* Helper motor to setup the two helper user limits */
#define WATCHDOG_HELPER_MOTOR (0)
#define SEQUENCE_NUMBER (-1) /* next available sequencer */
typedef struct WatchDogConfigData
{
MPIControl control;
MPIMotor* motor;
long motorCount;
MPIMotion* motion;
long motionCount;
MPISequence sequence;
double watchdog_time;
long user_buffer_index_watchdog;
long user_buffer_index_abort;
MPIAction watchdog_action;
MEIEventType watchdog_monitor_limit;
MEIEventType watchdog_reset_limit;
long helper_motor;
} WatchDogConfigData;
typedef struct WatchDogData
{
MPIControl control;
long* watchDogField;
} WatchDogData;
/* Perform basic command line parsing. (-control -server -port -trace) */
void basicParsing(int argc,
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress)
{
long argIndex;
/* Parse command line for Control type and address */
argIndex = argControl(argc, argv, controlType, controlAddress);
/* Check for unknown/invalid command line arguments */
if (argIndex < argc) {
fprintf(stderr,"usage: %s %s\n", argv[0], ArgUSAGE);
exit(MPIMessageARG_INVALID);
}
}
/* Create and initialize MPI objects */
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIMotion *motion,
long *numberOfMotions,
MPIMotor *motor,
long *numberOfMotors,
MPISequence *sequence,
long sequenceNumber)
{
MPIControlConfig controlConfig;
long index;
long returnValue;
/* Create motion controller object */
*control =
mpiControlCreate(controlType, controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
/* Obtain controller configuration */
returnValue =
mpiControlConfigGet(*control, &controlConfig, NULL);
msgCHECK(returnValue);
/* Record the number of enabled motors */
*numberOfMotors = controlConfig.motorCount;
/* Create motor objects */
for (index = 0; index < controlConfig.motorCount; index++)
{
motor[index] =
mpiMotorCreate(*control, index);
msgCHECK(mpiMotorValidate(motor[index]));
}
/* Record the number of enabled motions */
*numberOfMotions = controlConfig.motionCount;
/* Create motion objects */
for (index = 0; index < controlConfig.motionCount; index++)
{
motion[index] =
mpiMotionCreate(*control, index, MPIHandleVOID);
msgCHECK(mpiMotionValidate(motion[index]));
}
/* Make sure a sequencer is enabled */
if (controlConfig.sequenceCount == 0)
{
controlConfig.sequenceCount = 1;
returnValue =
mpiControlConfigSet(*control, &controlConfig, NULL);
msgCHECK(returnValue);
}
/* Create sequencer object */
*sequence =
mpiSequenceCreate(*control, sequenceNumber, 2+(*numberOfMotions));
msgCHECK(mpiSequenceValidate(*sequence));
}
/*
Perform certain cleanup actions and delete MPI objects
Deleting the program sequencer will stop the sequence. Then the sequencer
will not be able to perform its watchdog duties. For this reason, we
will not delete the sequencer.
*/
void programCleanup(MPIControl *control,
MPIMotion *motion,
long numberOfMotions,
MPIMotor *motor,
long numberOfMotors)
{
long index;
long returnValue;
/* Delete motion objects */
for (index = 0; index < numberOfMotions; index++)
{
returnValue =
mpiMotionDelete(motion[index]);
msgCHECK(returnValue);
motion[index] = MPIHandleVOID;
}
/* Delete motor objects */
for (index = 0; index < numberOfMotors; index++)
{
returnValue =
mpiMotorDelete(motor[index]);
msgCHECK(returnValue);
motor[index] = MPIHandleVOID;
}
/* Delete motion controller object */
returnValue =
mpiControlDelete(*control);
msgCHECK(returnValue);
*control = MPIHandleVOID;
}
/*
sequenceCommandAdd() creates a command (MPICommand object) and appends it to
sequence's list of commands. This function does return MPI error codes so
that sequence setup code may be more easily debugged.
Warning: sequenceCommandAdd() does not keep track of created command objects
so it is important that before a sequence object is deleted, that each
command object on sequence's list is itself deleted. Otherwise, there will
be memory leaks. One can use sequenceProgramDelete() to accomplish this.
*/
long sequenceCommandAdd(MPISequence sequence,
MPICommandType commandType,
MPICommandParams *commandParams,
const char *label)
{
MPICommand command;
long returnValue;
command =
mpiCommandCreate(commandType,
commandParams,
label);
returnValue =
mpiCommandValidate(command);
if (returnValue == MPIMessageOK) {
returnValue =
mpiSequenceCommandAppend(sequence,
command);
}
return returnValue;
}
/* Setup watchdog on the controller and return watchdog data */
void setupWatchdog(WatchDogConfigData* watchdogConfig,
WatchDogData* watchdogData)
{
MEIXmpBufferData* external;
MEIMotorEventConfig motorEventConfig;
MPICommandParams commandParams;
MPICommandMotion motionCommand;
MEIMotorConfig meiMotorConfig;
MPIMotion* motion = watchdogConfig->motion;
MPIMotor* motor = watchdogConfig->motor;
MPISequence sequence = watchdogConfig->sequence;
MEIXmpLimit xmpMonitorLimit;
double sampleRate;
long waitSamples;
long* abortAddr;
long motorIndex;
long motionIndex;
long zero = 0;
long returnValue;
/* Obtain controller memory addresses */
returnValue =
mpiControlMemory(watchdogConfig->control,
NULL,
&external);
msgCHECK(returnValue);
/* Obtain controller sample rate */
returnValue =
meiControlSampleRate(watchdogConfig->control,
&sampleRate);
msgCHECK(returnValue);
waitSamples = (long) ceil(watchdogConfig->watchdog_time * sampleRate);
/* Write the watchdog data */
watchdogData->control = watchdogConfig->control;
watchdogData->watchDogField =
&external->UserBuffer.Data[watchdogConfig->user_buffer_index_watchdog];
/* Calculate abort field address */
abortAddr =
&external->UserBuffer.Data[watchdogConfig->user_buffer_index_abort];
/* Setup watchdog monitor limit */
returnValue =
mpiMotorEventConfigGet(motor[watchdogConfig->helper_motor],
watchdogConfig->watchdog_monitor_limit,
NULL,
&motorEventConfig);
msgCHECK(returnValue);
motorEventConfig.Logic = MEIXmpLogicSINGLE +
(waitSamples << MEIXmpLogicBITS);
motorEventConfig.Status = MEIXmpStatusLIMIT;
motorEventConfig.Condition[0].Type = MEIXmpLimitTypeNE;
motorEventConfig.Condition[0].SourceAddress = watchdogData->watchDogField;
motorEventConfig.Condition[0].Mask = 0xFFFFFFFF;
motorEventConfig.Condition[0].LimitValue.g32.l = 0;
motorEventConfig.Condition[1].Type = MEIXmpLimitTypeFALSE;
motorEventConfig.Condition[1].SourceAddress = NULL;
motorEventConfig.Condition[1].Mask = 0;
motorEventConfig.Condition[1].LimitValue.g32.l = 0;
motorEventConfig.Output.OutputPtr = abortAddr;
motorEventConfig.Output.AndMask = 0;
motorEventConfig.Output.OrMask = 1;
motorEventConfig.Output.Enabled = TRUE;
returnValue =
mpiMotorEventConfigSet(motor[watchdogConfig->helper_motor],
watchdogConfig->watchdog_monitor_limit,
NULL,
&motorEventConfig);
msgCHECK(returnValue);
/* Setup watchdog reset limit */
returnValue =
mpiMotorEventConfigGet(motor[watchdogConfig->helper_motor],
watchdogConfig->watchdog_reset_limit,
NULL,
&motorEventConfig);
msgCHECK(returnValue);
xmpMonitorLimit = meiXmpLimitFromMPIEventType(watchdogConfig->watchdog_monitor_limit) - MEIXmpLimitDEDICATED_LAST;
motorEventConfig.Logic = MEIXmpLogicSINGLE;
motorEventConfig.Status = MEIXmpStatusLIMIT;
motorEventConfig.Condition[0].Type = MEIXmpLimitTypeEQ;
motorEventConfig.Condition[0].SourceAddress = &external->UserLimit[watchdogConfig->helper_motor].Limit[xmpMonitorLimit].Count;
motorEventConfig.Condition[0].Mask = 0xFFFFFFFF;
motorEventConfig.Condition[0].LimitValue.g32.l = 0;
motorEventConfig.Condition[1].Type = MEIXmpLimitTypeFALSE;
motorEventConfig.Condition[1].SourceAddress = NULL;
motorEventConfig.Condition[1].Mask = 0;
motorEventConfig.Condition[1].LimitValue.g32.l = 0;
motorEventConfig.Output.OutputPtr = watchdogData->watchDogField;
motorEventConfig.Output.AndMask = 0;
motorEventConfig.Output.OrMask = 1;
motorEventConfig.Output.Enabled = TRUE;
returnValue =
mpiMotorEventConfigSet(motor[watchdogConfig->helper_motor],
watchdogConfig->watchdog_reset_limit,
NULL,
&motorEventConfig);
msgCHECK(returnValue);
/*
Set the monitor and abort field to zero.
This clears any past watchdog failures.
*/
returnValue =
mpiControlMemorySet(watchdogConfig->control,
watchdogData->watchDogField,
&zero,
sizeof(zero));
msgCHECK(returnValue);
returnValue =
mpiControlMemorySet(watchdogConfig->control,
abortAddr,
&zero,
sizeof(zero));
msgCHECK(returnValue);
/* Set all motor user fault actions */
for (motorIndex=0; motorIndex<watchdogConfig->motorCount; ++motorIndex)
{
returnValue =
mpiMotorConfigGet(motor[motorIndex],
NULL,
&meiMotorConfig);
msgCHECK(returnValue);
meiMotorConfig.userFaultAction = watchdogConfig->watchdog_action;
returnValue =
mpiMotorConfigSet(motor[motorIndex],
NULL,
&meiMotorConfig);
msgCHECK(returnValue);
}
/* Setup sequencer */
/* Determine correct action for sequencer */
switch (watchdogConfig->watchdog_action)
{
case MPIActionSTOP:
motionCommand = MPICommandMotionSTOP;
break;
case MPIActionE_STOP:
motionCommand = MPICommandMotionE_STOP;
break;
case MPIActionE_STOP_ABORT:
motionCommand = MPICommandMotionE_STOP_ABORT;
break;
case MPIActionABORT:
motionCommand = MPICommandMotionABORT;
break;
default:
fprintf(stderr, "Unknown action for watchdog failure\n");
exit(-1);
}
/* Wait for the abort field to be set to a non-zero value */
commandParams.wait.expr.oper = MPICommandOperatorNOT_EQUAL;
commandParams.wait.expr.address.l = abortAddr;
commandParams.wait.expr.by.value.l = 0;
returnValue =
sequenceCommandAdd(sequence,
MPICommandTypeWAIT,
&commandParams,
"Start");
msgCHECK(returnValue);
for (motionIndex=0; motionIndex<watchdogConfig->motionCount; ++motionIndex)
{
/* command watchdog action */
commandParams.motion.motionCommand = motionCommand;
commandParams.motion.motion = motion[motionIndex];
returnValue =
sequenceCommandAdd(sequence,
MPICommandTypeMOTION,
&commandParams,
NULL);
msgCHECK(returnValue);
}
/* Goto the first statement */
commandParams.branch.label = "Start";
commandParams.branch.expr.oper = MPICommandOperatorALWAYS;
returnValue =
sequenceCommandAdd(sequence,
MPICommandTypeBRANCH,
&commandParams,
NULL);
msgCHECK(returnValue);
/* Start sequence */
returnValue =
mpiSequenceStart(sequence,
MPIHandleVOID);
msgCHECK(returnValue);
}
/* Set the watchdog field to zero */
void setWatchdogField(WatchDogData* watchdogData)
{
long zero = 0;
long returnValue;
returnValue =
mpiControlMemorySet(watchdogData->control,
watchdogData->watchDogField,
&zero,
sizeof(zero));
msgCHECK(returnValue);
}
/* Set Watchdog Until a Key is Pressed */
void setWatchdogUntilKeypress(WatchDogData* watchdogData,
double delay /* seconds */)
{
long delayMilliseonds = (long) (delay*1000);
do
{
setWatchdogField(watchdogData);
} while(meiPlatformKey(MPIWaitMSEC*delayMilliseonds) < 0);
}
int main(int argc,
char *argv[])
{
MPIControl control;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIMotion motion[MEIXmpMAX_MSs];
MPIMotor motor[MEIXmpMAX_Motors];
MPISequence sequence;
WatchDogConfigData watchdogConfig;
WatchDogData watchdogData;
long motionCount;
long motorCount;
/* Perform basic command line parsing. (-control -server -port -trace) */
basicParsing(argc,
argv,
&controlType,
&controlAddress);
/* Create and initialize MPI objects */
programInit(&control,
controlType,
&controlAddress,
motion,
&motionCount,
motor,
&motorCount,
&sequence,
SEQUENCE_NUMBER);
/* Set up watchdog configuration */
watchdogConfig.control = control;
watchdogConfig.motor = motor;
watchdogConfig.motorCount = motorCount;
watchdogConfig.motion = motion;
watchdogConfig.motionCount = motionCount;
watchdogConfig.sequence = sequence;
watchdogConfig.watchdog_time = WATCHDOG_TIME;
watchdogConfig.user_buffer_index_watchdog = USER_BUFFER_INDEX_WATCHDOG;
watchdogConfig.user_buffer_index_abort = USER_BUFFER_INDEX_ABORT;
watchdogConfig.watchdog_action = WATCHDOG_ACTION;
watchdogConfig.watchdog_monitor_limit = WATCHDOG_MONITOR_LIMIT;
watchdogConfig.watchdog_reset_limit = WATCHDOG_RESET_LIMIT;
watchdogConfig.helper_motor = WATCHDOG_HELPER_MOTOR;
/* Setup watchdog on the controller and return watchdog data */
setupWatchdog(&watchdogConfig, &watchdogData);
/* Set Watchdog Until a Key is Pressed */
printf("\nWatchdog is setup. Press any key to quit...\n\n");
setWatchdogUntilKeypress(&watchdogData, WATCHDOG_TIME/2);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(&control,
motion,
motionCount,
motor,
motorCount);
return MPIMessageOK;
}
|
