UsrLim4.c -- Position Comparison using User Limits to generate an
abort on excessive torque.
/* UsrLim4.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.
*/
/*
:Position comparison using User Limits to generate an abort on excessive torque.
This sample code shows how to configure the XMP controller's User Limits
to cause an abort event to occur when the commanded torque exceeds a limit
for more than a specified amount of time.
Normally a control system can be assumed to not exceed a commanded torque
threshold for very long in normal operation. An example would be commanding
a motion a very small distance past a hard stop. The integral can cause a full
torque command even though only a couple of counts of position error exist. By
causing an abort on full torque command for more than a fraction of a second,
this and other conditions can be guarded against.
The torque limits are separately configurable for high and low limits.
The method for requiring a user limit to be satisfied continuously for
some amount of time is demonstrated in SetTorqueLimits(). Causing an event
to occurr from the user limit is also shown in the same function.
The eventType argument in SetTorqueLimits sets the user limit number. You
can choose from the following list:
MEIEventTypeLIMIT_USER0,
MEIEventTypeLIMIT_USER1,
MEIEventTypeLIMIT_USER2,
MEIEventTypeLIMIT_USER3,
MEIEventTypeLIMIT_USER4,
MEIEventTypeLIMIT_USER5,
MEIEventTypeLIMIT_USER6,
MEIEventTypeLIMIT_USER7,
MEIEventTypeLIMIT_USER8,
MEIEventTypeLIMIT_USER9,
MEIEventTypeLIMIT_USER10,
MEIEventTypeLIMIT_USER11,
MEIEventTypeLIMIT_USER12,
MEIEventTypeLIMIT_USER13,
MEIEventTypeLIMIT_USER14,
MEIEventTypeLIMIT_USER15,
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 COMMAND_HIGH (32767.0)
#define COMMAND_LOW (-32767.0)
#define DURATION (0.5) /* Seconds */
/* Parse command line */
long parseCommandLine(int argc,
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress,
long *motorNumber)
{
long argIndex;
/* Application-specific command line arguments */
Arg argList[] =
{
{ "-motor", ArgTypeLONG, motorNumber, },
{ NULL, ArgTypeINVALID, NULL, }
};
/* 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) ||
(*motorNumber >= MEIXmpMAX_Motors))
{
fprintf(stderr,"usage: %s %s\n"
"\t\t[-motor # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
(MEIXmpMAX_Motors - 1));
exit(MPIMessageARG_INVALID);
}
return 0;
}
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIMotor *motor,
long motorNumber)
{
long returnValue;
/* Create motion controller object */
*control = mpiControlCreate(controlType, controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue = mpiControlInit(*control);
msgCHECK(returnValue);
/* Create motor object for motorNumber */
*motor = mpiMotorCreate(*control, motorNumber);
msgCHECK(mpiMotorValidate(*motor));
}
void programCleanup(MPIControl *control,
MPIMotor *motor)
{
long returnValue;
/* Delete objects */
returnValue = mpiMotorDelete(*motor);
msgCHECK(returnValue);
*motor = MPIHandleVOID;
returnValue = mpiControlDelete(*control);
msgCHECK(returnValue);
*control = MPIHandleVOID;
}
/*
EventType is MEIEventTypeLIMIT_USERn -- Choose the user limit you want to use.
The axis will abort out after continually exceeding the high or low limit for
duration seconds.
*/
long SetTorqueLimits(MPIMotor motor,
MEIEventType eventType,
float highLimit,
float lowLimit,
float duration) /* in seconds */
{
MPIControl control;
MEIXmpData *firmware;
MEIMotorEventConfig motorEventConfig;
MEIPlatform platform;
long returnValue;
long samples;
long ptr;
long motorNum;
/* Determine control handle */
control = mpiMotorControl(motor);
returnValue = meiControlSecondsToSamples(control, duration, &samples);
if(returnValue == MPIMessageOK)
{
returnValue = mpiMotorNumber(motor, &motorNum);
}
/* If duration is out of range, make it zero */
if(duration > (MEIXmpLogicDURATION_MASK >> MEIXmpLogicBITS) || duration < 0)
{
samples = 0;
}
/* Get pointer to XMP firmware */
if(returnValue == MPIMessageOK)
{
returnValue =
mpiControlMemory(control,
(void **)&firmware,
NULL);
}
/* The source address requires a host pointer, not a controller pointer */
platform = meiControlPlatform(control);
returnValue =
meiPlatformMemoryToHost(platform,
(void**)firmware->Motor[motorNum].Commutation.Input[0].Ptr,
(void**)&ptr);
if (returnValue == MPIMessageOK)
{
returnValue =
mpiMotorEventConfigGet(motor,
(MPIEventType)eventType,
NULL,
&motorEventConfig);
}
if (returnValue == MPIMessageOK)
{
/*
Set up the Condition blocks. They watch for the high and
low command levels
*/
motorEventConfig.Condition[0].Type = MEIXmpLimitTypeFGE;
motorEventConfig.Condition[0].SourceAddress = (void *)ptr;
motorEventConfig.Condition[0].LimitValue.g32.f = highLimit;
motorEventConfig.Condition[1].Type = MEIXmpLimitTypeFLE;
motorEventConfig.Condition[1].SourceAddress = (void *)ptr;
motorEventConfig.Condition[1].LimitValue.g32.f = lowLimit;
/* Abort on this condition */
motorEventConfig.Status = MEIXmpStatusABORT;
/*
Determine logic result from Condition[0] OR Condition[1].
The duration is logically combined with the logic.
*/
motorEventConfig.Logic = MEIXmpLogicOR | (samples << MEIXmpLogicBITS);
/* Disable the output feature */
motorEventConfig.Output.OutputPtr = NULL;
motorEventConfig.Output.AndMask = 0x0;
motorEventConfig.Output.OrMask = 0x0;
motorEventConfig.Output.Enabled = 0;
returnValue =
mpiMotorEventConfigSet(motor,
(MPIEventType)eventType,
NULL,
&motorEventConfig);
}
return returnValue;
}
int main(int argc, char *argv[])
{
MPIControl control; /* motion controller handle */
MPIMotor motor; /* motor object */
MPIControlType controlType;
MPIControlAddress controlAddress;
long motorNumber;
long returnValue; /* return value from library */
/* Perform basic command line parsing. (-control -server -port -trace) */
parseCommandLine(argc,
argv,
&controlType,
&controlAddress,
&motorNumber);
programInit(&control,
controlType,
&controlAddress,
&motor,
motorNumber);
/* Configure the Position Compares, using User Limits */
returnValue = SetTorqueLimits(motor,
MEIEventTypeLIMIT_USER0,
COMMAND_HIGH,
COMMAND_LOW,
DURATION);
msgCHECK(returnValue);
programCleanup(&control,
&motor);
return 0;
}
|
