forked from epics_driver_modules/motorBase
Torsten Bögershausen
532afdc5a0
They have no meaning in Git, are not updated and may cause unwanted diffs when otherwise nothing is changed in a file
1099 lines
39 KiB
C
1099 lines
39 KiB
C
/*
|
|
* drvMotorAsyn.c
|
|
*
|
|
* Motor record common Asyn driver support layer
|
|
*
|
|
* Copyright (C) 2005-6 Peter Denison, Diamond Light Source
|
|
*
|
|
* This software is distributed subject to the EPICS Open Licence, which can
|
|
* be found at http://www.aps.anl.gov/epics/licence/open.php
|
|
*
|
|
* Notwithstanding the above, explicit permission is granted for APS to
|
|
* redistribute this software.
|
|
*
|
|
* Derived from ip330 driver from GSE-CARS which was:
|
|
* Original Authors: Jim Kowalkowski, Mark Rivers, Joe Sullivan, and Marty Kraimer
|
|
* ********************COPYRIGHT NOTIFICATION******************************
|
|
* This software was developed under a United States Government license
|
|
* described on the COPYRIGHT_UniversityOfChicago file included as part
|
|
* of this distribution.
|
|
* ************************************************************************
|
|
*
|
|
*
|
|
* Original Author: Peter Denison
|
|
* Current Author: Peter Denison
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
#ifdef vxWorks
|
|
#include <taskLib.h>
|
|
#include <intLib.h>
|
|
#endif
|
|
|
|
#include <iocsh.h>
|
|
#include <epicsThread.h>
|
|
#include <epicsString.h>
|
|
#include <epicsTimer.h>
|
|
#include <epicsMutex.h>
|
|
#include <errlog.h>
|
|
#include <epicsMessageQueue.h>
|
|
#include <errlog.h>
|
|
#include <cantProceed.h> /* !! for callocMustSucceed() */
|
|
|
|
#include <asynDriver.h>
|
|
#include <asynInt32.h>
|
|
#include <asynUInt32Digital.h>
|
|
#include <asynFloat64.h>
|
|
#include <asynFloat64Array.h>
|
|
#include <asynGenericPointer.h>
|
|
#include <asynDrvUser.h>
|
|
|
|
#include <drvSup.h>
|
|
#include <registryDriverSupport.h>
|
|
|
|
#include "asynMotorController.h"
|
|
#include "motor_interface.h"
|
|
#include <limits.h>
|
|
#include <epicsExport.h>
|
|
|
|
/* Message queue size */
|
|
#define MAX_MESSAGES 100
|
|
|
|
#define NMASKBITS (sizeof(int) * CHAR_BIT)
|
|
#define BIT_ISSET(bit, n, mask) ( ((bit)/NMASKBITS < (n)) && \
|
|
((mask)[(bit)/NMASKBITS] & (1 << ((bit) % NMASKBITS))) )
|
|
#define BIT_SET(bit, mask, value) do { \
|
|
(mask)[(bit)/NMASKBITS] &= ~(1 << ((bit) % NMASKBITS)); \
|
|
if (value) { \
|
|
(mask)[(bit)/NMASKBITS] |= (1 << ((bit) % NMASKBITS)); \
|
|
} \
|
|
} while (0);
|
|
|
|
/* Note, these values must not be used for pasynUser->reason in device support */
|
|
typedef enum {
|
|
/* Parameters - these must match the definitions in motor_interface.h */
|
|
motorPosition = motorAxisPosition,
|
|
motorEncRatio = motorAxisEncoderRatio,
|
|
motorResolution = motorAxisResolution,
|
|
motorPGain = motorAxisPGain,
|
|
motorIGain = motorAxisIGain,
|
|
motorDGain = motorAxisDGain,
|
|
motorHighLimit = motorAxisHighLimit,
|
|
motorLowLimit = motorAxisLowLimit,
|
|
motorSetClosedLoop = motorAxisClosedLoop,
|
|
motorEncoderPosition = motorAxisEncoderPosn,
|
|
motorActualVel = motorAxisActualVel,
|
|
motorDeferMoves = motorAxisDeferMoves,
|
|
motorMoveToHome = motorAxisMoveToHome,
|
|
/* Status bits split out */
|
|
motorStatusDirection=motorAxisDirection,
|
|
motorStatusDone, motorStatusHighLimit, motorStatusAtHome,
|
|
motorStatusSlip, motorStatusPowerOn, motorStatusFollowingError,
|
|
motorStatusHome, motorStatusHasEncoder, motorStatusProblem,
|
|
motorStatusMoving, motorStatusGainSupport, motorStatusCommsError,
|
|
motorStatusLowLimit, motorStatusHomed, motorStatusLast,
|
|
/* Not exposed by the driver */
|
|
motorVelocity=100, motorVelBase, motorAccel,
|
|
/* Commands */
|
|
motorMoveRel, motorMoveAbs, motorMoveVel, motorHome, motorStop,
|
|
/* Status readback */
|
|
motorStatus, motorUpdateStatus
|
|
} motorCommand;
|
|
|
|
typedef struct {
|
|
motorCommand command;
|
|
char *commandString;
|
|
} motorCommandStruct;
|
|
|
|
static motorCommandStruct motorCommands[] = {
|
|
{motorMoveRel, motorMoveRelString},
|
|
{motorMoveAbs, motorMoveAbsString},
|
|
{motorMoveVel, motorMoveVelString},
|
|
{motorHome, motorHomeString},
|
|
{motorStop, motorStopString},
|
|
{motorVelocity, motorVelocityString},
|
|
{motorVelBase, motorVelBaseString},
|
|
{motorAccel, motorAccelString},
|
|
{motorPosition, motorPositionString},
|
|
{motorEncoderPosition, motorEncoderPositionString},
|
|
{motorDeferMoves, motorDeferMovesString},
|
|
{motorResolution, motorResolutionString},
|
|
{motorEncRatio, motorEncoderRatioString},
|
|
{motorPGain, motorPGainString},
|
|
{motorIGain, motorIGainString},
|
|
{motorDGain, motorDGainString},
|
|
{motorHighLimit, motorHighLimitString},
|
|
{motorLowLimit, motorLowLimitString},
|
|
{motorSetClosedLoop, motorClosedLoopString},
|
|
{motorDeferMoves, motorDeferMovesString},
|
|
{motorMoveToHome, motorMoveToHomeString},
|
|
{motorStatus, motorStatusString},
|
|
{motorUpdateStatus, motorUpdateStatusString},
|
|
{motorStatusDirection, motorStatusDirectionString},
|
|
{motorStatusDone, motorStatusDoneString},
|
|
{motorStatusHighLimit, motorStatusHighLimitString},
|
|
{motorStatusAtHome, motorStatusAtHomeString},
|
|
{motorStatusSlip, motorStatusSlipString},
|
|
{motorStatusPowerOn, motorStatusPowerOnString},
|
|
{motorStatusFollowingError, motorStatusFollowingErrorString},
|
|
{motorStatusHome, motorStatusHomeString},
|
|
{motorStatusHasEncoder, motorStatusHasEncoderString},
|
|
{motorStatusProblem, motorStatusProblemString},
|
|
{motorStatusMoving, motorStatusMovingString},
|
|
{motorStatusGainSupport, motorStatusGainSupportString},
|
|
{motorStatusCommsError, motorStatusCommsErrorString},
|
|
{motorStatusLowLimit, motorStatusLowLimitString},
|
|
{motorStatusHomed, motorStatusHomedString},
|
|
};
|
|
|
|
typedef enum{typeInt32, typeFloat64, typeFloat64Array} dataType;
|
|
|
|
struct drvmotorPvt;
|
|
|
|
typedef struct drvmotorAxisPvt {
|
|
AXIS_HDL axis;
|
|
int num;
|
|
/* Current parameters for move commands */
|
|
double max_velocity;
|
|
double min_velocity;
|
|
double accel;
|
|
/* Received status */
|
|
MotorStatus status;
|
|
struct drvmotorPvt *pPvt;
|
|
asynUser *pasynUser;
|
|
} drvmotorAxisPvt;
|
|
|
|
typedef struct drvmotorPvt {
|
|
char *portName;
|
|
motorAxisDrvSET_t *drvset;
|
|
int card;
|
|
int numAxes;
|
|
drvmotorAxisPvt *axisData;
|
|
/* Housekeeping */
|
|
epicsMutexId lock;
|
|
int rebooting;
|
|
epicsMessageQueueId intMsgQId;
|
|
int messagesSent;
|
|
int messagesFailed;
|
|
/* Asyn interfaces */
|
|
asynInterface common;
|
|
asynInterface int32;
|
|
void *int32InterruptPvt;
|
|
asynInterface uint32digital;
|
|
void *uint32digitalInterruptPvt;
|
|
asynInterface float64;
|
|
void *float64InterruptPvt;
|
|
asynInterface float64Array;
|
|
asynInterface genericPointer;
|
|
void *genericPointerInterruptPvt;
|
|
asynInterface drvUser;
|
|
asynUser *pasynUser;
|
|
} drvmotorPvt;
|
|
|
|
/* These functions are used by the interfaces */
|
|
static asynStatus readInt32 (void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 *value);
|
|
static asynStatus writeInt32 (void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 value);
|
|
static asynStatus readUInt32Digital (void *drvPvt, asynUser *pasynUser,
|
|
epicsUInt32 *value, epicsUInt32 mask);
|
|
static asynStatus writeUInt32Digital (void *drvPvt, asynUser *pasynUser,
|
|
epicsUInt32 value, epicsUInt32 mask);
|
|
static asynStatus getBounds (void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 *low, epicsInt32 *high);
|
|
static asynStatus readFloat64 (void *drvPvt, asynUser *pasynUser,
|
|
epicsFloat64 *value);
|
|
static asynStatus writeFloat64 (void *drvPvt, asynUser *pasynUser,
|
|
epicsFloat64 value);
|
|
static asynStatus readGenericPointer (void *drvPvt, asynUser *pasynUser,
|
|
void *value);
|
|
static asynStatus drvUserCreate (void *drvPvt, asynUser *pasynUser,
|
|
const char *drvInfo,
|
|
const char **pptypeName, size_t *psize);
|
|
static asynStatus drvUserGetType (void *drvPvt, asynUser *pasynUser,
|
|
const char **pptypeName, size_t *psize);
|
|
static asynStatus drvUserDestroy (void *drvPvt, asynUser *pasynUser);
|
|
|
|
static void report (void *drvPvt, FILE *fp, int details);
|
|
static asynStatus connect (void *drvPvt, asynUser *pasynUser);
|
|
static asynStatus disconnect (void *drvPvt, asynUser *pasynUser);
|
|
|
|
|
|
/* These are private functions, not used in any interfaces */
|
|
static void intCallback(void *drvPvt, unsigned int num, unsigned int *changed);
|
|
static int config (drvmotorPvt *pPvt);
|
|
static int logFunc (void *userParam,
|
|
const motorAxisLogMask_t logMask,
|
|
const char *pFormat, ...);
|
|
|
|
static asynCommon drvMotorCommon = {
|
|
report,
|
|
connect,
|
|
disconnect
|
|
};
|
|
|
|
static asynInt32 drvMotorInt32 = {
|
|
writeInt32,
|
|
readInt32,
|
|
getBounds
|
|
};
|
|
|
|
static asynUInt32Digital drvMotorUInt32Digital = {
|
|
writeUInt32Digital,
|
|
readUInt32Digital,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL
|
|
};
|
|
|
|
static asynFloat64 drvMotorFloat64 = {
|
|
writeFloat64,
|
|
readFloat64
|
|
};
|
|
|
|
static asynFloat64Array drvMotorFloat64Array = {
|
|
NULL,
|
|
NULL,
|
|
NULL,
|
|
NULL
|
|
};
|
|
|
|
static asynGenericPointer drvMotorGenericPointer = {
|
|
NULL,
|
|
readGenericPointer,
|
|
};
|
|
|
|
static asynDrvUser drvMotorDrvUser = {
|
|
drvUserCreate,
|
|
drvUserGetType,
|
|
drvUserDestroy
|
|
};
|
|
|
|
static asynUser *defaultAsynUser;
|
|
|
|
|
|
int drvAsynMotorConfigure(const char *portName, const char *driverName,
|
|
int card, int num_axes)
|
|
{
|
|
drvmotorPvt *pPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
asynStatus status;
|
|
int i;
|
|
|
|
pPvt = callocMustSucceed(1, sizeof(*pPvt), "drvAsynMotorConfigure");
|
|
pPvt->portName = epicsStrDup(portName);
|
|
pPvt->drvset = (motorAxisDrvSET_t *) registryDriverSupportFind( driverName );
|
|
if (pPvt->drvset == NULL) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't find driver: %s\n", driverName);
|
|
return -1;
|
|
}
|
|
|
|
/* Link with higher level routines */
|
|
pPvt->common.interfaceType = asynCommonType;
|
|
pPvt->common.pinterface = (void *)&drvMotorCommon;
|
|
pPvt->common.drvPvt = pPvt;
|
|
pPvt->int32.interfaceType = asynInt32Type;
|
|
pPvt->int32.pinterface = (void *)&drvMotorInt32;
|
|
pPvt->int32.drvPvt = pPvt;
|
|
pPvt->uint32digital.interfaceType = asynUInt32DigitalType;
|
|
pPvt->uint32digital.pinterface = (void *)&drvMotorUInt32Digital;
|
|
pPvt->uint32digital.drvPvt = pPvt;
|
|
pPvt->float64.interfaceType = asynFloat64Type;
|
|
pPvt->float64.pinterface = (void *)&drvMotorFloat64;
|
|
pPvt->float64.drvPvt = pPvt;
|
|
pPvt->float64Array.interfaceType = asynFloat64ArrayType;
|
|
pPvt->float64Array.pinterface = (void *)&drvMotorFloat64Array;
|
|
pPvt->float64Array.drvPvt = pPvt;
|
|
pPvt->genericPointer.interfaceType = asynGenericPointerType;
|
|
pPvt->genericPointer.pinterface = (void *)&drvMotorGenericPointer;
|
|
pPvt->genericPointer.drvPvt = pPvt;
|
|
pPvt->drvUser.interfaceType = asynDrvUserType;
|
|
pPvt->drvUser.pinterface = (void *)&drvMotorDrvUser;
|
|
pPvt->drvUser.drvPvt = pPvt;
|
|
|
|
status = pasynManager->registerPort(portName,
|
|
ASYN_MULTIDEVICE | ASYN_CANBLOCK,
|
|
1, /* autoconnect */
|
|
0, /* medium priority */
|
|
0); /* default stack size */
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register port\n");
|
|
return -1;
|
|
}
|
|
status = pasynManager->registerInterface(portName,&pPvt->common);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register common.\n");
|
|
return -1;
|
|
}
|
|
|
|
status = pasynInt32Base->initialize(pPvt->portName,&pPvt->int32);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register int32\n");
|
|
return -1;
|
|
}
|
|
pasynManager->registerInterruptSource(portName, &pPvt->int32,
|
|
&pPvt->int32InterruptPvt);
|
|
|
|
status = pasynUInt32DigitalBase->initialize(pPvt->portName,&pPvt->uint32digital);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register uint32digital\n");
|
|
return -1;
|
|
}
|
|
pasynManager->registerInterruptSource(portName, &pPvt->uint32digital,
|
|
&pPvt->uint32digitalInterruptPvt);
|
|
|
|
status = pasynFloat64Base->initialize(pPvt->portName,&pPvt->float64);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register float64\n");
|
|
return -1;
|
|
}
|
|
pasynManager->registerInterruptSource(portName, &pPvt->float64,
|
|
&pPvt->float64InterruptPvt);
|
|
|
|
status = pasynFloat64ArrayBase->initialize(pPvt->portName,&pPvt->float64Array);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register float64Array\n");
|
|
return -1;
|
|
}
|
|
|
|
status = pasynGenericPointerBase->initialize(pPvt->portName,&pPvt->genericPointer);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register genericPointer\n");
|
|
return -1;
|
|
}
|
|
pasynManager->registerInterruptSource(portName, &pPvt->genericPointer,
|
|
&pPvt->genericPointerInterruptPvt);
|
|
|
|
status = pasynManager->registerInterface(pPvt->portName,&pPvt->drvUser);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure ERROR: Can't register drvUser\n");
|
|
return -1;
|
|
}
|
|
/* Create asynUser for debugging */
|
|
pPvt->pasynUser = pasynManager->createAsynUser(0, 0);
|
|
|
|
/* Connect to device */
|
|
status = pasynManager->connectDevice(pPvt->pasynUser, portName, -1);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure, connectDevice failed\n");
|
|
return -1;
|
|
}
|
|
|
|
pPvt->lock = epicsMutexCreate();
|
|
pPvt->card = card;
|
|
config(pPvt);
|
|
|
|
pPvt->numAxes = num_axes;
|
|
|
|
pPvt->axisData = callocMustSucceed(num_axes, sizeof(drvmotorAxisPvt), "drvAsynMotorConfigure");
|
|
for ( i = 0; i < num_axes; i++) {
|
|
pAxis = &pPvt->axisData[i];
|
|
pAxis->axis = (*pPvt->drvset->open)(card, i, "");
|
|
if (!pAxis->axis) {
|
|
asynPrint(pPvt->pasynUser, ASYN_TRACE_ERROR,
|
|
"drvAsynMotorConfigure: Failed to open axis %d\n", i);
|
|
}
|
|
pAxis->num = i;
|
|
pAxis->pPvt = pPvt;
|
|
/* Create asynUser for debugging */
|
|
pAxis->pasynUser = pasynManager->createAsynUser(0, 0);
|
|
/* Connect to device */
|
|
status = pasynManager->connectDevice(pAxis->pasynUser, portName, i);
|
|
if (status != asynSuccess) {
|
|
errlogPrintf("drvAsynMotorConfigure, connectDevice failed\n");
|
|
return -1;
|
|
}
|
|
if (pAxis->axis) {
|
|
(*pPvt->drvset->setCallback)(pAxis->axis, intCallback, (void *)pAxis);
|
|
(*pPvt->drvset->setLog)(pAxis->axis, logFunc, pAxis->pasynUser);
|
|
}
|
|
/* All other axis parameters are initialised to zero at allocation */
|
|
}
|
|
/* Create a fallback asynUser for logging, but only the first time */
|
|
if (!defaultAsynUser) {
|
|
defaultAsynUser = pasynManager->createAsynUser(0,0);
|
|
}
|
|
(*pPvt->drvset->setLog)(NULL, logFunc, defaultAsynUser );
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
static int config(drvmotorPvt *pPvt)
|
|
{
|
|
/* XXX call the driver config? with pPvt->card */
|
|
return(0);
|
|
}
|
|
|
|
static asynStatus readInt32(void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 *value)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readInt32 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readInt32 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
switch(command) {
|
|
case -1: /* This is commands we know about but don't want int32 interface for */
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readInt32 invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
case motorStatus:
|
|
*value = pAxis->status.status;
|
|
break;
|
|
case motorPosition:
|
|
case motorEncoderPosition:
|
|
default:
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, command, value);
|
|
break;
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::readInt32, reason=%d, value=%d\n",
|
|
command, *value);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static asynStatus readUInt32Digital(void *drvPvt, asynUser *pasynUser,
|
|
epicsUInt32 *value, epicsUInt32 mask)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readInt32 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readInt32 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
switch(command) {
|
|
case -1: /* This is commands we know about but don't want uint32digital interface for */
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readUInt32Digital invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
default:
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, command, (epicsInt32*)value);
|
|
break;
|
|
}
|
|
|
|
*value = *value & mask;
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::readUInt32Digital, reason=%d, value=%d\n",
|
|
command, *value);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static asynStatus readFloat64(void *drvPvt, asynUser *pasynUser,
|
|
epicsFloat64 *value)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
asynStatus status = asynSuccess;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readFloat64 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readFloat64 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
switch(command) {
|
|
case -1:
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readFloat64 invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
case motorVelocity:
|
|
*value = pAxis->max_velocity;
|
|
break;
|
|
case motorVelBase:
|
|
*value = pAxis->min_velocity;
|
|
break;
|
|
case motorAccel:
|
|
*value = pAxis->accel;
|
|
break;
|
|
case motorPosition:
|
|
case motorEncoderPosition:
|
|
default:
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, command, value);
|
|
break;
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::readFloat64, reason=%d, value=%f\n",
|
|
command, *value);
|
|
return(status);
|
|
}
|
|
|
|
static asynStatus getBounds(void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 *low, epicsInt32 *high)
|
|
{
|
|
*low = 0;
|
|
*high = 65535;
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::getBounds,low=%d, high=%d\n", *low, *high);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static asynStatus writeInt32(void *drvPvt, asynUser *pasynUser,
|
|
epicsInt32 value)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
asynStatus status=asynSuccess;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeInt32 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeInt32 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
switch(command) {
|
|
case -1:
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeInt32 invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
case motorStop:
|
|
status = (*pPvt->drvset->stop)(pAxis->axis, pAxis->accel);
|
|
break;
|
|
case motorHome:
|
|
status = (*pPvt->drvset->home)(pAxis->axis, pAxis->min_velocity,
|
|
pAxis->max_velocity, pAxis->accel,
|
|
(value == 0) ? 0 : 1);
|
|
break;
|
|
case motorSetClosedLoop:
|
|
status = (*pPvt->drvset->setInteger)(pAxis->axis, motorAxisClosedLoop,
|
|
value);
|
|
break;
|
|
case motorUpdateStatus:
|
|
if (pPvt->drvset->forceCallback != NULL)
|
|
status = (*pPvt->drvset->forceCallback)(pAxis->axis);
|
|
break;
|
|
default:
|
|
status = (*pPvt->drvset->setInteger)(pAxis->axis, command, value);
|
|
break;
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::writeInt32, reason=%d, value=%d\n",
|
|
command, value);
|
|
return(status);
|
|
}
|
|
|
|
static asynStatus writeUInt32Digital(void *drvPvt, asynUser *pasynUser,
|
|
epicsUInt32 value, epicsUInt32 mask)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
asynStatus status;
|
|
epicsUInt32 temp = 0;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeInt32 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeInt32 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
switch(command) {
|
|
case -1:
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeUInt32Digital invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
default:
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, command, (epicsInt32*)&temp);
|
|
temp &= ~mask;
|
|
temp |= (value & mask);
|
|
status = (*pPvt->drvset->setInteger)(pAxis->axis, command, temp);
|
|
break;
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::writeUInt32Digital, reason=%d, value=%d\n",
|
|
command, temp);
|
|
return(status);
|
|
}
|
|
|
|
static asynStatus writeFloat64(void *drvPvt, asynUser *pasynUser,
|
|
epicsFloat64 value)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
motorCommand command = pasynUser->reason;
|
|
asynStatus status = asynError;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeFloat64 Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeFloat64 Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::writeFloat64, reason=%d, pasynUser=%p pAxis=%p\n",
|
|
command, pasynUser, pAxis);
|
|
|
|
switch(command) {
|
|
case -1:
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::writeFloat64 invalid command=%d",
|
|
command);
|
|
return(asynError);
|
|
case motorMoveRel:
|
|
status = (*pPvt->drvset->move)(pAxis->axis, value, 1, pAxis->min_velocity,
|
|
pAxis->max_velocity, pAxis->accel);
|
|
break;
|
|
case motorMoveAbs:
|
|
status = (*pPvt->drvset->move)(pAxis->axis, value, 0, pAxis->min_velocity,
|
|
pAxis->max_velocity, pAxis->accel);
|
|
break;
|
|
case motorMoveVel:
|
|
status = (*pPvt->drvset->velocityMove)(pAxis->axis, pAxis->min_velocity,
|
|
value, pAxis->accel);
|
|
break;
|
|
case motorHome:
|
|
status = (*pPvt->drvset->home)(pAxis->axis, pAxis->min_velocity,
|
|
pAxis->max_velocity, pAxis->accel,
|
|
(value == 0) ? 0 : 1);
|
|
break;
|
|
case motorVelocity:
|
|
pAxis->max_velocity = value;
|
|
status = asynSuccess;
|
|
break;
|
|
case motorVelBase:
|
|
pAxis->min_velocity = value;
|
|
status = asynSuccess;
|
|
break;
|
|
case motorAccel:
|
|
pAxis->accel = value;
|
|
status = asynSuccess;
|
|
break;
|
|
case motorPosition:
|
|
case motorResolution:
|
|
case motorEncRatio:
|
|
case motorPGain:
|
|
case motorIGain:
|
|
case motorDGain:
|
|
case motorHighLimit:
|
|
case motorLowLimit:
|
|
default:
|
|
status = (*pPvt->drvset->setDouble)(pAxis->axis, command, value);
|
|
break;
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::writeFloat64, reason=%d, value=%f\n",
|
|
command, value);
|
|
return(status);
|
|
}
|
|
|
|
static asynStatus readGenericPointer(void *drvPvt, asynUser *pasynUser,
|
|
void *pValue)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
drvmotorAxisPvt *pAxis;
|
|
int channel;
|
|
MotorStatus *value = (MotorStatus *)pValue;
|
|
|
|
pasynManager->getAddr(pasynUser, &channel);
|
|
if (channel >= pPvt->numAxes) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readGenericPointer Invalid axis %d", channel);
|
|
return(asynError);
|
|
}
|
|
|
|
pAxis = &pPvt->axisData[channel];
|
|
if (!pAxis->axis) {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvMotorAsyn::readGenericPointer Uninitialised axis %d", pAxis->num);
|
|
return(asynError);
|
|
}
|
|
|
|
if (value) {
|
|
value->status = pAxis->status.status;
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, motorPosition,
|
|
&(value->position));
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, motorEncoderPosition,
|
|
&(value->encoderPosition));
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, motorVelocity,
|
|
&(value->velocity));
|
|
}
|
|
|
|
asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
|
|
"drvMotorAsyn::readMotorStatus, [%08x,%f,%f,%f]\n",
|
|
value->status, value->position, value->encoderPosition,
|
|
value->velocity);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static int logFunc(void *userParam,
|
|
const motorAxisLogMask_t logMask,
|
|
const char *pFormat, ...)
|
|
{
|
|
va_list pvar;
|
|
asynUser *pasynUser = (asynUser *)userParam;
|
|
|
|
if (!pasynUser) {
|
|
pasynUser = defaultAsynUser;
|
|
}
|
|
|
|
va_start(pvar, pFormat);
|
|
switch(logMask) {
|
|
case motorAxisTraceError:
|
|
pasynTrace->vprint(pasynUser, ASYN_TRACE_ERROR, pFormat, pvar);
|
|
break;
|
|
case motorAxisTraceIODevice:
|
|
pasynTrace->vprint(pasynUser, ASYN_TRACEIO_DEVICE, pFormat, pvar);
|
|
break;
|
|
case motorAxisTraceIOFilter:
|
|
pasynTrace->vprint(pasynUser, ASYN_TRACEIO_FILTER, pFormat, pvar);
|
|
break;
|
|
case motorAxisTraceIODriver:
|
|
pasynTrace->vprint(pasynUser, ASYN_TRACEIO_DRIVER, pFormat, pvar);
|
|
break;
|
|
case motorAxisTraceFlow:
|
|
pasynTrace->vprint(pasynUser, ASYN_TRACE_FLOW, pFormat, pvar);
|
|
break;
|
|
default:
|
|
asynPrint(pasynUser, ASYN_TRACE_ERROR, "drvMotorAsyn:logFunc unknown logMask %d\n", logMask);
|
|
}
|
|
va_end (pvar);
|
|
return(MOTOR_AXIS_OK);
|
|
}
|
|
|
|
static void intCallback(void *axisPvt, unsigned int nChanged,
|
|
unsigned int *changed)
|
|
{
|
|
drvmotorAxisPvt *pAxis = (drvmotorAxisPvt *)axisPvt;
|
|
drvmotorPvt *pPvt = pAxis->pPvt;
|
|
int addr;
|
|
unsigned int reason;
|
|
ELLLIST *pclientList;
|
|
interruptNode *pnode;
|
|
int ivalue;
|
|
double dvalue;
|
|
unsigned int i, bit_num;
|
|
epicsInt32 changedmask = 0;
|
|
|
|
/* We are called back with an array of things that have changed.
|
|
First put these into a single int32 word for passing up to higher layers
|
|
Note that for now this relies on the order of the changed flags being
|
|
correct */
|
|
for (i = 0; i < nChanged; i++) {
|
|
if (changed[i] >= motorAxisDirection &&
|
|
changed[i] <= motorAxisHomed) {
|
|
bit_num = changed[i] - motorAxisDirection;
|
|
changedmask |= (1 << bit_num);
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, changed[i], &ivalue);
|
|
BIT_SET(bit_num, &(pAxis->status.status), ivalue);
|
|
}
|
|
if (changed[i] == motorPosition) {
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, changed[i],
|
|
&(pAxis->status.position));
|
|
}
|
|
if (changed[i] == motorEncoderPosition) {
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, changed[i],
|
|
&(pAxis->status.encoderPosition));
|
|
}
|
|
if (changed[i] == motorAxisActualVel) {
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, changed[i],
|
|
&(pAxis->status.velocity));
|
|
}
|
|
}
|
|
|
|
/* Pass float64 interrupts */
|
|
pasynManager->interruptStart(pPvt->float64InterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynFloat64Interrupt *pfloat64Interrupt = pnode->drvPvt;
|
|
addr = pfloat64Interrupt->addr;
|
|
reason = pfloat64Interrupt->pasynUser->reason;
|
|
if (addr == pAxis->num) {
|
|
for (i = 0; i < nChanged; i++) {
|
|
if (changed[i] == reason) {
|
|
(*pPvt->drvset->getDouble)(pAxis->axis, changed[i], &dvalue);
|
|
pfloat64Interrupt->callback(pfloat64Interrupt->userPvt,
|
|
pfloat64Interrupt->pasynUser,
|
|
dvalue);
|
|
}
|
|
}
|
|
}
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->float64InterruptPvt);
|
|
|
|
/* Pass motorStatus interrupts */
|
|
pasynManager->interruptStart(pPvt->genericPointerInterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynGenericPointerInterrupt *pInterrupt = pnode->drvPvt;
|
|
addr = pInterrupt->addr;
|
|
reason = pInterrupt->pasynUser->reason;
|
|
if (addr == pAxis->num) {
|
|
pInterrupt->callback(pInterrupt->userPvt,
|
|
pInterrupt->pasynUser,
|
|
(void *)&pAxis->status);
|
|
}
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->genericPointerInterruptPvt);
|
|
|
|
/* Pass int32 interrupts */
|
|
pasynManager->interruptStart(pPvt->int32InterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynInt32Interrupt *pint32Interrupt = pnode->drvPvt;
|
|
addr = pint32Interrupt->addr;
|
|
reason = pint32Interrupt->pasynUser->reason;
|
|
if (addr == pAxis->num) {
|
|
if ( reason >= motorStatusDirection &&
|
|
reason < motorStatusLast ) {
|
|
if (BIT_ISSET(reason - motorStatusDirection, 1, &changedmask))
|
|
{
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, reason, &ivalue);
|
|
pint32Interrupt->callback(pint32Interrupt->userPvt,
|
|
pint32Interrupt->pasynUser,
|
|
ivalue);
|
|
}
|
|
}
|
|
/* If we've subscribed to the aggregate status */
|
|
else if (reason == motorStatus) {
|
|
pint32Interrupt->callback(pint32Interrupt->userPvt,
|
|
pint32Interrupt->pasynUser,
|
|
pAxis->status.status);
|
|
}
|
|
else {
|
|
(*pPvt->drvset->getInteger)(pAxis->axis, reason, &ivalue);
|
|
pint32Interrupt->callback(pint32Interrupt->userPvt,
|
|
pint32Interrupt->pasynUser,
|
|
ivalue);
|
|
}
|
|
}
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->int32InterruptPvt);
|
|
}
|
|
|
|
|
|
/*static void rebootCallback(void *drvPvt)*/
|
|
/*{*/
|
|
/* drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;*/
|
|
/* Anything special we have to do on reboot */
|
|
/* pPvt->rebooting = 1;*/
|
|
/*}*/
|
|
|
|
/* asynDrvUser routines */
|
|
static asynStatus drvUserCreate(void *drvPvt, asynUser *pasynUser,
|
|
const char *drvInfo,
|
|
const char **pptypeName, size_t *psize)
|
|
{
|
|
int i;
|
|
char *pstring;
|
|
int ncommands = sizeof(motorCommands)/sizeof(motorCommands[0]);
|
|
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::drvUserCreate, drvInfo=%s, pptypeName=%p, psize=%p, pasynUser=%p\n", drvInfo, pptypeName, psize, pasynUser);
|
|
|
|
for (i=0; i < ncommands; i++) {
|
|
pstring = motorCommands[i].commandString;
|
|
if (epicsStrCaseCmp(drvInfo, pstring) == 0) {
|
|
break;
|
|
}
|
|
}
|
|
if (i < ncommands) {
|
|
pasynUser->reason = motorCommands[i].command;
|
|
if (pptypeName) {
|
|
*pptypeName = epicsStrDup(pstring);
|
|
}
|
|
if (psize) {
|
|
*psize = sizeof(motorCommands[i].command);
|
|
}
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::drvUserCreate, command=%s\n", pstring);
|
|
return(asynSuccess);
|
|
} else {
|
|
epicsSnprintf(pasynUser->errorMessage, pasynUser->errorMessageSize,
|
|
"drvmotorAsyn::drvUserCreate, unknown command=%s", drvInfo);
|
|
return(asynError);
|
|
}
|
|
}
|
|
|
|
static asynStatus drvUserGetType(void *drvPvt, asynUser *pasynUser,
|
|
const char **pptypeName, size_t *psize)
|
|
{
|
|
motorCommand command = pasynUser->reason;
|
|
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::drvUserGetType entered");
|
|
|
|
*pptypeName = NULL;
|
|
*psize = 0;
|
|
if (pptypeName)
|
|
*pptypeName = epicsStrDup(motorCommands[command].commandString);
|
|
if (psize) *psize = sizeof(command);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static asynStatus drvUserDestroy(void *drvPvt, asynUser *pasynUser)
|
|
{
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::drvUserDestroy, drvPvt=%p, pasynUser=%p\n",
|
|
drvPvt, pasynUser);
|
|
|
|
return(asynSuccess);
|
|
}
|
|
|
|
/* asynCommon routines */
|
|
|
|
/* Report parameters */
|
|
static void report(void *drvPvt, FILE *fp, int details)
|
|
{
|
|
drvmotorPvt *pPvt = (drvmotorPvt *)drvPvt;
|
|
interruptNode *pnode;
|
|
ELLLIST *pclientList;
|
|
|
|
fprintf(fp, "Port: %s\n", pPvt->portName);
|
|
if (details >= 1) {
|
|
fprintf(fp, " messages sent OK=%d; send failed (queue full)=%d\n",
|
|
pPvt->messagesSent, pPvt->messagesFailed);
|
|
/* Report int32 interrupts */
|
|
pasynManager->interruptStart(pPvt->int32InterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynInt32Interrupt *pint32Interrupt = pnode->drvPvt;
|
|
fprintf(fp, " int32 callback client address=%p, addr=%d, reason=%d\n",
|
|
pint32Interrupt->callback, pint32Interrupt->addr,
|
|
pint32Interrupt->pasynUser->reason);
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->int32InterruptPvt);
|
|
|
|
/* Report float64 interrupts */
|
|
pasynManager->interruptStart(pPvt->float64InterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynFloat64Interrupt *pfloat64Interrupt = pnode->drvPvt;
|
|
fprintf(fp, " float64 callback client address=%p, addr=%d, reason=%d\n",
|
|
pfloat64Interrupt->callback, pfloat64Interrupt->addr,
|
|
pfloat64Interrupt->pasynUser->reason);
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->float64InterruptPvt);
|
|
|
|
/* Report motorStatus interrupts */
|
|
pasynManager->interruptStart(pPvt->genericPointerInterruptPvt, &pclientList);
|
|
pnode = (interruptNode *)ellFirst(pclientList);
|
|
while (pnode) {
|
|
asynGenericPointerInterrupt *pInterrupt = pnode->drvPvt;
|
|
fprintf(fp, " motorStatus callback client address=%p, reason=%d\n",
|
|
pInterrupt->callback,
|
|
pInterrupt->pasynUser->reason);
|
|
pnode = (interruptNode *)ellNext(&pnode->node);
|
|
}
|
|
pasynManager->interruptEnd(pPvt->genericPointerInterruptPvt);
|
|
}
|
|
}
|
|
|
|
/* Connect */
|
|
static asynStatus connect(void *drvPvt, asynUser *pasynUser)
|
|
{
|
|
pasynManager->exceptionConnect(pasynUser);
|
|
|
|
asynPrint(pasynUser, ASYN_TRACE_FLOW,
|
|
"drvMotorAsyn::connect, pasynUser=%p\n", pasynUser);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
/* Disconnect */
|
|
static asynStatus disconnect(void *drvPvt, asynUser *pasynUser)
|
|
{
|
|
pasynManager->exceptionDisconnect(pasynUser);
|
|
return(asynSuccess);
|
|
}
|
|
|
|
static const iocshArg initArg0 = { "portName",iocshArgString};
|
|
static const iocshArg initArg1 = { "driverName",iocshArgString};
|
|
static const iocshArg initArg2 = { "cardNum",iocshArgInt};
|
|
static const iocshArg initArg3 = { "numAxes",iocshArgInt};
|
|
static const iocshArg * const initArgs[4] = {&initArg0,
|
|
&initArg1,
|
|
&initArg2,
|
|
&initArg3};
|
|
static const iocshFuncDef initFuncDef = {"drvAsynMotorConfigure",4,initArgs};
|
|
static void initCallFunc(const iocshArgBuf *args)
|
|
{
|
|
drvAsynMotorConfigure(args[0].sval, args[1].sval, args[2].ival,
|
|
args[3].ival);
|
|
}
|
|
|
|
void motorRegister(void)
|
|
{
|
|
iocshRegister(&initFuncDef,initCallFunc);
|
|
}
|
|
|
|
epicsExportRegistrar(motorRegister);
|