From 5c0d5d8c1f15952f9bd3b6bc0c649d87f3f771e5 Mon Sep 17 00:00:00 2001 From: Keenan Lang Date: Wed, 14 Sep 2016 15:22:27 -0500 Subject: [PATCH] Scriptable motor support --- .../iocWithAsyn/motor.substitutions.script | 15 + iocBoot/iocWithAsyn/scripts/softMotor.lua | 61 ++ iocBoot/iocWithAsyn/scripts/vmc.lua | 66 ++ iocBoot/iocWithAsyn/st.cmd.script | 18 + motorApp/Db/ScriptMotorReload.db | 9 + motorApp/Makefile | 6 + motorApp/ScriptMotorSrc/Makefile | 30 + motorApp/ScriptMotorSrc/ScriptMotorDriver.cpp | 859 ++++++++++++++++++ motorApp/ScriptMotorSrc/ScriptMotorDriver.dbd | 1 + motorApp/ScriptMotorSrc/ScriptMotorDriver.h | 76 ++ 10 files changed, 1141 insertions(+) create mode 100644 iocBoot/iocWithAsyn/motor.substitutions.script create mode 100644 iocBoot/iocWithAsyn/scripts/softMotor.lua create mode 100644 iocBoot/iocWithAsyn/scripts/vmc.lua create mode 100644 iocBoot/iocWithAsyn/st.cmd.script create mode 100644 motorApp/Db/ScriptMotorReload.db create mode 100644 motorApp/ScriptMotorSrc/Makefile create mode 100644 motorApp/ScriptMotorSrc/ScriptMotorDriver.cpp create mode 100644 motorApp/ScriptMotorSrc/ScriptMotorDriver.dbd create mode 100644 motorApp/ScriptMotorSrc/ScriptMotorDriver.h diff --git a/iocBoot/iocWithAsyn/motor.substitutions.script b/iocBoot/iocWithAsyn/motor.substitutions.script new file mode 100644 index 00000000..c2f63ea9 --- /dev/null +++ b/iocBoot/iocWithAsyn/motor.substitutions.script @@ -0,0 +1,15 @@ +file "$(TOP)/db/asyn_motor.db" +{ + pattern + {P, M, DTYP, PORT, ADDR, DESC, EGU, DIR, VMAX, VELO, VBAS, ACCL, BDST, BVEL, BACC, MRES, PREC, DHLM, DLLM, INIT} + {IOC:, m1, "asynMotor", VMC_MOTOR, 0, "VMCMotor", mm, Pos, 15., 3., .05, .5, 0, 1.0, 2, .025, 5, 0, 0, ""} + {IOC:, m2, "asynMotor", SOFT_MOTOR, 0, "SoftMotor", mm, Pos, 15., 3., .05, .5, 0, 1.0, 2, .025, 5, 0, 0, ""} +} + +file "$(TOP)/db/ScriptMotorReload.db" +{ + pattern + {P, PORT} + {IOC:, SOFT_MOTOR} + {IOC:, VMC_MOTOR} +} diff --git a/iocBoot/iocWithAsyn/scripts/softMotor.lua b/iocBoot/iocWithAsyn/scripts/softMotor.lua new file mode 100644 index 00000000..a03e698e --- /dev/null +++ b/iocBoot/iocWithAsyn/scripts/softMotor.lua @@ -0,0 +1,61 @@ +MovingPollPeriod = 0.25 + +lastPos = 0 +targetPos = 0 + +function move(position, relative, minVel, maxVel, accel) + local MRES = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_REC_RESOLUTION") + + if (relative) then + local prev = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_POSITION") + targetPos = prev + (position * MRES) + else + targetPos = (position * MRES) + end + + epics.put(DRIVE_PV, targetPos) + + if (position > 0) then + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", 1) + else + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", 0) + end + + asyn.callParamCallbacks(DRIVER, AXIS) +end + + +function poll() + local curr = epics.get(READBACK_PV) + local MRES = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_REC_RESOLUTION") + + asyn.setDoubleParam( DRIVER, AXIS, "MOTOR_POSITION", curr / MRES) + + local done = 0 + local moving = 1 + + if (curr == targetPos) then + done = 1 + moving = 0 + elseif (math.abs(curr - targetPos) <= MRES and lastPos == curr) then + done = 1 + moving = 0 + end + + lastPos = curr + + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DONE", done) + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_MOVING", moving) + + asyn.callParamCallbacks(DRIVER, AXIS) + + return (done ~= 1) +end + + +function stop(acceleration) + local curr = asyn.getDoubleParam( DRIVER, AXIS, "MOTOR_POSITION") + + epics.put(DRIVE_PV, curr) + targetPos = curr +end diff --git a/iocBoot/iocWithAsyn/scripts/vmc.lua b/iocBoot/iocWithAsyn/scripts/vmc.lua new file mode 100644 index 00000000..46ff05fe --- /dev/null +++ b/iocBoot/iocWithAsyn/scripts/vmc.lua @@ -0,0 +1,66 @@ +IdlePollPeriod = 1.0 +MovingPollPeriod = 0.25 +ForcedFastPolls = 2 + +InTerminator = "\r\n" +OutTerminator = "\r\n" + +function sendAccelAndVelocity(minVel, maxVel, accel) + asyn.writeread( string.format( "%d BAS %f", AXIS + 1, minVel) , PORT); + asyn.writeread( string.format( "%d VEL %f", AXIS + 1, maxVel) , PORT); + asyn.writeread( string.format( "%d ACC %f", AXIS + 1, accel ) , PORT); +end + + +function move(position, relative, minVel, maxVel, accel) + sendAccelAndVelocity( minVel, maxVel, accel) + + if (relative) then + asyn.writeread( string.format( "%d MR %d", AXIS + 1, math.floor(position) ) , PORT) + else + asyn.writeread( string.format( "%d MV %d", AXIS + 1, math.floor(position) ) , PORT) + end +end + + +function moveVelocity(minVel, maxVel, accel) + sendAccelAndVelocity( minVel, maxVel, accel) + + asyn.writeread( string.format( "%d JOG %f", AXIS + 1, maxVel) , PORT); +end + + +function poll() + asyn.write( string.format( "%d POS?", AXIS + 1) , PORT) + + asyn.setDoubleParam( DRIVER, AXIS, "MOTOR_POSITION", tonumber( asyn.read(PORT) ) ) + + asyn.write( string.format( "%d ST?", AXIS + 1) , PORT) + + local status = tonumber( asyn.read(PORT) ) + + local direction = (status & 1) + local done = (status & 2) >> 1 + local limit_high = (status & 8) >> 3 + local limit_low = (status & 16) >> 4 + + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DIRECTION", direction) + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_DONE", done) + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_MOVING", done ~ 1) + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_HIGH_LIMIT", limit_high) + asyn.setIntegerParam( DRIVER, AXIS, "MOTOR_STATUS_LOW_LIMIT", limit_low) + + asyn.callParamCallbacks(DRIVER, AXIS) + + return (done ~= 1) +end + + +function stop(acceleration) + asyn.writeread( string.format( "%d AB", AXIS + 1) , PORT); +end + + +function setPosition(position) + asyn.writeread( string.format( "%d POS %d", AXIS + 1, math.floor(position) ) , PORT); +end diff --git a/iocBoot/iocWithAsyn/st.cmd.script b/iocBoot/iocWithAsyn/st.cmd.script new file mode 100644 index 00000000..995c60a4 --- /dev/null +++ b/iocBoot/iocWithAsyn/st.cmd.script @@ -0,0 +1,18 @@ +< envPaths + +dbLoadDatabase("$(TOP)/dbd/WithAsyn.dbd") +WithAsyn_registerRecordDeviceDriver(pdbbase) + +epicsEnvSet("LUA_SCRIPT_PATH", "./scripts") + +# Connect to virtual motor controller server +drvAsynIPPortConfigure("VMC","127.0.0.1:31337", 0, 0, 0) + +#ScriptControllerConfig( "PORT_NAME", num_axes, "lua_script", "PARAMS=") +ScriptControllerConfig("VMC_MOTOR", 1, "vmc.lua", "PORT=VMC") + +ScriptControllerConfig("SOFT_MOTOR", 1, "softMotor.lua", "DRIVE_PV='IOC:m1.VAL', READBACK_PV='IOC:m2.RBV'") + +dbLoadTemplate("motor.substitutions.script") + +iocInit diff --git a/motorApp/Db/ScriptMotorReload.db b/motorApp/Db/ScriptMotorReload.db new file mode 100644 index 00000000..63904454 --- /dev/null +++ b/motorApp/Db/ScriptMotorReload.db @@ -0,0 +1,9 @@ +record(bo, "$(P)$(PORT):ScriptReload") +{ + field(DESC, "$(PORT) Script Reload") + field(DTYP, "asynInt32") + field(OUT, "@asyn($(PORT), 0, 0)RELOAD_SCRIPT") + + field(ZNAM, "Loaded") + field(ONAM, "Reload") +} diff --git a/motorApp/Makefile b/motorApp/Makefile index 36ce18e1..a2d877d7 100644 --- a/motorApp/Makefile +++ b/motorApp/Makefile @@ -105,6 +105,12 @@ MicronixSrc_DEPEND_DIRS = MotorSrc DIRS += PhytronSrc PhytronSrc_DEPEND_DIRS = MotorSrc +# The Script motor support requires the lua scripting module +ifdef SCRIPT +DIRS += ScriptMotorSrc +ScriptMotorSrc_DEPEND_DIRS = MotorSrc +endif + endif # Install the edl files diff --git a/motorApp/ScriptMotorSrc/Makefile b/motorApp/ScriptMotorSrc/Makefile new file mode 100644 index 00000000..e27e5fd6 --- /dev/null +++ b/motorApp/ScriptMotorSrc/Makefile @@ -0,0 +1,30 @@ +# Makefile +TOP=../.. + +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE +#============================= + +#================================================== +# Build an IOC support library +LIBRARY_IOC = ScriptMotor + +# motorRecord.h will be created from motorRecord.dbd +# install devMotorSoft.dbd into /dbd +DBD += ScriptMotorDriver.dbd + +INC += ScriptMotorDriver.h + +# The following are compiled and added to the Support library +ScriptMotor_SRCS += ScriptMotorDriver.cpp + +ScriptMotor_LIBS += motor +ScriptMotor_LIBS += asyn +ScriptMotor_LIBS += script +ScriptMotor_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/motorApp/ScriptMotorSrc/ScriptMotorDriver.cpp b/motorApp/ScriptMotorSrc/ScriptMotorDriver.cpp new file mode 100644 index 00000000..bea0d553 --- /dev/null +++ b/motorApp/ScriptMotorSrc/ScriptMotorDriver.cpp @@ -0,0 +1,859 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#include +#include "ScriptMotorDriver.h" + +#define NINT(f) (int)((f)>0 ? (f)+0.5 : (f)-0.5) + + +/************************************************ + * These are the ScriptMotorController methods * + ************************************************/ + + +/** Creates a new ScriptMotorController object. + * \param[in] asyn_port The name of the asyn port that will be created for this driver + * \param[in] serial_port The name of the drvAsynSerialPort that was created previously to connect to the VirtualMotor controller + * \param[in] max_axes The number of axes that this controller supports + * \param[in] script_file + * \param[in] params + */ +ScriptMotorController::ScriptMotorController(const char* asyn_port, + int max_axes, + const char* script_file, + const char* params) + : asynMotorController(asyn_port, + max_axes, + 1, // No. ScriptMotorController asyn parameters + 0, // No. additional interfaces beyond those in base class + 0, // No. additional callback interfaces beyond those in base class + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, // Default priority + 0) // Default stack size +{ + this->script = std::string(script_file); + + if (params) { this->init_params = std::string(params); } + else { this->init_params = std::string(""); } + + this->createParam("RELOAD_SCRIPT", asynParamInt32, &this->ScriptMotorReload); + + for (int axis = 0; axis < max_axes; axis += 1) + { + new ScriptMotorAxis(this, axis, script_file, params); + } + + this->startPoller(this->movingPollPeriod_, this->idlePollPeriod_, this->forcedFastPolls_); +} + +void ScriptMotorController::reload() +{ + this->lock(); + for (int index = 0; index < this->numAxes_; index += 1) + { + ScriptMotorAxis* axis = this->getAxis(index); + axis->reload(this->script.c_str(), this->init_params.c_str()); + } + this->unlock(); + + printf("Controller %s reloaded %s.\n", this->portName, this->script.c_str()); +} + +/** Creates a new ScriptMotorController object. + * Configuration command, called directly or from iocsh + * \param[in] asyn_port The name of the asyn port that will be created for this driver + * \param[in] max_axes The number of axes that this controller supports + * \param[in] script_file + * \param[in] params + */ +extern "C" int ScriptMotorCreateController(const char* asyn_port, + int max_axes, + const char* script_file, + const char* params) +{ + new ScriptMotorController(asyn_port, max_axes, script_file, params); + 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 ScriptMotorController::report(FILE *fp, int level) +{ + fprintf(fp, "Script Motor Controller driver %s\n", this->portName); + fprintf(fp, " numAxes=%d\n", numAxes_); + fprintf(fp, " moving poll period=%f\n", movingPollPeriod_); + fprintf(fp, " idle poll period=%f\n", idlePollPeriod_); + + // Call the base class method + asynMotorController::report(fp, level); +} + +asynStatus ScriptMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value) +{ + int function = pasynUser->reason; + + if (function == this->ScriptMotorReload) + { + if (value == 1) { this->reload(); } + return asynSuccess; + } + else + { + return asynMotorController::writeInt32(pasynUser, value); + } +} + +asynStatus ScriptMotorController::setIntegerParam(int list, int function, int value) +{ + if (function >= this->motorStatusDirection_ && function <= this->motorStatusHomed_) + { + ScriptMotorAxis* axis = (ScriptMotorAxis*) this->getAxis(list); + epicsUInt32 status = axis->setStatusParam(function, value); + asynMotorController::setIntegerParam(list, this->motorStatus_, status); + } + + return asynMotorController::setIntegerParam(list, function, value); +} + +asynStatus ScriptMotorController::setDoubleParam(int list, int function, double value) +{ + if (function == this->motorPosition_ || function == this->motorEncoderPosition_) + { + ScriptMotorAxis* axis = (ScriptMotorAxis*) this->getAxis(list); + axis->setPositionParam(function, value); + } + + return asynMotorController::setDoubleParam(list, function, value); +} + +void ScriptMotorController::configAxis(int axisNo, const char* params) +{ + ScriptMotorAxis* axis = this->getAxis(axisNo); + + if (params) { axis->params = std::string(params); } + else { axis->params = std::string(""); } + + axis->config(params); +} + +/** Returns a pointer to an ScriptMotorAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +ScriptMotorAxis* ScriptMotorController::getAxis(asynUser *pasynUser) +{ + return static_cast(asynMotorController::getAxis(pasynUser)); +} + + +/** Returns a pointer to an ScriptMotorAxis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +ScriptMotorAxis* ScriptMotorController::getAxis(int axisNo) +{ + return static_cast(asynMotorController::getAxis(axisNo)); +} + + +/****************************************** + * These are the ScriptMotorAxis methods * + ******************************************/ + + +/** Creates a new ScriptMotorAxis object. + * \param[in] pC Pointer to the ScriptMotorController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +ScriptMotorAxis::ScriptMotorAxis(ScriptMotorController *pC, int axisNo, const char* script_file, const char* params) + : asynMotorAxis(pC, axisNo), + pC_(pC) +{ + this->initState(script_file); + this->config(params); + + int isnum; + + lua_getglobal(this->state, "MovingPollPeriod"); + double MovingPollPeriod = lua_tonumberx(this->state, -1, &isnum); + if (isnum) { pC->movingPollPeriod_ = MovingPollPeriod; } + lua_remove(this->state, -1); + + lua_getglobal(this->state, "IdlePollPeriod"); + double IdlePollPeriod = lua_tonumberx(this->state, -1, &isnum); + if (isnum) { pC->idlePollPeriod_ = IdlePollPeriod; } + lua_remove(this->state, -1); + + lua_getglobal(this->state, "ForcedFastPolls"); + double ForcedFastPolls = lua_tonumberx(this->state, -1, &isnum); + if (isnum) { pC->forcedFastPolls_ = ForcedFastPolls; } + lua_remove(this->state, -1); + + // Zero the encoder position (this only appears to be a problem on windows) + setDoubleParam(pC_->motorEncoderPosition_, 0.0); + + // Make the changed parameters take effect + callParamCallbacks(); +} + +void ScriptMotorAxis::initState(const char* script_file) +{ + this->state = luaL_newstate(); + int status = luaLoadScript(this->state, script_file); + + if (status) { printf("Error compiling script file: %s\n", script_file); } + + lua_pushstring(this->state, (const char*) this->pC_->portName); + lua_setglobal(this->state, "DRIVER"); + + lua_pushnumber(this->state, axisNo_); + lua_setglobal(this->state, "AXIS"); +} + +void ScriptMotorAxis::reload(const char* script_file, const char* controller_params) +{ + this->initState(script_file); + this->config(controller_params); + this->config(this->params.c_str()); +} + +epicsUInt32 ScriptMotorAxis::setStatusParam(int index, int value) +{ + if (index >= pC_->motorStatusDirection_ && index <= pC_->motorStatusHomed_) + { + epicsUInt32 status = status_.status; + int mask = 1 << (index - pC_->motorStatusDirection_); + + if (value) { status |= mask; } + else { status &= ~mask; } + + if (status != status_.status) + { + status_.status = status; + statusChanged_ = 1; + } + + return status; + } + + return 0; +} + +void ScriptMotorAxis::setPositionParam(int index, double value) +{ + if (index == pC_->motorPosition_) + { + if (value != status_.position) + { + statusChanged_ = 1; + status_.position = value; + } + } + else if (index == pC_->motorEncoderPosition_) + { + if (value != status_.encoderPosition) + { + statusChanged_ = 1; + status_.encoderPosition = value; + } + } +} + + +/* + * + */ +extern "C" int ScriptMotorCreateAxis(const char* ScriptMotorName, int axisNo, const char* params) +{ + static const char *functionName = "VirtualMotorCreateAxis"; + + ScriptMotorController *pC = (ScriptMotorController*) findAsynPortDriver(ScriptMotorName); + if (!pC) + { + printf("Error port %s not found\n", ScriptMotorName); + return asynError; + } + + pC->lock(); + pC->configAxis(axisNo, params); + pC->unlock(); + + return asynSuccess; +} + + +/** 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 ScriptMotorAxis::report(FILE *fp, int level) +{ + if (level > 0) { + fprintf(fp, " Axis #%d\n", axisNo_); + fprintf(fp, " axisIndex_=%d\n", axisIndex_); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + + +/* + * move() is called by asynMotor device support when an absolute or a relative move is requested. + * It can be called multiple times if BDST > 0 or RTRY > 0. + * + * Arguments in terms of motor record fields: + * position (steps) = RVAL = DVAL / MRES + * baseVelocity (steps/s) = VBAS / abs(MRES) + * velocity (step/s) = VELO / abs(MRES) + * acceleration (step/s/s) = (velocity - baseVelocity) / ACCL + */ +asynStatus ScriptMotorAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) +{ + asynStatus status; + // static const char *functionName = "ScriptMotorAxis::move"; + + int result = lua_getglobal(this->state, "move"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, position); + lua_pushboolean(this->state, relative); + lua_pushnumber(this->state, minVelocity); + lua_pushnumber(this->state, maxVelocity); + lua_pushnumber(this->state, acceleration); + + if (lua_pcall(this->state, 5, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + +/* + * home() is called by asynMotor device support when a home is requested. + * Note: forwards is set by device support, NOT by the motor record. + * + * Arguments in terms of motor record fields: + * minVelocity (steps/s) = VBAS / abs(MRES) + * maxVelocity (step/s) = HVEL / abs(MRES) + * acceleration (step/s/s) = (maxVelocity - minVelocity) / ACCL + * forwards = 1 if HOMF was pressed, 0 if HOMR was pressed + */ + +asynStatus ScriptMotorAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) +{ + int result = lua_getglobal(this->state, "home"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, minVelocity); + lua_pushnumber(this->state, maxVelocity); + lua_pushnumber(this->state, acceleration); + lua_pushboolean(this->state, forwards); + + if (lua_pcall(this->state, 4, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + + +/* + * moveVelocity() is called by asynMotor device support when a jog is requested. + * If a controller doesn't have a jog command (or jog commands), this a jog can be simulated here. + * + * Arguments in terms of motor record fields: + * minVelocity (steps/s) = VBAS / abs(MRES) + * maxVelocity (step/s) = (jog_direction == forward) ? JVEL * DIR / MRES : -1 * JVEL * DIR / MRES + * acceleration (step/s/s) = JAR / abs(EGU) + */ +asynStatus ScriptMotorAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration) +{ + int result = lua_getglobal(this->state, "moveVelocity"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, minVelocity); + lua_pushnumber(this->state, maxVelocity); + lua_pushnumber(this->state, acceleration); + + if (lua_pcall(this->state, 3, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + +/* + * stop() is called by asynMotor device support whenever a user presses the stop button. + * It is also called when the jog button is released. + * + * Arguments in terms of motor record fields: + * acceleration = ??? + */ +asynStatus ScriptMotorAxis::stop(double acceleration) +{ + int result = lua_getglobal(this->state, "stop"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, acceleration); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + +/* + * setPosition() is called by asynMotor device support when a position is redefined. + * It is also required for autosave to restore a position to the controller at iocInit. + * + * Arguments in terms of motor record fields: + * position (steps) = DVAL / MRES = RVAL + */ +asynStatus ScriptMotorAxis::setPosition(double position) +{ + int result = lua_getglobal(this->state, "setPosition"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, position); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setEncoderPosition(double position) +{ + int result = lua_getglobal(this->state, "setEncoderPosition"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, position); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setHighLimit(double highLimit) +{ + int result = lua_getglobal(this->state, "setHighLimit"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, highLimit); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setLowLimit(double lowLimit) +{ + int result = lua_getglobal(this->state, "setLowLimit"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, lowLimit); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + +asynStatus ScriptMotorAxis::setPGain(double PGain) +{ + int result = lua_getglobal(this->state, "setPGain"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, PGain); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setIGain(double IGain) +{ + int result = lua_getglobal(this->state, "setIGain"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, IGain); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setDGain(double DGain) +{ + int result = lua_getglobal(this->state, "setDGain"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, DGain); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +/* + * setClosedLoop() is called by asynMotor device support when a user enables or disables torque, + * usually from the motorx_all.adl, but only for drivers that set the following params to 1: + * pC->motorStatusGainSupport_ + * pC->motorStatusHasEncoder_ + * What is actually implemented here varies greatly based on the specfics of the controller. + * + * Arguments in terms of motor record fields: + * closedLoop = CNEN + */ + +asynStatus ScriptMotorAxis::setClosedLoop(bool closedLoop) +{ + int result = lua_getglobal(this->state, "setClosedLoop"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushboolean(this->state, (int) closedLoop); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + +asynStatus ScriptMotorAxis::setEncoderRatio(double EncoderRatio) +{ + int result = lua_getglobal(this->state, "setEncoderRatio"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + lua_pushnumber(this->state, EncoderRatio); + + if (lua_pcall(this->state, 1, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + // Do something with returned value + + lua_pop(this->state, 1); + } + + return asynSuccess; +} + + + +/** 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 ScriptMotorAxis::poll(bool *moving) +{ + int result = lua_getglobal(this->state, "poll"); + if (result != LUA_TFUNCTION) + { + // No function in script + } + else + { + if (lua_pcall(this->state, 0, 1, 0)) + { + this->logError(); + return asynError; + } + + int rettype = lua_type(this->state, -1); + + if (rettype == LUA_TBOOLEAN) + { + if (lua_toboolean(this->state, -1)) { *moving = true; } + else { *moving = false; } + } + + lua_pop(this->state, 1); + } + + this->callParamCallbacks(); + return asynSuccess; +} + +void ScriptMotorAxis::config(const char* params) +{ + luaLoadParams(this->state, params); +} + +void ScriptMotorAxis::logError() +{ + std::string err(lua_tostring(this->state, -1)); + lua_pop(this->state, 1); + + printf("%s\n", err.c_str()); +} + +void ScriptMotorReload(const char* port) +{ + ScriptMotorController* controller = (ScriptMotorController*) findAsynPortDriver(port); + + if (controller != NULL) { controller->reload(); } +} + + +/** Code for iocsh registration */ +static const iocshArg ScriptMotorReloadArg0 = {"Motor Port name", iocshArgString}; + +static const iocshArg* const ScriptMotorReloadArgs[] = {&ScriptMotorReloadArg0}; + +static const iocshFuncDef ScriptMotorReloadDef = {"ScriptMotorReload", 1, ScriptMotorReloadArgs}; + +static void ScriptMotorReloadCallFunc(const iocshArgBuf *args) +{ + ScriptMotorReload(args[0].sval); +} + + +static const iocshArg ScriptMotorCreateControllerArg0 = {"Motor Port name", iocshArgString}; +static const iocshArg ScriptMotorCreateControllerArg1 = {"Number of axes", iocshArgInt}; +static const iocshArg ScriptMotorCreateControllerArg2 = {"Control Script", iocshArgString}; +static const iocshArg ScriptMotorCreateControllerArg3 = {"Parameters", iocshArgString}; +static const iocshArg * const ScriptMotorCreateControllerArgs[] = {&ScriptMotorCreateControllerArg0, + &ScriptMotorCreateControllerArg1, + &ScriptMotorCreateControllerArg2, + &ScriptMotorCreateControllerArg3}; +static const iocshFuncDef ScriptMotorCreateControllerDef = {"ScriptControllerConfig", 4, ScriptMotorCreateControllerArgs}; +static void ScriptMotorCreateContollerCallFunc(const iocshArgBuf *args) +{ + ScriptMotorCreateController(args[0].sval, args[1].ival, args[2].sval, args[3].sval); +} + + +static const iocshArg ScriptMotorCreateAxisArg0 = {"Controller port name", iocshArgString}; +static const iocshArg ScriptMotorCreateAxisArg1 = {"Axis number", iocshArgInt}; +static const iocshArg ScriptMotorCreateAxisArg2 = {"Parameters", iocshArgString}; + +static const iocshArg * const ScriptMotorCreateAxisArgs[] = {&ScriptMotorCreateAxisArg0, + &ScriptMotorCreateAxisArg1, + &ScriptMotorCreateAxisArg2}; + +static const iocshFuncDef ScriptMotorCreateAxisDef = {"ScriptAxisConfig", 3, ScriptMotorCreateAxisArgs}; +static void ScriptMotorCreateAxisCallFunc(const iocshArgBuf *args) +{ + ScriptMotorCreateAxis(args[0].sval, args[1].ival, args[2].sval); +} + + + +static void ScriptMotorRegister(void) +{ + iocshRegister(&ScriptMotorReloadDef, ScriptMotorReloadCallFunc); + iocshRegister(&ScriptMotorCreateControllerDef, ScriptMotorCreateContollerCallFunc); + iocshRegister(&ScriptMotorCreateAxisDef, ScriptMotorCreateAxisCallFunc); +} + + +extern "C" { +epicsExportRegistrar(ScriptMotorRegister); +} diff --git a/motorApp/ScriptMotorSrc/ScriptMotorDriver.dbd b/motorApp/ScriptMotorSrc/ScriptMotorDriver.dbd new file mode 100644 index 00000000..8f5bbc23 --- /dev/null +++ b/motorApp/ScriptMotorSrc/ScriptMotorDriver.dbd @@ -0,0 +1 @@ +registrar(ScriptMotorRegister) diff --git a/motorApp/ScriptMotorSrc/ScriptMotorDriver.h b/motorApp/ScriptMotorSrc/ScriptMotorDriver.h new file mode 100644 index 00000000..efa010cd --- /dev/null +++ b/motorApp/ScriptMotorSrc/ScriptMotorDriver.h @@ -0,0 +1,76 @@ +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#include +#include "epicsScript.h" + +class epicsShareClass ScriptMotorAxis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + ScriptMotorAxis(class ScriptMotorController *pC, int axisNo, const char* script_file, const char* params); + + void reload(const char* script, const char* params); + + 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 setEncoderPosition(double position); + asynStatus setHighLimit(double highLimit); + asynStatus setLowLimit(double lowLimit); + asynStatus setPGain(double pGain); + asynStatus setIGain(double iGain); + asynStatus setDGain(double dGain); + asynStatus setClosedLoop(bool closedLoop); + asynStatus setEncoderRatio(double ratio); + + virtual epicsUInt32 setStatusParam(int index, int value); + virtual void setPositionParam(int index, double value); + +private: + ScriptMotorController *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + int axisIndex_; + + std::string params; + + lua_State* state; + + void initState(const char* script_file); + void config(const char* params); + void logError(); + +friend class ScriptMotorController; +}; + +class epicsShareClass ScriptMotorController : public asynMotorController { +public: + ScriptMotorController(const char *asyn_port, int max_axes, const char* script_file, const char* params); + + virtual asynStatus setIntegerParam(int list, int function, int value); + virtual asynStatus setDoubleParam(int list, int function, double value); + + virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); + + void report(FILE *fp, int level); + ScriptMotorAxis* getAxis(asynUser *pasynUser); + ScriptMotorAxis* getAxis(int axisNo); + + void configAxis(int axisNo, const char* params); + + void reload(); + +protected: + int ScriptMotorReload; + +private: + std::string script; + std::string init_params; + +friend class ScriptMotorAxis; +};