commit 100daf7036b91859eb4013668bff5abb38f05df7 Author: Michele Brambilla Date: Sat Mar 19 14:01:07 2022 +0100 Initial commit Motor driver for the xHuber SMC9300, courtesy of Dongliang Yang (noya269@126.com). This is an improved version of what Dongliang provided. In particular SMC9300Axis::poll - add a new parameter for the messge text that comes from the controller - parse pC_->inString_ using sscanf and check that the reply has all the expected parameters - make usre that the whole poll is executed. For some reason the original version did not update the position during the movement diff --git a/MODULE b/MODULE new file mode 100644 index 0000000..82fe110 --- /dev/null +++ b/MODULE @@ -0,0 +1,5 @@ +# Please change the following email with yours. +Email: michele.brambilla@psi.ch +Module-Name: motorHuber +Description: motorHuber - motor driver for the xHuber SMC9300 - courtesy of Dongliang Yang (noya269@126.com) +Project-Name: diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..1c81c93 --- /dev/null +++ b/Makefile @@ -0,0 +1,8 @@ +include /ioc/tools/driver.makefile + +MODULE=motorHuber +LIBVERSION=brambilla_m +BUILDCLASSES=Linux +EPICS_VERSIONS=3.14.12 7.0.4.1 + +DBDS += devHuberMotor.dbd diff --git a/SMC9300Driver.cpp b/SMC9300Driver.cpp new file mode 100644 index 0000000..c30a6f8 --- /dev/null +++ b/SMC9300Driver.cpp @@ -0,0 +1,417 @@ +/* +FILENAME... SMC9300Driver.cpp +USAGE... Motor driver support for the HUBER SMC9300 controller. + +Yang Dongliang +March 1, 2020 + +*/ + +#include "SMC9300Driver.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#define STEPS_PER_EGU 10000. + +#define NINT(f) (int)((f) > 0 ? (f) + 0.5 : (f)-0.5) + + +/** Creates a new SMC9300Controller object. + * \param[in] portName The name of the asyn port that will be created + * for this driver \param[in] SMC9300PortName The name of the + * drvAsynSerialPort that was created previously to connect to the SMC9300 + * controller \param[in] numAxes The number of axes that this + * controller supports \param[in] movingPollPeriod The time between polls when + * any axis is moving \param[in] idlePollPeriod The time between polls when + * no axis is moving + */ +SMC9300Controller::SMC9300Controller(const char *portName, + const char *SMC9300PortName, int numAxes, + double movingPollPeriod, + double idlePollPeriod, + const int &extraParams = 1) + : asynMotorController( + portName, numAxes, NUM_SMC9300_PARAMS+extraParams, + 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, 0) // Default priority and stack size +{ + int axis; + asynStatus status; + SMC9300Axis *pAxis; + static const char *functionName = "SMC9300Controller::SMC9300Controller"; + + /* Connect to SMC9300 controller */ + status = pasynOctetSyncIO->connect(SMC9300PortName, 0, &pasynUserController_, + NULL); + if (status) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: cannot connect to Huber_SMC9300 controller\n", functionName); + } + for (axis = 1; axis < numAxes; axis++) { + pAxis = new SMC9300Axis(this, axis); + } + + startPoller(movingPollPeriod, idlePollPeriod, 2); + createParam(motorMessageTextString, asynParamOctet, &motorMessageText_); +} + +/** Creates a new SMC9300Controller 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] SMC9300PortName The name of the + * drvAsynIPPPort that was created previously to connect to the SMC9300 + * controller \param[in] numAxes The number of axes that this + * controller supports \param[in] movingPollPeriod The time in ms between polls + * when any axis is moving \param[in] idlePollPeriod The time in ms between + * polls when no axis is moving + */ +extern "C" int SMC9300CreateController(const char *portName, + const char *SMC9300PortName, int numAxes, + int movingPollPeriod, + int idlePollPeriod) { + SMC9300Controller *pSMC9300Controller = + new SMC9300Controller(portName, SMC9300PortName, numAxes, + movingPollPeriod / 1000., idlePollPeriod / 1000.); + pSMC9300Controller = NULL; + 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 SMC9300Controller::report(FILE *fp, int level) { + fprintf(fp, + "Huber_SMC9300 motor driver %s, numAxes=%d, moving poll period=%f, " + "idle poll period=%f\n", + this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); + + // Call the base class method + asynMotorController::report(fp, level); +} + +/** Returns a pointer to an SMC9300Axis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. + */ +SMC9300Axis *SMC9300Controller::getAxis(asynUser *pasynUser) { + return static_cast(asynMotorController::getAxis(pasynUser)); +} + +/** Returns a pointer to an SMC9300Axis object. + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +SMC9300Axis *SMC9300Controller::getAxis(int axisNo) { + return static_cast(asynMotorController::getAxis(axisNo)); +} + +// These are the SMC9300Axis methods + +/** Creates a new SMC9300Axis object. + * \param[in] pC Pointer to the SMC9300Controller to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ +SMC9300Axis::SMC9300Axis(SMC9300Controller *pC, int axisNo) + : asynMotorAxis(pC, axisNo), pC_(pC) { + static const char *functionName = "SMC9300Axis::SMC9300Axis"; + + // Do an initial poll to get some values from the axis + if (getAxisInitialStatus() != asynSuccess) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "%s: getAxisInitialStatus failed to return asynSuccess. " + "Controller: %s, Axis: %d.\n", + functionName, pC_->portName, 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 SMC9300Axis::report(FILE *fp, int level) { + if (level > 0) { + fprintf(fp, " axis %d\n", axisNo_); + } + + // Call the base class method + asynMotorAxis::report(fp, level); +} + +// asynStatus SMC9300Axis::sendAccelAndVelocity(double acceleration, double +// velocity) +// { +// asynStatus status; +// int ival; +// // static const char *functionName = "SMC9300::sendAccelAndVelocity"; + +// // Send the velocity +// // ival = NINT(fabs(115200./velocity)); +// // if (ival < 2) ival=5000; +// // if (ival > 2) ival=5000; +// ival = 5000; +// if (axisNo_ == 1) ival=5000; +// if (axisNo_ == 2) ival=30000; +// if (axisNo_ == 3) ival=30000; +// if (axisNo_ == 4) ival=30000; +// sprintf(pC_->outString_, "ffast%d:%d", axisNo_, ival); +// status = pC_->writeController(); + +// // Send the acceleration +// // acceleration is in steps/sec/sec +// // SMC is programmed with Ramp Index (R) where: +// // dval (steps/sec/sec) = 720,000/(256-R) */ +// // or R=256-(720,000/dval) */ +// // ival = NINT(256-(720000./acceleration)); +// // if (ival < 1) ival=20; +// // if (ival > 1) ival=20; +// ival = 20; +// if (axisNo_ == 1) ival=20; +// if (axisNo_ == 2) ival=80; +// if (axisNo_ == 3) ival=80; +// if (axisNo_ == 4) ival=80; +// sprintf(pC_->outString_, "acc%d:%d", axisNo_, ival); +// status = pC_->writeController(); +// return status; +// } + +asynStatus SMC9300Axis::move(double position, int relative, double minVelocity, + double maxVelocity, double acceleration) { + asynStatus status; + // static const char *functionName = "SMC9300Axis::move"; + + // status = sendAccelAndVelocity(acceleration, maxVelocity); + + if (relative) { + sprintf(pC_->outString_, "move%d:%f", axisNo_, position / STEPS_PER_EGU); + } else { + sprintf(pC_->outString_, "goto%d:%f", axisNo_, position / STEPS_PER_EGU); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMC9300Axis::home(double minVelocity, double maxVelocity, + double acceleration, int forwards) { + asynStatus status; + // static const char *functionName = "SMC9300Axis::home"; + + // status = sendAccelAndVelocity(acceleration, maxVelocity); + + if (forwards) { + sprintf(pC_->outString_, "home%d:hs", axisNo_); + } else { + sprintf(pC_->outString_, "home%d:hs", axisNo_); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMC9300Axis::moveVelocity(double minVelocity, double maxVelocity, + double acceleration) { + asynStatus status; + static const char *functionName = "SMC9300Axis::moveVelocity"; + + asynPrint(pasynUser_, ASYN_TRACE_FLOW, + "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n", + functionName, minVelocity, maxVelocity, acceleration); + + // status = sendAccelAndVelocity(acceleration, maxVelocity); + + /* SMC-9300 does not have jog command. Move 1 million steps */ + if (maxVelocity > 0.) { + /* This is a positive move in SMC9300 coordinates */ + sprintf(pC_->outString_, "move%d:0.1", axisNo_); + } else { + /* This is a negative move in SMC9300 coordinates */ + sprintf(pC_->outString_, "move%d:-0.1", axisNo_); + } + status = pC_->writeController(); + return status; +} + +asynStatus SMC9300Axis::stop(double acceleration) { + asynStatus status; + // static const char *functionName = "SMC9300Axis::stop"; + + sprintf(pC_->outString_, "q%d", axisNo_); + status = pC_->writeController(); + return status; +} + +asynStatus SMC9300Axis::setPosition(double position) { + asynStatus status; + // static const char *functionName = "SMC9300Axis::setPosition"; + + sprintf(pC_->outString_, "pos%d:%f", axisNo_, position / STEPS_PER_EGU); + status = pC_->writeController(); + return status; +} + +asynStatus SMC9300Axis::setClosedLoop(bool closedLoop) { + asynStatus status; + // static const char *functionName = "SMC9300Axis::setClosedLoop"; + + sprintf(pC_->outString_, "ecl%d:%d", axisNo_, closedLoop ? 0 : 1); + status = pC_->writeController(); + return status; +} + +/** 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 SMC9300Axis::poll(bool *moving) { + int done; + int driveOn; + int limit; + int axisNo; + int nvals; + int value; + double position; + asynStatus comStatus; + static const char *functionName = "SMC9300Controller::poll"; + char errString[1024]; + + updateMsgTxtFromDriver(""); + + // Read errors from controller + sprintf(pC_->outString_, "?err%d", axisNo_); + comStatus = pC_->writeReadController(); + nvals = sscanf(pC_->inString_, "%d:%d %[^NULL]", &axisNo, &value, errString); + if (comStatus || nvals != 3 || nvals != 2 ) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvSMC9300AxisGetStatus: Failed to read error message\nStatus: %d" + "\nCommand :%s\nResponse:%s\n", + comStatus, pC_->outString_, pC_->inString_); + } + if (axisNo == axisNo_) { + updateMsgTxtFromDriver(errString); + } + + // Read the current motor position + sprintf(pC_->outString_, "?p%d", axisNo_); + comStatus = pC_->writeReadController(); + nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); + if (comStatus || nvals != 2) { + asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvSMC9300AxisGetStatus: Failed to read position\nStatus: %d" + "\nCommand :%s\nResponse:%s\n", + comStatus, pC_->outString_, pC_->inString_); + // updateMsgTxtFromDriver("Cannot read Axis position"); + } + if (axisNo == axisNo_) { + setDoubleParam(pC_->motorPosition_, position * STEPS_PER_EGU); + } + + // Read the current motor encoder position + sprintf(pC_->outString_, "?e%d", axisNo_); + comStatus = pC_->writeReadController(); + nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); + if (comStatus || nvals != 2) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvSMC9300AxisGetStatus: Failed to read encoder position\nStatus: %d" + "\nCommand :%s\nResponse:%s\n", + comStatus, pC_->outString_, pC_->inString_); + // updateMsgTxtFromDriver("Cannot read Axis encoder position"); + } + if (axisNo == axisNo_) { + setDoubleParam(pC_->motorEncoderPosition_, position * STEPS_PER_EGU); + } + + // Read the moving status of this motor + sprintf(pC_->outString_, "?s%d", axisNo_); + comStatus = pC_->writeReadController(); + nvals = sscanf(pC_->inString_, "%d:%d", &axisNo, &value); + if (comStatus || nvals != 2) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvSMC9300AxisGetStatus: Failed to read axis moving status\nStatus: %d" + "\nCommand :%s\nResponse:%s\n", + comStatus, pC_->outString_, pC_->inString_); + // updateMsgTxtFromDriver("Cannot read Axis position"); + } + if (axisNo == axisNo_) { + done = value & 0x1; + setIntegerParam(pC_->motorStatusDone_, done); + *moving = done ? false : true; + + setIntegerParam(pC_->motorStatusHighLimit_, value >> 2 & 0x1); + setIntegerParam(pC_->motorStatusLowLimit_, value >> 3 & 0x1); + setIntegerParam(pC_->motorStatusAtHome_, 0); + setIntegerParam(pC_->motorStatusPowerOn_, value >> 7 & 0x1); + setIntegerParam(pC_->motorStatusProblem_, 0); + // setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0); + } + + comStatus = callParamCallbacks(); + return comStatus ? asynError : asynSuccess; +} + +asynStatus SMC9300Axis::updateMsgTxtFromDriver(const char *value) +{ + if (value && value[0]) { + return setStringParam(pC_->motorMessageText_,value); + } + return setStringParam(pC_->motorMessageText_,""); +} + + +asynStatus SMC9300Axis::getAxisInitialStatus(void) { return asynSuccess; } + + + + + + +/** Code for iocsh registration */ +static const iocshArg SMC9300CreateControllerArg0 = {"Port name", + iocshArgString}; +static const iocshArg SMC9300CreateControllerArg1 = {"SMC9300 port name", + iocshArgString}; +static const iocshArg SMC9300CreateControllerArg2 = {"Number of axes", + iocshArgInt}; +static const iocshArg SMC9300CreateControllerArg3 = {"Moving poll period (ms)", + iocshArgInt}; +static const iocshArg SMC9300CreateControllerArg4 = {"Idle poll period (ms)", + iocshArgInt}; +static const iocshArg *const SMC9300CreateControllerArgs[] = { + &SMC9300CreateControllerArg0, &SMC9300CreateControllerArg1, + &SMC9300CreateControllerArg2, &SMC9300CreateControllerArg3, + &SMC9300CreateControllerArg4}; +static const iocshFuncDef SMC9300CreateControllerDef = { + "SMC9300CreateController", 5, SMC9300CreateControllerArgs}; +static void SMC9300CreateContollerCallFunc(const iocshArgBuf *args) { + SMC9300CreateController(args[0].sval, args[1].sval, args[2].ival, + args[3].ival, args[4].ival); +} + +static void SMC9300Register(void) { + iocshRegister(&SMC9300CreateControllerDef, SMC9300CreateContollerCallFunc); +} + +extern "C" { +epicsExportRegistrar(SMC9300Register); +} diff --git a/SMC9300Driver.h b/SMC9300Driver.h new file mode 100644 index 0000000..acd225e --- /dev/null +++ b/SMC9300Driver.h @@ -0,0 +1,57 @@ +/* +FILENAME... SMC9300Driver.h +USAGE... Motor driver support for the HUBER SMC9300 controller. + +Yang Dongliang +March 1, 2020 + +*/ + +#include "asynMotorController.h" +#include "asynMotorAxis.h" + +#define MAX_SMC9300_AXES 4 + +// No controller-specific parameters yet +#define NUM_SMC9300_PARAMS 0 + +#define motorMessageTextString "MOTOR_MESSAGE_TEXT" + +class epicsShareClass SMC9300Axis : public asynMotorAxis +{ +public: + /* These are the methods we override from the base class */ + SMC9300Axis(class SMC9300Controller *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); + asynStatus getAxisInitialStatus(void); + + +protected: + SMC9300Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ + // asynStatus sendAccelAndVelocity(double accel, double velocity); + asynStatus updateMsgTxtFromDriver(const char *value); + +friend class SMC9300Controller; +}; + +class epicsShareClass SMC9300Controller : public asynMotorController { +public: + SMC9300Controller(const char *portName, const char *SMC9300PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, const int &extraParams ); + + void report(FILE *fp, int level); + SMC9300Axis* getAxis(asynUser *pasynUser); + SMC9300Axis* getAxis(int axisNo); + +protected: + int motorMessageText_; + +friend class SMC9300Axis; +}; diff --git a/devHuberMotor.dbd b/devHuberMotor.dbd new file mode 100644 index 0000000..66e7a0d --- /dev/null +++ b/devHuberMotor.dbd @@ -0,0 +1,8 @@ +# Advanced Control Systems driver support. + +# Model 3 driver +registrar(SMC9300Register) + + + +