diff --git a/motorApp/AttocubeSrc/Makefile b/motorApp/AttocubeSrc/Makefile new file mode 100644 index 00000000..4799070a --- /dev/null +++ b/motorApp/AttocubeSrc/Makefile @@ -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 + diff --git a/motorApp/AttocubeSrc/devAttocube.dbd b/motorApp/AttocubeSrc/devAttocube.dbd new file mode 100644 index 00000000..8d242a16 --- /dev/null +++ b/motorApp/AttocubeSrc/devAttocube.dbd @@ -0,0 +1,4 @@ +# attocube ANC 150 asyn motor driver support. +driver(motorANC150) +registrar(ANC150Register) + diff --git a/motorApp/AttocubeSrc/drvANC150Asyn.cc b/motorApp/AttocubeSrc/drvANC150Asyn.cc new file mode 100644 index 00000000..7a07acca --- /dev/null +++ b/motorApp/AttocubeSrc/drvANC150Asyn.cc @@ -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 +#include +#include + +#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" + +