forked from epics_driver_modules/motorBase
Revert "Merge branch 'master' into master"
This reverts commit3ec6ec433c, reversing changes made to8966aeabf2.
This commit is contained in:
kpetersn
@@ -1,859 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
|
||||
#include <iocsh.h>
|
||||
#include <epicsThread.h>
|
||||
|
||||
#include <asynOctetSyncIO.h>
|
||||
|
||||
#include "asynMotorController.h"
|
||||
#include "asynMotorAxis.h"
|
||||
|
||||
#include <epicsExport.h>
|
||||
#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 ScriptControllerConfig(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<ScriptMotorAxis*>(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<ScriptMotorAxis*>(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 ScriptAxisConfig(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)
|
||||
{
|
||||
luaLoadMacros(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)
|
||||
{
|
||||
ScriptControllerConfig(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)
|
||||
{
|
||||
ScriptAxisConfig(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);
|
||||
}
|
||||
Reference in New Issue
Block a user