diff --git a/Makefile.RHEL7 b/Makefile.RHEL7 index 010c19b..bdd2198 100644 --- a/Makefile.RHEL7 +++ b/Makefile.RHEL7 @@ -26,6 +26,7 @@ SOURCES += sinqEPICSApp/src/EL734Driver.cpp SOURCES += sinqEPICSApp/src/NanotecDriver.cpp SOURCES += sinqEPICSApp/src/stptok.cpp SOURCES += sinqEPICSApp/src/PhytronDriver.cpp +SOURCES += sinqEPICSApp/src/EuroMoveDriver.cpp SOURCES += sinqEPICSApp/src/pmacAsynIPPort.c SOURCES += sinqEPICSApp/src/pmacAxis.cpp SOURCES += sinqEPICSApp/src/pmacController.cpp diff --git a/iocBoot/iocsinqEPICS/euromovetest.cmd b/iocBoot/iocsinqEPICS/euromovetest.cmd new file mode 100755 index 0000000..7039110 --- /dev/null +++ b/iocBoot/iocsinqEPICS/euromovetest.cmd @@ -0,0 +1,25 @@ +#!/usr/local/bin/iocsh + +require sinq,koennecke + +epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/iocBoot/iocsinqEPICS") +epicsEnvSet("BASE","/afs/psi.ch/project/sinqdev/sinqepicsapp") +epicsEnvSet("dbPATH","${EPICS_BASE}/dbd:${ASYN}/dbd:${MOTOR}/dbd") + +cd ${TOP} + +## Register all support components +dbLoadDatabase "../../dbd/sinqEPICS.dbd" + + +drvAsynIPPortConfigure("serial1", "amor-ts:3002",0,0,0) +#drvAsynIPPortConfigure("serial1", "localhost:9090",0,0,0) +EuroMoveCreateController("llb","serial1",1); + +### Motors + +dbLoadRecords("$(BASE)/sinqEPICSApp/Db/asynRecord.db","P=KM36:,R=serial1,PORT=serial1,ADDR=0,OMAX=80,IMAX=80") +dbLoadTemplate "motor.substitutions.eurollb" + + +iocInit diff --git a/iocBoot/iocsinqEPICS/motor.substitutions.eurollb b/iocBoot/iocsinqEPICS/motor.substitutions.eurollb new file mode 100644 index 0000000..ac19d78 --- /dev/null +++ b/iocBoot/iocsinqEPICS/motor.substitutions.eurollb @@ -0,0 +1,13 @@ +file "$(BASE)/sinqEPICSApp/Db/basic_asyn_motor.db" +{ +pattern +{P, N, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, RDBD, DHLM, DLLM, INIT} +{KM36:llb:, 1, "m$(N)", "asynMotor", llb, 1, "m1", mm, Pos, 2.0, 0.1, .2, 0, 1, .2, .01, 3, 0.2, 1000, 0, "1"} +} + +file "$(BASE)/sinqEPICSApp/Db/motorMessage.db" +{ +pattern +{P,N, M,PORT} +{KM36:llb:, 1, "m$(N)",llb} +} \ No newline at end of file diff --git a/sinqEPICSApp/src/EuroMoveDriver.cpp b/sinqEPICSApp/src/EuroMoveDriver.cpp new file mode 100644 index 0000000..f69b7c7 --- /dev/null +++ b/sinqEPICSApp/src/EuroMoveDriver.cpp @@ -0,0 +1,481 @@ +/* +FILENAME... EuroMoveDriver.cpp +USAGE... Motor driver support for the EuroMove motor controller from LLB. + +This is a driver for the EuroMove motor controller from LLB. This device talks to us either via +GPIB or USB. The controller is fairly simple. It can control quite a number of motors. + +*/ + + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "EuroMoveDriver.h" +#include + +#define IDLEPOLL 60 + +/** Creates a new EuroMoveController object. + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] EuroMovePortName The name of the drvAsynSerialPort that was created previously to connect to the EuroMove controller + * \param[in] numAxes The number of axes that this controller supports + */ +EuroMoveController::EuroMoveController(const char *portName, const char *EuroMovePortName, int numAxis) + : SINQController(portName, EuroMovePortName,numAxis+1) +{ + asynStatus status; + static const char *functionName = "EuroMoveController::EuroMoveController"; + + /* Connect to EuroMove controller */ + status = pasynOctetSyncIO->connect(EuroMovePortName, 0, &pasynUserController_, NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to EuroMove controller\n", + functionName); + } + const char *terminator = "\r"; + pasynOctetSyncIO->setOutputEos(pasynUserController_,terminator,strlen(terminator)); + pasynOctetSyncIO->setInputEos(pasynUserController_,terminator,strlen(terminator)); + + for(int i = 0; i < numAxis; i++){ + new EuroMoveAxis(this, i+1); + } + startPoller(1000./1000., IDLEPOLL, 2); +} + + +/** Creates a new EuroMoveController object. + * Configuration command, called directly or from iocsh + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] EuroMovePortName The name of the drvAsynIPPPort that was created previously to connect to the EuroMove controller + * \param[in] numAxes The number of axes that this controller supports + */ +extern "C" int EuroMoveCreateController(const char *portName, const char *EuroMovePortName, const int numAxis) +{ + new EuroMoveController(portName, EuroMovePortName, numAxis); + return(asynSuccess); +} + +/** Reports on status of the driver + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * If details > 0 then information is printed about each axis. + * After printing controller-specific information it calls asynMotorController::report() + */ +void EuroMoveController::report(FILE *fp, int level) +{ + fprintf(fp, "EuroMove motor driver %s, numAxes=%d\n", + this->portName, numAxes_); + + // Call the base class method + asynMotorController::report(fp, level); +} + +/** Returns a pointer to an EuroMoveAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +EuroMoveAxis* EuroMoveController::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an EuroMoveAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +EuroMoveAxis* EuroMoveController::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + +/** + * send a command to the EuroMove and read the reply. Do test for errors. + * \param[in] command The command to send + * \param[out] reply The controllers reply + */ + +asynStatus EuroMoveController::transactController(int axisNo,char command[COMLEN], char reply[COMLEN]) +{ + asynStatus status; + SINQAxis *axis = getAxis(axisNo); + size_t out, in; + int reason, errCode, lCode; + char pDummy[50], lReply[50]; + const char *lCommand = {"L"}; + + + + pasynOctetSyncIO->flush(pasynUserController_); + + + status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command), + reply,sizeof(reply), 10.,&out,&in,&reason); + if(status != asynSuccess){ + if(axis!= NULL){ + axis->updateMsgTxtFromDriver("Lost connection to motor controller"); + errlogPrintf("Lost connection to motor controller\n"); + } + return status; + } + + if(strstr(reply,"ERROR") != NULL){ + sscanf((const char *)reply, "%s %d", pDummy, &errCode); + switch(errCode){ + case 0: + axis->updateMsgTxtFromDriver("Syntax error or parameter out of range"); + errlogPrintf("Syntax error or parameter out or range on %d\n", axisNo); + break; + case 1: + axis->updateMsgTxtFromDriver("Timeout communicating with daughter board"); + errlogPrintf("Timeout communicating with daughter board on %d\n", axisNo); + break; + case 2: + axis->updateMsgTxtFromDriver("This is not a drivable motor"); + errlogPrintf("This is not a drivable motor on %d\n", axisNo); + break; + case 3: + axis->updateMsgTxtFromDriver("Encoder anomaly"); + errlogPrintf("Encoder anomaly on %d\n", axisNo); + break; + case 4: + axis->updateMsgTxtFromDriver("Attempt to move across limit switch"); + errlogPrintf("Attempt to move across limit switch on %d\n", axisNo); + break; + case 5: + axis->updateMsgTxtFromDriver("Badly configured relay system"); + errlogPrintf("Badly configured relay system on %d\n", axisNo); + break; + case 6: + axis->updateMsgTxtFromDriver("Non valid backup"); + errlogPrintf("Invalid backup on %d\n", axisNo); + break; + case 7: + axis->updateMsgTxtFromDriver("Backup failed"); + errlogPrintf("Backup failed on %d\n", axisNo); + break; + } + return asynError; + } + + /* + Test system status + */ + status = pasynOctetSyncIO->writeRead(pasynUserController_, lCommand, strlen(lCommand), + lReply,sizeof(lReply), 1.,&out,&in,&reason); + if(status != asynSuccess){ + if(axis!= NULL){ + axis->updateMsgTxtFromDriver("Lost connection to motor controller"); + errlogPrintf("Lost connection to motor controller\n"); + } + return status; + } + sscanf(lReply, "%x", &lCode); + //errlogPrintf("System status returned %s, converted to %d\n", lReply, lCode); + if((lCode & 1) > 0){ + axis->updateMsgTxtFromDriver("Syntax error or impossible to execute"); + errlogPrintf("Syntax error or impossible to execute for %s on %d\n", command, axisNo); + return asynError; + } + + + return status; +} + +// These are the EuroMoveAxis methods + +/** Creates a new EuroMoveAxis object. + * \param[in] pC Pointer to the EuroMoveController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +EuroMoveAxis::EuroMoveAxis(EuroMoveController *pC, int axisNo) + : SINQAxis(pC, axisNo), + pC_(pC) +{ + motNo = axisNo; +} + + + +/** Reports on status of the axis + * \param[in] fp The file pointer on which report information will be written + * \param[in] level The level of report detail desired + * + * After printing device-specific information calls asynMotorAxis::report() + */ +void EuroMoveAxis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " axis %d\n", + axisNo_); + } + asynMotorAxis::report(fp, level); +} + + +asynStatus EuroMoveAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + //static const char *functionName = "EuroMoveAxis::move"; + char command[COMLEN], reply[COMLEN]; + + updateMsgTxtFromDriver(""); + + if (relative) { + position += this->position; + } + homing = 0; + sprintf(command,"G%d=%d", motNo, (int)position); + status = pC_->transactController(motNo,command,reply); + next_poll = -1; + return status; +} + +asynStatus EuroMoveAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + asynStatus status = asynSuccess; + //static const char *functionName = "EuroMoveAxis::home"; + + /* + I do not know if this controller actually can do homing + */ + return status; +} + +asynStatus EuroMoveAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + double target; + + //static const char *functionName = "EuroMoveAxis::moveVelocity"; + + // asynPrint(pasynUser_, ASYN_TRACE_FLOW, + // "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", + // functionName, minVelocity, maxVelocity, acceleration); + + updateMsgTxtFromDriver(""); + + if (maxVelocity > 0.) { + /* This is a positive move */ + pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&target); + } else { + /* This is a negative move */ + pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&target); + } + status = move(target,0,0,0,0); + next_poll = -1; + return status; +} + +asynStatus EuroMoveAxis::sendStop() +{ + asynStatus status; + char command[COMLEN], reply[COMLEN]; + + sprintf(command, "B%d", motNo); + status = pC_->transactController(axisNo_,command,reply); + return status; +} + +asynStatus EuroMoveAxis::stop(double acceleration ) +{ + asynStatus status; + // static const char *functionName = "EuroMoveAxis::stop"; + + status = sendStop(); + errlogPrintf("Sent STOP on Axis %d\n", axisNo_); + updateMsgTxtFromDriver("Axis interrupted"); + + return status; +} + +asynStatus EuroMoveAxis::setPosition(double position) +{ + asynStatus status; + //static const char *functionName = "EuroMoveAxis::setPosition"; + char command[COMLEN], reply[COMLEN]; + + errlogPrintf("EuroMoveAxis::setPosition called with %lf\n", position); + + sprintf(command, "I%d=%d",motNo,(int)position); + status = pC_->transactController(axisNo_,command,reply); + next_poll = -1; + + return status; +} + +asynStatus EuroMoveAxis::setClosedLoop(bool closedLoop) +{ + //static const char *functionName = "EuroMoveAxis::setClosedLoop"; + + /* + This belongs into the Kingdom of Electronics. + We do not do this. + */ + + return asynError; +} + +/** Polls the axis. + * This function reads the motor position, the limit status, the home status, the moving status, + * and the drive power-on status. + * It calls setIntegerParam() and setDoubleParam() for each item that it polls, + * and then calls callParamCallbacks() at the end. + * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */ +asynStatus EuroMoveAxis::poll(bool *moving) +{ + asynStatus comStatus = asynSuccess; + char command[COMLEN], reply[COMLEN]; + unsigned int axStatus; + + // protect against excessive polling + if(time(NULL) < next_poll){ + *moving = false; + return asynSuccess; + } + + setIntegerParam(pC_->motorStatusProblem_, false); + + /* + read the current position + */ + sprintf(command,"A%d",motNo); + comStatus = pC_->transactController(axisNo_,command,reply); + if(comStatus == asynError) { + setIntegerParam(pC_->motorStatusProblem_, true); + goto skip; + } + sscanf(reply,"%d",&position); + setDoubleParam(pC_->motorPosition_,(double)position); + + // Read the moving status of this motor + sprintf(command,"E%d",motNo); + comStatus = pC_->transactController(axisNo_,command,reply); + if(comStatus == asynError){ + setIntegerParam(pC_->motorStatusProblem_, true); + goto skip; + } + + errlogPrintf("Axis %d, status reply %s, position %d\n", axisNo_, reply, position); + sscanf(reply, "%x", &axStatus); + + if((axStatus & 128) > 0) { // test bit 8 + *moving = true; + next_poll = -1; + setIntegerParam(pC_->motorStatusDone_, false); + } else { + *moving = false; + next_poll = time(NULL)+IDLEPOLL; + setIntegerParam(pC_->motorStatusDone_, true); + } + + /* Testing limit switches */ + if((axStatus & 2) > 0){ // bit 2 + setIntegerParam(pC_->motorStatusHighLimit_, true); + errlogPrintf("HighLimit detected on %d\n", motNo); + sendStop(); + } else { + setIntegerParam(pC_->motorStatusHighLimit_, false); + } + if((axStatus & 1) > 0){ // bit 1 + setIntegerParam(pC_->motorStatusLowLimit_, true); + errlogPrintf("LowLimit detected on %d\n", motNo); + sendStop(); + } else { + setIntegerParam(pC_->motorStatusLowLimit_, false); + } + + /* there could be errors reported in the motor status */ + if((axStatus & 8) > 0){ // bit 4 + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Internal timeout detected"); + errlogPrintf("Internal timeout detected on %d\n", motNo); + comStatus = asynError; + } + if((axStatus & 4) > 0){ // bit 3 + setIntegerParam(pC_->motorStatusProblem_, true); + updateMsgTxtFromDriver("Encoding anomaly"); + errlogPrintf("Encoding anomaly on %d\n", motNo); + comStatus = asynError; + } + + + skip: + callParamCallbacks(); + return comStatus; +} + +/** Code for configuring the motNo on axis **/ +extern "C" { + asynStatus EuroMoveConfigureAxis(const char *euroMoveName, unsigned int axisNo, unsigned int motNo) + { + EuroMoveController *pC = NULL; + EuroMoveAxis *pAxis = NULL; + + pC = (EuroMoveController *)findAsynPortDriver(euroMoveName); + if(!pC){ + errlogPrintf("%s driver not found", euroMoveName); + return asynError; + } + pC->unlock(); + + pAxis = (EuroMoveAxis *)pC->getAxis(axisNo); + if(!pAxis){ + errlogPrintf("axis %d driver not found on %s", axisNo, euroMoveName); + return asynError; + } + pAxis->setMotNo(motNo); + + return asynSuccess; + } +} + +static const iocshArg EuroMoveConfigArg0 = {"Port name", iocshArgString}; +static const iocshArg EuroMoveConfigArg1 = {"Axis Index", iocshArgInt}; +static const iocshArg EuroMoveConfigArg2 = {"Motor number in EuroMove", iocshArgInt}; +static const iocshArg * const EuroMoveConfigArgs[] = {&EuroMoveConfigArg0, + &EuroMoveConfigArg1, + &EuroMoveConfigArg2 +}; +static const iocshFuncDef EuroMoveConfigDef = {"EuroMoveConfigureAxis", 3, EuroMoveConfigArgs}; +static void EuroMoveConfigCallFunc(const iocshArgBuf *args) +{ + EuroMoveConfigureAxis(args[0].sval, (unsigned int)args[1].ival, (unsigned int)args[2].ival); +} + + +/** Code for iocsh registration */ +static const iocshArg EuroMoveCreateControllerArg0 = {"Port name", iocshArgString}; +static const iocshArg EuroMoveCreateControllerArg1 = {"EuroMove port name", iocshArgString}; +static const iocshArg EuroMoveCreateControllerArg2 = {"Number of axis", iocshArgInt}; +static const iocshArg * const EuroMoveCreateControllerArgs[] = {&EuroMoveCreateControllerArg0, + &EuroMoveCreateControllerArg1, + &EuroMoveCreateControllerArg2 +}; +static const iocshFuncDef EuroMoveCreateControllerDef = {"EuroMoveCreateController", 3, EuroMoveCreateControllerArgs}; +static void EuroMoveCreateControllerCallFunc(const iocshArgBuf *args) +{ + EuroMoveCreateController(args[0].sval, args[1].sval, args[2].ival); +} + +static void EuroMoveRegister(void) +{ + iocshRegister(&EuroMoveCreateControllerDef, EuroMoveCreateControllerCallFunc); + iocshRegister(&EuroMoveConfigDef, EuroMoveConfigCallFunc); +} + +extern "C" { +epicsExportRegistrar(EuroMoveRegister); +} diff --git a/sinqEPICSApp/src/EuroMoveDriver.h b/sinqEPICSApp/src/EuroMoveDriver.h new file mode 100644 index 0000000..38cff35 --- /dev/null +++ b/sinqEPICSApp/src/EuroMoveDriver.h @@ -0,0 +1,57 @@ +/* +FILENAME... EuroMoveDriver.h +USAGE... Motor driver support for the LLB EuroMove controller + +Mark Koennecke +January 2020 + +*/ + +#include "SINQController.h" +#include "SINQAxis.h" + +#define COMLEN 80 + +class EuroMoveAxis : public SINQAxis +{ +public: + /* These are the methods we override from the base class */ + EuroMoveAxis(class EuroMoveController *pC, int axis); + void report(FILE *fp, int level); + asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration); + asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration); + asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards); + asynStatus stop(double acceleration); + asynStatus poll(bool *moving); + asynStatus setPosition(double position); + asynStatus setClosedLoop(bool closedLoop); + void setMotNo(unsigned int no){ + motNo = no; + } + +private: + EuroMoveController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + time_t next_poll; + int motNo; // motor number in the EuroMove controller + int position; + int homing; + asynStatus sendStop(); +friend class EuroMoveController; +}; + +class EuroMoveController : public SINQController { +public: + EuroMoveController(const char *portName, const char *EuroMovePortName, const int nAxis); + + void report(FILE *fp, int level); + EuroMoveAxis* getAxis(asynUser *pasynUser); + EuroMoveAxis* getAxis(int axisNo); + +friend class EuroMoveAxis; + private: + asynUser *pasynUserController_; + + asynStatus transactController(int axisNo, char command[COMLEN], char reply[COMLEN]); + +}; diff --git a/sinqEPICSApp/src/Makefile b/sinqEPICSApp/src/Makefile index ce12a6b..fbe1eca 100644 --- a/sinqEPICSApp/src/Makefile +++ b/sinqEPICSApp/src/Makefile @@ -28,6 +28,7 @@ sinqEPICS_SRCS += EL734Driver.cpp devScalerEL737.c pmacAsynIPPort.c SINQAxis.cpp sinqEPICS_SRCS += pmacController.cpp pmacAxis.cpp sinqEPICS_SRCS += NanotecDriver.cpp stptok.cpp sinqEPICS_SRCS += PhytronDriver.cpp +sinqEPICS_SRCS += EuroMoveDriver.cpp # Build the main IOC entry point on workstation OSs. diff --git a/sinqEPICSApp/src/sinq.dbd b/sinqEPICSApp/src/sinq.dbd index aec15c4..b641a9e 100644 --- a/sinqEPICSApp/src/sinq.dbd +++ b/sinqEPICSApp/src/sinq.dbd @@ -3,6 +3,7 @@ #--------------------------------------------- registrar(EL734Register) registrar(PhytronRegister) +registrar(EuroMoveRegister) registrar(NanotecRegister) registrar(pmacControllerRegister) registrar(pmacAsynIPPortRegister)