lin-epics-modules/motorBase
0
0
Fork 0
forked from epics_driver_modules/motorBase
Code Pull Requests Activity

Start

Browse Source
This commit is contained in:
Ron Sluiter
2008-09-10 18:50:48 +00:00
parent 1dc1700146
commit 6725d264c0
3 changed files with 864 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
motorApp/AttocubeSrc/Makefile
+20
View File
@@ -0,0 +1,20 @@
# Makefile
TOP = ../..
include $(TOP)/configure/CONFIG
#
# Both the following line, and a line in the *.dbd file,
# must be uncommented to use diagnostic debugging messages.
#!USR_CXXFLAGS += -DDEBUG
DBD += devAttocube.dbd
LIBRARY_IOC = Attocube
# ANC 150 asyn motor driver.
Attocube_SRCS += drvANC150Asyn.cc
Attocube_LIBS += motor asyn
Attocube_LIBS += $(EPICS_BASE_IOC_LIBS)
include $(TOP)/configure/RULES
motorApp/AttocubeSrc/devAttocube.dbd
+4
View File
@@ -0,0 +1,4 @@
# attocube ANC 150 asyn motor driver support.
driver(motorANC150)
registrar(ANC150Register)
motorApp/AttocubeSrc/drvANC150Asyn.cc
+840
View File
@@ -0,0 +1,840 @@
/*
FILENAME... drvANC150Asyn.cc
USAGE... Motor record driver level support for Kohzu SC800
Version: $Revision: 1.1 $
Modified By: $Author: sluiter $
Last Modified: $Date: 2008-09-10 18:50:48 $
*/
/*
* Original Author: Ron Sluiter
* Date: 07/24/2008
*
* Experimental Physics and Industrial Control System (EPICS)
*
* Copyright 1991, the Regents of the University of California,
* and the University of Chicago Board of Governors.
*
* This software was produced under U.S. Government contracts:
* (W-7405-ENG-36) at the Los Alamos National Laboratory,
* and (W-31-109-ENG-38) at Argonne National Laboratory.
*
* Initial development by:
* The Controls and Automation Group (AT-8)
* Ground Test Accelerator
* Accelerator Technology Division
* Los Alamos National Laboratory
*
* Co-developed with
* The Controls and Computing Group
* Accelerator Systems Division
* Advanced Photon Source
* Argonne National Laboratory
*
* Modification Log:
* -----------------
* .01 11-09-07 rls copied from drvMM4000Asyn.cc
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "epicsThread.h"
#include "epicsExport.h"
#include "epicsEvent.h"
#include "epicsMutex.h"
#include "epicsTime.h"
#include "epicsTimer.h"
#include "epicsString.h"
#include "iocsh.h"
#include "drvSup.h"
#include "asynOctetSyncIO.h"
#define DEFINE_MOTOR_PROTOTYPES 1
#include "motor_interface.h"
#include "paramLib.h"
/* End-of-string defines */
#define ANC150_OUT_EOS "\r\n" /* Command */
#define ANC150_IN_EOS "\r\n" /* Reply */
#define NINT(f) (long)((f)>0 ? (f)+0.5 : (f)-0.5) /* Nearest integer. */
motorAxisDrvSET_t motorANC150 =
{
14,
motorAxisReport, /**< Standard EPICS driver report function (optional) */
motorAxisInit, /**< Standard EPICS dirver initialisation function (optional) */
motorAxisSetLog, /**< Defines an external logging function (optional) */
motorAxisOpen, /**< Driver open function */
motorAxisClose, /**< Driver close function */
motorAxisSetCallback, /**< Provides a callback function the driver can call when the status updates */
motorAxisSetDouble, /**< Pointer to function to set a double value */
motorAxisSetInteger, /**< Pointer to function to set an integer value */
motorAxisGetDouble, /**< Pointer to function to get a double value */
motorAxisGetInteger, /**< Pointer to function to get an integer value */
motorAxisHome, /**< Pointer to function to execute a more to reference or home */
motorAxisMove, /**< Pointer to function to execute a position move */
motorAxisVelocityMove, /**< Pointer to function to execute a velocity mode move */
motorAxisStop, /**< Pointer to function to stop motion */
motorAxisforceCallback /**< Pointer to function to request a poller status update */
};
extern "C" {epicsExportAddress(drvet, motorANC150);}
typedef struct
{
asynUser *pasynUser;
int numAxes;
char firmwareVersion[100];
double movingPollPeriod;
double idlePollPeriod;
epicsEventId pollEventId;
AXIS_HDL pAxis; /* array of axes */
} ANC150Controller;
typedef struct motorAxisHandle
{
ANC150Controller *pController;
PARAMS params;
double currentPosition;
double highLimit;
double lowLimit;
double homePreset;
int axisStatus;
int card;
int axis;
int maxDigits;
motorAxisLogFunc print;
void *logParam;
bool moving_ind; /* Moving indicator. */
epicsMutexId mutexId;
epicsTime *movetimer; /* Moving timer. */
int frequency;
} motorAxis;
typedef struct
{
AXIS_HDL pFirst;
epicsThreadId motorThread;
motorAxisLogFunc print;
void *logParam;
epicsTimeStamp now;
} motorANC150_t;
static int motorANC150LogMsg(void *, const motorAxisLogMask_t, const char *, ...);
static int sendOnly(ANC150Controller *, char *);
static asynStatus sendAndReceive(ANC150Controller *, char *, char *, int);
static asynStatus getFreq(ANC150Controller *, int, char *, char *, int);
#define PRINT (drv.print)
#define FLOW motorAxisTraceFlow
#define IODRIVER motorAxisTraceIODriver
#define ANC150_MAX_AXES 6
#define BUFFER_SIZE 100 /* Size of input and output buffers */
#define TIMEOUT 2.0 /* Timeout for I/O in seconds */
#define ANC150_HOME 0x20 /* Home LS. */
#define ANC150_LOW_LIMIT 0x10 /* Minus Travel Limit. */
#define ANC150_HIGH_LIMIT 0x08 /* Plus Travel Limit. */
#define ANC150_DIRECTION 0x04 /* Motor direction: 0 - minus; 1 - plus. */
#define ANC150_POWER_ON 0x02 /* Motor power 0 - ON; 1 - OFF. */
#define ANC150_MOVING 0x01 /* In-motion indicator. */
#define TCP_TIMEOUT 2.0
static motorANC150_t drv = {NULL, NULL, motorANC150LogMsg, 0, {0, 0}};
static int numANC150Controllers;
/* Pointer to array of controller strutures */
static ANC150Controller *pANC150Controller = NULL;
#define MAX(a,b) ((a)>(b)? (a): (b))
#define MIN(a,b) ((a)<(b)? (a): (b))
static void motorAxisReportAxis(AXIS_HDL pAxis, int level)
{
if (level > 0)
{
printf("Axis %d\n", pAxis->axis);
printf(" axisStatus: 0x%x\n", pAxis->axisStatus);
printf(" high limit: %f\n", pAxis->highLimit);
printf(" low limit: %f\n", pAxis->lowLimit);
printf(" home preset: %f\n", pAxis->homePreset);
printf(" max digits: %d\n", pAxis->maxDigits);
}
}
static void motorAxisReport(int level)
{
int i, j;
for (i = 0; i < numANC150Controllers; i++)
{
printf("Controller %d firmware version: %s\n", i, pANC150Controller[i].firmwareVersion);
if (level)
{
printf(" model: attocube ANC 150\n");
printf(" moving poll period: %f\n", pANC150Controller[i].movingPollPeriod);
printf(" idle poll period: %f\n", pANC150Controller[i].idlePollPeriod);
printf("Controller %d firmware version: %s\n", i, pANC150Controller[i].firmwareVersion);
}
for (j = 0; j < pANC150Controller[i].numAxes; j++)
{
motorAxisReportAxis(&pANC150Controller[i].pAxis[j], level);
}
}
}
static int motorAxisInit(void)
{
return(MOTOR_AXIS_OK);
}
static int motorAxisSetLog(AXIS_HDL pAxis, motorAxisLogFunc logFunc, void *param)
{
if (pAxis == NULL)
{
if (logFunc == NULL)
{
drv.print = motorANC150LogMsg;
drv.logParam = NULL;
}
else
{
drv.print = logFunc;
drv.logParam = param;
}
}
else
{
if (logFunc == NULL)
{
pAxis->print = motorANC150LogMsg;
pAxis->logParam = NULL;
}
else
{
pAxis->print = logFunc;
pAxis->logParam = param;
}
}
return(MOTOR_AXIS_OK);
}
static AXIS_HDL motorAxisOpen(int card, int axis, char *param)
{
AXIS_HDL pAxis;
if (card > numANC150Controllers)
return(NULL);
if (axis > pANC150Controller[card].numAxes)
return(NULL);
pAxis = &pANC150Controller[card].pAxis[axis];
return(pAxis);
}
static int motorAxisClose(AXIS_HDL pAxis)
{
return(MOTOR_AXIS_OK);
}
static int motorAxisGetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int *value)
{
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
else
{
return(motorParam->getInteger(pAxis->params, (paramIndex) function, value));
}
}
static int motorAxisGetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double *value)
{
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
else
{
return(motorParam->getDouble(pAxis->params, (paramIndex) function, value));
}
}
static int motorAxisSetCallback(AXIS_HDL pAxis, motorAxisCallbackFunc callback, void *param)
{
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
else
{
return(motorParam->setCallback(pAxis->params, callback, param));
}
}
static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double value)
{
int ret_status = MOTOR_AXIS_ERROR;
int status;
char buff[20];
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
else
{
switch (function)
{
case motorAxisPosition:
{
pAxis->currentPosition = value;
break;
}
case motorAxisEncoderRatio:
{
PRINT(pAxis->logParam, motorAxisTraceError, "motorAxisSetDouble: ANC150 does not support setting encoder ratio\n");
break;
}
case motorAxisResolution:
{
PRINT(pAxis->logParam, motorAxisTraceError, "motorAxisSetDouble: ANC150 does not support setting resolution\n");
break;
}
case motorAxisLowLimit:
case motorAxisHighLimit:
break;
case motorAxisPGain:
{
PRINT(pAxis->logParam, motorAxisTraceError, "ANC150 does not support setting proportional gain\n");
break;
}
case motorAxisIGain:
{
PRINT(pAxis->logParam, motorAxisTraceError, "ANC150 does not support setting integral gain\n");
break;
}
case motorAxisDGain:
{
PRINT(pAxis->logParam, motorAxisTraceError, "ANC150 does not support setting derivative gain\n");
break;
}
case motorAxisClosedLoop:
{
if (value == 0.0)
sprintf(buff, "setm %d gnd", pAxis->axis + 1);
else
sprintf(buff, "setm %d stp", pAxis->axis + 1);
status = sendOnly(pAxis->pController, buff);
break;
}
default:
PRINT(pAxis->logParam, motorAxisTraceError, "motorAxisSetDouble: unknown function %d\n", function);
break;
}
}
if (ret_status != MOTOR_AXIS_ERROR)
status = motorParam->setDouble(pAxis->params, function, value);
return(ret_status);
}
static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value)
{
int ret_status = MOTOR_AXIS_ERROR;
int status;
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
switch (function)
{
case motorAxisClosedLoop:
if (value)
{
/*
* The ANC150 only allows turning on and off ALL motors (MO and MF
* commands), not individual axes
*/
/* Don't implement */
ret_status = MOTOR_AXIS_OK;
}
else
{
ret_status = MOTOR_AXIS_OK;
}
break;
default:
PRINT(pAxis->logParam, motorAxisTraceError, "motorAxisSetInteger: unknown function %d\n", function);
break;
}
if (ret_status != MOTOR_AXIS_ERROR)
status = motorParam->setInteger(pAxis->params, function, value);
return(ret_status);
}
static int motorAxisMove(AXIS_HDL pAxis, double position, int relative,
double min_velocity, double max_velocity, double acceleration)
{
int status;
long imove;
char buff[100];
const char *moveCommand;
bool posdir;
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f\n",
pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration);
if (relative)
{
if (position >= 0.0)
posdir = true;
else
posdir = false;
pAxis->currentPosition += position;
imove = NINT(position);
}
else
{
imove = NINT(position - pAxis->currentPosition);
if (imove >= 0)
posdir = true;
else
posdir = false;
pAxis->currentPosition = position;
}
if (posdir == true)
moveCommand = "stepu";
else
moveCommand = "stepd";
if (imove == 0)
return(MOTOR_AXIS_OK);
pAxis->moving_ind = true;
imove = abs(imove);
*pAxis->movetimer = epicsTime::getCurrent() + (imove / pAxis->frequency);
sprintf(buff, "%s %d %ld", moveCommand, pAxis->axis + 1, imove);
status = sendOnly(pAxis->pController, buff);
if (status)
return(MOTOR_AXIS_ERROR);
/*
* Send a signal to the poller task which will make it do a poll, and
* switch to the moving poll rate
*/
epicsEventSignal(pAxis->pController->pollEventId);
return(MOTOR_AXIS_OK);
}
static int motorAxisHome(AXIS_HDL pAxis, double min_velocity, double max_velocity, double acceleration, int forwards)
{
return(MOTOR_AXIS_ERROR);
}
static int motorAxisVelocityMove(AXIS_HDL pAxis, double min_velocity, double velocity, double acceleration)
{
int status;
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
/*
* ANC150 does not have a jog command. Simulate with move absolute to the
* appropriate software limit. We can move to ANC150 soft limits. If the
* record soft limits are set tighter than the ANC150 limits the record
* will prevent JOG motion beyond its soft limits
*/
if (velocity > 0.)
status = motorAxisMove(pAxis, pAxis->highLimit, 0, min_velocity, velocity, acceleration);
else
status = motorAxisMove(pAxis, pAxis->lowLimit, 0, min_velocity, -velocity, acceleration);
return(status);
}
static int motorAxisProfileMove(AXIS_HDL pAxis, int npoints, double positions[], double times[], int relative, int trigger)
{
return(MOTOR_AXIS_ERROR);
}
static int motorAxisTriggerProfile(AXIS_HDL pAxis)
{
return(MOTOR_AXIS_ERROR);
}
static int motorAxisStop(AXIS_HDL pAxis, double acceleration)
{
int status;
char buff[100];
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d to stop with accel=%f\n",
pAxis->card, pAxis->axis, acceleration);
sprintf(buff, "stop %d", pAxis->axis + 1);
status = sendOnly(pAxis->pController, buff);
if (status)
return(MOTOR_AXIS_ERROR);
return(MOTOR_AXIS_OK);
}
static int motorAxisforceCallback(AXIS_HDL pAxis)
{
if (pAxis == NULL)
return(MOTOR_AXIS_ERROR);
PRINT(pAxis->logParam, FLOW, "motorAxisforceCallback: request card %d, axis %d status update\n",
pAxis->card, pAxis->axis);
/* Force a status update. */
motorParam->forceCallback(pAxis->params);
/* Send a signal to the poller task which will make it do a status update */
epicsEventSignal(pAxis->pController->pollEventId);
return(MOTOR_AXIS_OK);
}
static void ANC150Poller(ANC150Controller * pController)
{
/* This is the task that polls the ANC150 */
double timeout;
AXIS_HDL pAxis;
int status;
int itera;
int axisDone;
int anyMoving;
int forcedFastPolls = 0;
timeout = pController->idlePollPeriod;
epicsEventSignal(pController->pollEventId); /* Force on poll at startup */
while (1)
{
if (timeout != 0.)
status = epicsEventWaitWithTimeout(pController->pollEventId, timeout);
else
status = epicsEventWait(pController->pollEventId);
if (status == epicsEventWaitOK)
{
/*
* We got an event, rather than a timeout. This is because other
* software knows that an axis should have changed state (started
* moving, etc.). Force a minimum number of fast polls, because the
* controller status might not have changed the first few polls
*/
forcedFastPolls = 10;
}
anyMoving = 0;
for (itera = 0; itera < pController->numAxes; itera++)
{
pAxis = &pController->pAxis[itera];
if (!pAxis->mutexId)
break;
epicsMutexLock(pAxis->mutexId);
if (pAxis->moving_ind == true)
{
axisDone = 0;
anyMoving = 1;
if (*pAxis->movetimer < epicsTime::getCurrent())
pAxis->moving_ind = false;
}
else
axisDone = 1;
motorParam->setInteger(pAxis->params, motorAxisDone, axisDone);
if (pAxis->axisStatus & ANC150_HOME)
motorParam->setInteger(pAxis->params, motorAxisHomeSignal, 1);
else
motorParam->setInteger(pAxis->params, motorAxisHomeSignal, 0);
if (pAxis->axisStatus & ANC150_HIGH_LIMIT)
motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, 1);
else
motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, 0);
if (pAxis->axisStatus & ANC150_LOW_LIMIT)
motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, 1);
else
motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, 0);
motorParam->setDouble(pAxis->params, motorAxisPosition, pAxis->currentPosition);
motorParam->setDouble(pAxis->params, motorAxisEncoderPosn, pAxis->currentPosition);
PRINT(pAxis->logParam, IODRIVER, "ANC150Poller: axis %d axisStatus=%x, position=%f\n",
pAxis->axis, pAxis->axisStatus, pAxis->currentPosition);
motorParam->callCallback(pAxis->params);
epicsMutexUnlock(pAxis->mutexId);
} /* Next axis */
if (forcedFastPolls > 0)
{
timeout = pController->movingPollPeriod;
forcedFastPolls--;
}
else if (anyMoving)
{
timeout = pController->movingPollPeriod;
}
else
{
timeout = pController->idlePollPeriod;
}
} /* End while */
}
static int motorANC150LogMsg(void *param, const motorAxisLogMask_t mask, const char *pFormat,...)
{
va_list pvar;
int nchar;
va_start(pvar, pFormat);
nchar = vfprintf(stdout, pFormat, pvar);
va_end(pvar);
printf("\n");
return(nchar);
}
int ANC150AsynSetup(int num_controllers) /* number of ANC150 controllers in system. */
{
if (num_controllers < 1)
{
printf("ANC150Setup, num_controllers must be > 0\n");
return(MOTOR_AXIS_ERROR);
}
numANC150Controllers = num_controllers;
pANC150Controller = (ANC150Controller *) calloc(numANC150Controllers, sizeof(ANC150Controller));
return(MOTOR_AXIS_OK);
}
int ANC150AsynConfig(int card, /* Controller number */
const char *portName, /* asyn serial port name */
int numAxes, /* Number of axes this controller supports */
int movingPollPeriod, /* Time to poll (msec) when an axis is in motion */
int idlePollPeriod) /* Time to poll (msec) when an axis is idle. 0 for no polling */
{
AXIS_HDL pAxis;
int axis;
ANC150Controller *pController;
char threadName[20];
int status;
int retry = 0;
char inputBuff[BUFFER_SIZE];
char outputBuff[BUFFER_SIZE];
size_t nRead;
int eomReason;
if (numANC150Controllers < 1)
{
printf("ANC150Config: no ANC150 controllers allocated, call ANC150Setup first\n");
return(MOTOR_AXIS_ERROR);
}
if ((card < 0) || (card >= numANC150Controllers))
{
printf("ANC150Config: card must in range 0 to %d\n", numANC150Controllers - 1);
return(MOTOR_AXIS_ERROR);
}
if ((numAxes < 1) || (numAxes > ANC150_MAX_AXES))
{
printf("ANC150Config: numAxes must in range 1 to %d\n", ANC150_MAX_AXES);
return(MOTOR_AXIS_ERROR);
}
pController = &pANC150Controller[card];
pController->numAxes = numAxes;
pController->movingPollPeriod = movingPollPeriod / 1000.;
pController->idlePollPeriod = idlePollPeriod / 1000.;
status = pasynOctetSyncIO->connect(portName, 0, &pController->pasynUser, NULL);
if (status != asynSuccess)
{
printf("ANC150AsynConfig: cannot connect to asyn port %s\n", portName);
return(MOTOR_AXIS_ERROR);
}
/* Set command End-of-string */
pasynOctetSyncIO->setInputEos(pController->pasynUser, ANC150_IN_EOS,strlen(ANC150_IN_EOS));
pasynOctetSyncIO->setOutputEos(pController->pasynUser, ANC150_OUT_EOS,strlen(ANC150_OUT_EOS));
do
{
pasynOctetSyncIO->flush(pController->pasynUser);
status = sendAndReceive(pController, "ver", inputBuff, sizeof(inputBuff));
if (status == asynSuccess && strncmp(inputBuff, "attocube", 8) == 0)
strncpy(pController->firmwareVersion, &inputBuff[19], sizeof(inputBuff));
else
status = asynError;
retry++;
/* Return value is length of response string */
} while (status != asynSuccess && retry < 3);
if (status != asynSuccess)
return(MOTOR_AXIS_ERROR);
status = pasynOctetSyncIO->read(pController->pasynUser, inputBuff, sizeof(inputBuff),
TIMEOUT, &nRead, &eomReason);
strcat(pController->firmwareVersion, " ");
strcat(pController->firmwareVersion, inputBuff);
pasynOctetSyncIO->flush(pController->pasynUser);
/* Don't initialize pAxis until all the error checks have passed;
* prevents drvAsynMotorConfigure from crashing. */
pController->pAxis = (AXIS_HDL) calloc(numAxes, sizeof(motorAxis));
for (axis = 0; axis < numAxes; axis++)
{
pAxis = &pController->pAxis[axis];
pAxis->pController = pController;
pAxis->card = card;
pAxis->axis = axis;
pAxis->mutexId = epicsMutexMustCreate();
pAxis->params = motorParam->create(0, MOTOR_AXIS_NUM_PARAMS);
motorParam->setInteger(pAxis->params, motorAxisClosedLoop, 1);
pAxis->currentPosition = 0.0;
pAxis->movetimer = new epicsTime();
pAxis->moving_ind = false;
getFreq(pController, axis, outputBuff, inputBuff, sizeof(inputBuff));
sprintf(outputBuff, "setm %d stp", pAxis->axis + 1);
status = sendOnly(pAxis->pController, outputBuff);
}
pController->pollEventId = epicsEventMustCreate(epicsEventEmpty);
/* Create the poller thread for this controller */
epicsSnprintf(threadName, sizeof(threadName), "ANC150:%d", card);
epicsThreadCreate(threadName,
epicsThreadPriorityMedium,
epicsThreadGetStackSize(epicsThreadStackMedium),
(EPICSTHREADFUNC) ANC150Poller, (void *) pController);
return(MOTOR_AXIS_OK);
}
static int sendOnly(ANC150Controller * pController, char *outputBuff)
{
char inputBuff[BUFFER_SIZE];
int nRequested = strlen(outputBuff);
size_t nActual, nRead;
asynStatus status;
int eomReason;
status = pasynOctetSyncIO->writeRead(pController->pasynUser, outputBuff, nRequested,
inputBuff, sizeof(inputBuff), TIMEOUT, &nActual,
&nRead, &eomReason);
if (nActual != (size_t) nRequested)
status = asynError;
if (status != asynSuccess)
{
asynPrint(pController->pasynUser, ASYN_TRACE_ERROR,
"drvANC150Asyn:sendOnly: error sending command %d, sent=%d, status=%d\n",
outputBuff, nActual, status);
}
else /* Eat the echo. */
status = pasynOctetSyncIO->read(pController->pasynUser, inputBuff, sizeof(inputBuff),
TIMEOUT, &nRead, &eomReason);
return(status);
}
static asynStatus sendAndReceive(ANC150Controller *pController, char *outputBuff, char *inputBuff, int inputSize)
{
int nWriteRequested = strlen(outputBuff);
size_t nWrite, nRead;
int eomReason;
asynStatus status;
status = pasynOctetSyncIO->writeRead(pController->pasynUser,
outputBuff, nWriteRequested,
inputBuff, inputSize,
TIMEOUT, &nWrite, &nRead, &eomReason);
if (nWrite != (size_t) nWriteRequested)
status = asynError;
if (status != asynSuccess)
{
asynPrint(pController->pasynUser, ASYN_TRACE_ERROR,
"drvANC150Asyn:sendAndReceive error calling writeRead, output=%s status=%d, error=%s\n",
outputBuff, status, pController->pasynUser->errorMessage);
}
else /* Eat the echo. */
status = pasynOctetSyncIO->read(pController->pasynUser, inputBuff, inputSize, TIMEOUT, &nRead, &eomReason);
return(status);
}
static asynStatus getFreq(ANC150Controller *pController, int axis,
char *outputBuff, char *inputBuff, int inputSize)
{
AXIS_HDL pAxis;
asynStatus status;
sprintf(outputBuff, "getf %d", axis + 1);
status = sendAndReceive(pController, outputBuff, inputBuff, inputSize);
pAxis = pController->pAxis;
if (status != asynSuccess || sscanf(inputBuff, "frequency = %d", &pAxis->frequency) != 1)
pAxis->frequency = 1;
return(status);
}
extern "C"
{
// Setup arguments
static const iocshArg setupArg0 = {"Maximum # of controllers", iocshArgInt};
// Config arguments
static const iocshArg configArg0 = {"Card# being configured", iocshArgInt};
static const iocshArg configArg1 = {"asyn port name", iocshArgString};
static const iocshArg configArg2 = {"Number of Axes", iocshArgInt};
static const iocshArg configArg3 = {"Moving poll rate", iocshArgInt};
static const iocshArg configArg4 = {"Idle poll rate", iocshArgInt};
static const iocshArg *const SetupArgs[1] = {&setupArg0};
static const iocshArg *const ConfigArgs[5] = {&configArg0, &configArg1, &configArg2,
&configArg3, &configArg4};
static const iocshFuncDef setupANC150 = {"ANC150AsynSetup", 1, SetupArgs};
static const iocshFuncDef configANC150 = {"ANC150AsynConfig", 5, ConfigArgs};
static void setupANC150CallFunc(const iocshArgBuf *args)
{
ANC150AsynSetup(args[0].ival);
}
static void configANC150CallFunc (const iocshArgBuf *args)
{
ANC150AsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
}
static void ANC150Register(void)
{
iocshRegister(&setupANC150, setupANC150CallFunc);
iocshRegister(&configANC150, configANC150CallFunc);
}
epicsExportRegistrar(ANC150Register);
} // extern "C"