From 451cd433c2d0305103cbaa5af4afa50a89cf589a Mon Sep 17 00:00:00 2001 From: MarkRivers Date: Wed, 12 Sep 2012 17:19:03 +0000 Subject: [PATCH] New file from Nia Fong at SLAC --- motorApp/SmarActMCSSrc/ChangeLog | 2 + motorApp/SmarActMCSSrc/Makefile | 29 + motorApp/SmarActMCSSrc/README | 260 +++++++ motorApp/SmarActMCSSrc/devSmarActMCSMotor.dbd | 2 + .../SmarActMCSSrc/smarActMCSMotorDriver.cpp | 647 ++++++++++++++++++ .../SmarActMCSSrc/smarActMCSMotorDriver.h | 93 +++ 6 files changed, 1033 insertions(+) create mode 100644 motorApp/SmarActMCSSrc/ChangeLog create mode 100644 motorApp/SmarActMCSSrc/Makefile create mode 100644 motorApp/SmarActMCSSrc/README create mode 100644 motorApp/SmarActMCSSrc/devSmarActMCSMotor.dbd create mode 100644 motorApp/SmarActMCSSrc/smarActMCSMotorDriver.cpp create mode 100644 motorApp/SmarActMCSSrc/smarActMCSMotorDriver.h diff --git a/motorApp/SmarActMCSSrc/ChangeLog b/motorApp/SmarActMCSSrc/ChangeLog new file mode 100644 index 00000000..139597f9 --- /dev/null +++ b/motorApp/SmarActMCSSrc/ChangeLog @@ -0,0 +1,2 @@ + + diff --git a/motorApp/SmarActMCSSrc/Makefile b/motorApp/SmarActMCSSrc/Makefile new file mode 100644 index 00000000..58b94c01 --- /dev/null +++ b/motorApp/SmarActMCSSrc/Makefile @@ -0,0 +1,29 @@ +# Makefile +TOP=../.. + +include $(TOP)/configure/CONFIG +#---------------------------------------- +# ADD MACRO DEFINITIONS AFTER THIS LINE +#============================= + +#================================================== +# Build an IOC support library +LIBRARY_IOC = smarActMCSMotor + +# motorRecord.h will be created from motorRecord.dbd +# install devMotorSoft.dbd into /dbd +DBD += devSmarActMCSMotor.dbd + +INC += smarActMCSMotorDriver.h + +# The following are compiled and added to the Support library +smarActMCSMotor_SRCS += smarActMCSMotorDriver.cpp + +smarActMCSMotor_LIBS += motor +smarActMCSMotor_LIBS += asyn +smarActMCSMotor_LIBS += $(EPICS_BASE_IOC_LIBS) + +include $(TOP)/configure/RULES +#---------------------------------------- +# ADD RULES AFTER THIS LINE + diff --git a/motorApp/SmarActMCSSrc/README b/motorApp/SmarActMCSSrc/README new file mode 100644 index 00000000..49175510 --- /dev/null +++ b/motorApp/SmarActMCSSrc/README @@ -0,0 +1,260 @@ +Driver for smarAct MCS Positioner +================================== + +Till Straumann , 9/2011 + +Introduction +------------ + +This driver supports the smarAct MCS Positioner +by implementing subclasses of the 'asynMotorController' +and 'asynMotorAxis' objects. + +Communication with the hardware is established via +a 'asyn port' driver layer and it is thus possible +to use either a physical RS232 port or a terminal +server in a transparent fashion. + +Note: the reader is assumed to be familiar with the +motor record. Please consult relevant documentation. + +Restrictions +------------ + +The driver does not support all features of the MCS +firmware (some of which are outside of the scope of +the motor record). Neither are all features of the +motor record supported. + +The driver currently relies on the presence of a +linear position sensor and uses a command set specific +to such sensors. Rotary sensors and the associated +command set are not supported. Closed-loop mode is +used exclusively for all motion commands. + +Limit switches are not supported (the MCS command +set defines no interface to hardware limit switches). + +The driver operates directly in user coordinates (nm) +no translation into 'steps' is supported. + +The 'JOG' feature of the motor record is implemented +using a relative-move command to a 'far-away' target. +This might or might not work if a different position +sensor (working at a different scale) is employed. + +Acceleration-control is not supported. + +The drivers makes the following assumptions about +the setup + - reset command ('R') is never used by third party + while the driver is running. + - 'synchronous' communication mode ('SCM') is in + effect. + - 'keep-alive' mode is not in effect (but if the + keep-alive time is longer than the driver's + slowest polling period then 'keep-alive' mode + should work). + - sensor is never disabled ('SSE'). Permanent 'on' + or power-save mode are OK. + - sensor type ('SST') is configured correctly (must + be performed once by third-party when a new unit + is commissioned). + - sensor is properly calibrated ('CS') but this + must not be performed by third party while this + driver is running. + - baud rate is configured correctly at both ends + ('CB') and terminal-server or UART port setup. + +Otherwise, a secondary driver using an orthogonal +command set may run concurrently with this driver +as long as it properly shares the asyn port driver +in charge of serial communication. + +Special Features +---------------- + +Position Reference +.................. +After power-up the position sensor must be positioned +to a reference mark in order for it to provide +absolute positions. The 'MOTOR_HOMED' bit in the +'MSTA' field reflects the status of the positioner. +Only if 'MOTOR_HOMED' is set the absolute position +is correct. Note that this bit is read from the +hardware and is hence preserved across IOC reboots +(as long as power from the MCS is not removed). + +Position Holding +................ +All motion commands are executed in 'closed-loop' +mode of the MCS controller, i.e., the controller +hardware itself drives the positioner to the target +in 'closed-loop' mode. +The MCS can also be instructed to actively hold the +target position (compensating small movements, drift +etc.). This feature can be programmed using the +motor record's CNEN field (enabled: MCS holds +target position indefinitely; disabled: MCS goes +to open-loop mode as soon as the target is +reached). + +Usage Information +----------------- +The position sensor has a very high (1nm) +resolution and it is unlikely (especially +in 'no-hold' mode, i.e., if CNEN is 'disabled') +that the target position can be reached +exactly. In order to avoid many retries +the dead-band (RDBD) and/or other parameters +of the motor record may need appropriate +tuning. + +It should also be noted that the positioner +is quite fast and performes moves almost +'instantaneously' (as perceived by an operator) +unless the speed is reduced (VELO). +'JOG' mode is probably useless unless the jog +velocity (JVEL) is set to a relatively low +value which allows the operator to observe +the motion. However, performing a JOG operation +with JVEL at zero will be rejected (since zero +would choose the default speed which is too +high to be useful). The driver immediately +reports the motion as 'DONE' (in MSTA). +It is the user's responsibility to clear the +JOGF/JOGR field if this happens. + +Building an Application +- - - - - - - - - - - - +Building the driver into an application requires + +a) the 'asyn', 'motor' and 'smarActMCSMotor' (this + driver) packages/modules to be built and + installed. The application's RELEASE file must + point to these packages so that the build process + locates headers and libraries etc. + +b) the application's '.dbd' file must contain + - motorSupport.dbd + - devSmarActMCSMotor.dbd + as well as an 'asyn' port driver for serial + communication. In case of a connection via + TCP/IP + terminal server this would be + - drvAsynIPPort.dbd + + These '.dbd' files are best listed in the + application Makefile: + _DBD += motorSupport.dbd + _DBD += devSmarActMCSMotor.dbd + _DBD += drvAsynIPPort.dbd + +c) the application must be linked against + the 'smarActMCSMotor', 'motor' and 'asyn' + libraries, e.g., + + _LIBS += smarActMCSMotor motor asyn + +Driver Run-Time Configuration +- - - - - - - - - - - - - - - +For each MCS controller a driver instance needs +to be configured either from a startup script, +C or C++ code (the C++ constructor takes the +same arguments). +However, at first an asyn port driver for +serial communication must be created (e.g. +from the startup script). E.g., when using +a terminal server then drvAsynIPPort may be +used: + + drvAsynIPPortConfigure("myTS1","terminalserver:port",0,0,0) + +Next, an MCS controller driver instance is +created. The respective call takes the following +arguments: + + +smarActMCSCreateController( + const char *motorPortName, + const char *ioPortName, + int numAxes, + double movingPollPeriod, + double idlePollPeriod); + + motorPortName: unique string to identify this + instance to be used in the + motor record's 'OUT' field. + ioPortName: asyn port name identifying the + serial link for communication + with the MCS. + Note that such a link must + be created prior to creating + a MCS controller driver instance. + numAxes: number of axes this MCS supports. + movingPollPeriod: period (in seconds - since + this is a 'double' parameter + fractional seconds are possible) + at which the MCS is polled for + status changes while the + positioner is moving. + idlePollPeriod: period (in seconds) at which + the MCS is polled for status + changes while the positioner + is stopped. + +E.g., to configure a driver with one axis using +the serial connection 'myTS1' configured in the +prior example we use + +smarActMCSCreateController("myMotor1","myTS1",1,0.02,1.0) + +This results in a polling interval of 20ms +while moving and 1s while idle. The driver +instance is named 'myMotor1' and can be +addressed from a motorRecord's OUT field: + + field(DTYP, "asynMotor") + field(OUT, "asyn(myMotor1,0)") + +After creating a controller, one or more MCS axes +are created for that controller. The respective +call takes the following arguments: + + +smarActMCSCreateAxis( + const char *motorPortName, + int axisNumber, + int channel) +{ + + motorPortName: unique string to identify this + instance to be used in the + motor record's 'OUT' field. + axisNumber: axis number being created. + channel: channel that the axis is physically + connected to. + Note that the channel will be + prepended to the command string + sent to the particular axis. + +Call the smarActMCSCreateAxis() function for +each axis that needs to be configured. + +Terminal Server / RS232 Port Notes +- - - - - - - - - - - - - - - - - - +It is the user's responsibility to configure +the serial port so it matches the MCS' setup. + +Note that a terminal server might need +additional configuration to make sure that +a 'raw' TCP connection is used. Otherwise, +the terminal server might use other protocols +(most notably: the TELNET protocol) on top +of TCP which result in extra characters being +inserted in the data stream which are not +or mis-interpreted by this driver which +expects a 'raw' communication channel. +On some terminal servers a raw TCP socket +may listen on a different port number than +used for a TELNET connection. + diff --git a/motorApp/SmarActMCSSrc/devSmarActMCSMotor.dbd b/motorApp/SmarActMCSSrc/devSmarActMCSMotor.dbd new file mode 100644 index 00000000..4b28dd52 --- /dev/null +++ b/motorApp/SmarActMCSSrc/devSmarActMCSMotor.dbd @@ -0,0 +1,2 @@ +registrar(smarActMCSMotorRegister) +#driver(motorMCS) diff --git a/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.cpp b/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.cpp new file mode 100644 index 00000000..d1c72331 --- /dev/null +++ b/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.cpp @@ -0,0 +1,647 @@ + +/* Motor driver support for smarAct MCS RS-232 Controller */ + +/* Derived from ACRMotorDriver.cpp by Mark Rivers, 2011/3/28 */ + +/* Author: Till Straumann , 9/11 */ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +/* Static configuration parameters (compile-time constants) */ +#undef DEBUG + +#define CMD_LEN 50 +#define REP_LEN 50 +#define DEFLT_TIMEOUT 2.0 + +#define HOLD_FOREVER 60000 +#define HOLD_NEVER 0 +#define FAR_AWAY 1000000000 /*nm*/ + +/* The asyn motor driver apparently can't cope with exceptions */ +#undef ASYN_CANDO_EXCEPTIONS +/* Define this if exceptions should be thrown and it is OK to abort the application */ +#undef DO_THROW_EXCEPTIONS + +#if defined(ASYN_CANDO_EXCEPTIONS) || defined(DO_THROW_EXCEPTIONS) +#define THROW_(e) throw e +#else +#define THROW_(e) epicsPrintf("%s\n",e.what()); +#endif + +enum SmarActMCSStatus { + Stopped = 0, + Stepping = 1, + Scanning = 2, + Holding = 3, + Targeting = 4, + MoveDelay = 5, + Calibrating = 6, + FindRefMark = 7, + Locked = 9 +}; + +SmarActMCSException::SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, ...) + : t_(t) +{ +va_list ap; + if ( fmt ) { + va_start(ap, fmt); + vsnprintf(str_, sizeof(str_), fmt, ap); + va_end(ap); + } else { + str_[0] = 0; + } +}; + +SmarActMCSException::SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, va_list ap) + : t_(t) +{ + vsnprintf(str_, sizeof(str_), fmt, ap); +} + +/* Parse reply from MCS and return the value converted to a number. + * + * If the string cannot be parsed, i.e., is not in the format + * ':' , , , ',' , + * + * then the routine returns '-1'. + * + * Otherwise, if starts with 'E' + * (which means an 'Error' code) then the (always non-negative) + * error code is returned (may be zero in case of an 'acknowledgement' + * message in synchronous mode). + * + * If the string is parsed successfully then is passed up + * in *val_p. + * + * Hence - return code nonzero -> error (error code if > 0, parse error + * otherwise) + * - return code zero -> successful ACK or command reply; value + * in *val_p. + */ +int +SmarActMCSController::parseReply(const char *reply, int *ax_p, int *val_p) +{ +char cmd[10]; + if ( 3 != sscanf(reply, ":%10[A-Z]%i,%i", cmd, ax_p, val_p) ) + return -1; + return 'E' == cmd[0] ? *val_p : 0; +} + +SmarActMCSController::SmarActMCSController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod) + : asynMotorController(portName, numAxes, + 0, // parameters + 0, // interface mask + 0, // interrupt mask + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0,0) // default priority and stack size + , asynUserMot_p_(0) +{ +asynStatus status; +char junk[100]; +size_t got_junk; +int eomReason; +pAxes_ = (SmarActMCSAxis **)(asynMotorController::pAxes_); + + status = pasynOctetSyncIO->connect(IOPortName, 0, &asynUserMot_p_, NULL); + if ( status ) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "SmarActMCSController:SmarActMCSController: cannot connect to MCS controller\n"); + THROW_(SmarActMCSException(MCSConnectionError, "SmarActMCSController: unable to connect serial channel")); + } + + // slurp away any initial telnet negotiation; there is no guarantee that + // the other end will not send some telnet chars in the future. The terminal + // server should really be configured to 'raw' mode! + pasynOctetSyncIO->read(asynUserMot_p_, junk, sizeof(junk), 2.0, &got_junk, &eomReason); + if ( got_junk ) { + epicsPrintf("SmarActMCSController(%s): WARNING - detected unexpected characters on link (%s); make sure you have a RAW (not TELNET) connection\n", portName, IOPortName); + } + + pasynOctetSyncIO->setInputEos ( asynUserMot_p_, "\n", 1 ); + pasynOctetSyncIO->setOutputEos( asynUserMot_p_, "\n", 1 ); + + // Create axes +/* for ( ax=0; axwriteRead( asynUserMot_p_, buf, strlen(buf), rep, len, timeout, &nwrite, got_p, &eomReason); + + //asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "sendCmd()=%s", buf); + + return status; +} + +asynStatus +SmarActMCSController::sendCmd(size_t *got_p, char *rep, int len, double timeout, const char *fmt, ...) +{ +va_list ap; +asynStatus status; + va_start(ap, fmt); + status = sendCmd(got_p, rep, len, timeout, fmt, ap); + va_end(ap); + return status; +} + + +asynStatus SmarActMCSController::sendCmd(size_t *got_p, char *rep, int len, const char *fmt, ...) +{ +va_list ap; +asynStatus status; + va_start(ap, fmt); + status = sendCmd(got_p, rep, len, DEFLT_TIMEOUT, fmt, ap); + va_end(ap); + return status; +} + +asynStatus SmarActMCSController::sendCmd(char *rep, int len, const char *fmt, ...) +{ +va_list ap; +asynStatus status; +size_t got; + va_start(ap, fmt); + status = sendCmd(&got, rep, len, DEFLT_TIMEOUT, fmt, ap); + va_end(ap); + return status; +} + +/* Obtain value of the 'motorSetClosedLoop_' parameter (which + * maps to the record's CNEN field) + */ +int +SmarActMCSAxis::getClosedLoop() +{ +int val; + c_p_->getIntegerParam(axisNo_, c_p_->motorSetClosedLoop_, &val); + return val; +} + +SmarActMCSAxis::SmarActMCSAxis(class SmarActMCSController *cnt_p, int axis, int channel) + : asynMotorAxis(cnt_p, axis), c_p_(cnt_p) +{ + int val; + channel_ = channel; + + asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "SmarActMCSAxis::SmarActMCSAxis -- creating axis %u\n", axis); + + comStatus_ = getVal("GCLS",&vel_); +#ifdef DEBUG + printf("GCLS %u returned %i\n", axis, comStatus_); +#endif + if ( comStatus_ ) + goto bail; + if ( (comStatus_ = getVal("GS", &val)) ) + goto bail; + + if ( Holding == val ) { + // still holding? This means that - in a previous life - the + // axis was configured for 'infinite holding'. Inherit this + // (until the next 'move' command that is). + /// + holdTime_ = HOLD_FOREVER; + } else { + // initial value from 'closed-loop' property + holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + } + + if ( asynSuccess == getVal("GP",&val) ) { + setIntegerParam(c_p_->motorStatusHasEncoder_, 1); + setIntegerParam(c_p_->motorStatusGainSupport_, 1); + } + +bail: + setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0 ); + setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0 ); + + callParamCallbacks(); + + if ( comStatus_ ) { + THROW_(SmarActMCSException(MCSCommunicationError, "SmarActMCSAxis::SmarActMCSAxis -- channel %u ASYN error %i", axis, comStatus_)); + } + +} + +/* Read a parameter from the MCS (nothing to do with asyn's parameter + * library). + * + * parm_cmd: MCS command (w/o ':' char) to read parameter + * val_p: where to store the value returned by the MCS + * + * RETURNS: asynError if an error occurred, asynSuccess otherwise. + */ +asynStatus +SmarActMCSAxis::getVal(const char *parm_cmd, int *val_p) +{ +char rep[REP_LEN]; +asynStatus st; +int ax; + + //asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "getVal() cmd=:%s%u", parm_cmd, this->channel_); + + //st = c_p_->sendCmd(rep, sizeof(rep), ":%s%u", parm_cmd, this->axisNo_); + st = c_p_->sendCmd(rep, sizeof(rep), ":%s%u", parm_cmd, this->channel_); + if ( st ) + return st; + return c_p_->parseReply(rep, &ax, val_p) ? asynError: asynSuccess; +} + +asynStatus +SmarActMCSAxis::poll(bool *moving_p) +{ +int val; +enum SmarActMCSStatus status; + + if ( (comStatus_ = getVal("GP", &val)) ) + goto bail; + + setDoubleParam(c_p_->motorEncoderPosition_, (double)val); + setDoubleParam(c_p_->motorPosition_, (double)val); +#ifdef DEBUG + printf("POLL (position %d)", val); +#endif + + if ( (comStatus_ = getVal("GS", &val)) ) + goto bail; + + status = (enum SmarActMCSStatus)val; + + switch ( status ) { + default: + *moving_p = false; + break; + + /* If we use 'infinite' holding (until the next 'move' command) + * then the 'Holding' state must be considered 'not moving'. However, + * if we use a 'finite' holding time then we probably should consider + * the 'move' command incomplete until the holding time expires. + */ + case Holding: + *moving_p = HOLD_FOREVER == holdTime_ ? false : true; + break; + + case Stepping: + case Scanning: + case Targeting: + case MoveDelay: + case Calibrating: + case FindRefMark: + *moving_p = true; + break; + } + + setIntegerParam(c_p_->motorStatusDone_, ! *moving_p ); + + + /* Check if the sensor 'knows' absolute position and + * update the MSTA 'HOMED' bit. + */ + if ( (comStatus_ = getVal("GPPK", &val)) ) + goto bail; + + setIntegerParam(c_p_->motorStatusHomed_, val ? 1 : 0 ); + +#ifdef DEBUG + printf(" status %u", status); +#endif + +bail: + setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0 ); + setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0 ); +#ifdef DEBUG + printf("\n"); +#endif + + callParamCallbacks(); + + return comStatus_; +} + +asynStatus +SmarActMCSAxis::moveCmd(const char *fmt, ...) +{ +int val, ax; +char rep[REP_LEN]; +size_t got; +double tout = DEFLT_TIMEOUT; +va_list ap; + + va_start(ap, fmt); + comStatus_ = c_p_->sendCmd(&got, rep, sizeof(rep), tout, fmt, ap); + va_end(ap); + + if ( comStatus_ ) + goto bail; + + if ( c_p_->parseReply(rep, &ax, &val) ) { + comStatus_ = asynError; + goto bail; + } + +bail: +#ifdef DEBUG + printf("\n"); +#endif + + return comStatus_; +} + +asynStatus +SmarActMCSAxis::setSpeed(double velocity) +{ +long vel; +asynStatus status; + + if ( (vel = (long)rint(fabs(velocity))) != vel_ ) { + /* change speed */ + if ( asynSuccess == (status = moveCmd(":SCLS%u,%ld", channel_, vel)) ) { + vel_ = vel; + } + return status; + } + return asynSuccess; +} + +asynStatus +SmarActMCSAxis::move(double position, int relative, double min_vel, double max_vel, double accel) +{ +const char *fmt = relative ? ":MPR%u,%ld,%d" : ":MPA%u,%ld,%d"; + +#ifdef DEBUG + printf("Move to %f (speed %f - %f)\n", position, min_vel, max_vel); +#endif + + if ( (comStatus_ = setSpeed(max_vel)) ) + goto bail; + + + /* cache 'closed-loop' setting until next move */ + holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + + comStatus_ = moveCmd(fmt, channel_, (long)rint(position), holdTime_); + +bail: + if ( comStatus_ ) { + setIntegerParam(c_p_->motorStatusProblem_, 1); + setIntegerParam(c_p_->motorStatusCommsError_, 1); + callParamCallbacks(); + } + return comStatus_; +} + +asynStatus +SmarActMCSAxis::home(double min_vel, double max_vel, double accel, int forwards) +{ + +#ifdef DEBUG + printf("Home %u\n", forwards); +#endif + + if ( (comStatus_ = setSpeed(max_vel)) ) + goto bail; + + /* cache 'closed-loop' setting until next move */ + holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + + comStatus_ = moveCmd(":FRM%u,%u,%d,0", channel_, forwards ? 0 : 1, holdTime_); + +bail: + if ( comStatus_ ) { + setIntegerParam(c_p_->motorStatusProblem_, 1); + setIntegerParam(c_p_->motorStatusCommsError_, 1); + callParamCallbacks(); + } + return comStatus_; +} + +asynStatus +SmarActMCSAxis::stop(double acceleration) +{ +#ifdef DEBUG + printf("Stop\n"); +#endif + comStatus_ = moveCmd(":S%u", channel_); + + if ( comStatus_ ) { + setIntegerParam(c_p_->motorStatusProblem_, 1); + setIntegerParam(c_p_->motorStatusCommsError_, 1); + callParamCallbacks(); + } + return comStatus_; +} + +asynStatus +SmarActMCSAxis::setPosition(double position) +{ + comStatus_ = moveCmd(":SP%u,%d", channel_, (long)rint(position)); + if ( comStatus_ ) { + setIntegerParam(c_p_->motorStatusProblem_, 1); + setIntegerParam(c_p_->motorStatusCommsError_, 1); + callParamCallbacks(); + } + return comStatus_; +} + +asynStatus +SmarActMCSAxis::moveVelocity(double min_vel, double max_vel, double accel) +{ +long speed = (long)rint(fabs(max_vel)); +long tgt_pos = FAR_AWAY; + + /* No MCS command we an use directly. Just use a 'relative move' to + * very far target. + */ + +#ifdef DEBUG + printf("moveVelocity (%f - %f)\n", min_vel, max_vel); +#endif + + if ( 0 == speed ) { + /* Here we are in a dilemma. If we set the MCS' speed to zero + * then it will move at unlimited speed which is so fast that + * 'JOG' makes no sense. + * Just 'STOP' the motion - hope that works... + */ + setIntegerParam(c_p_->motorStop_, 1); + callParamCallbacks(); + return asynSuccess; + } + + if ( max_vel < 0 ) { + tgt_pos = -tgt_pos; + } + + if ( (comStatus_ = setSpeed(max_vel)) ) + goto bail; + + comStatus_ = moveCmd(":MPR%u,%ld,0", channel_, tgt_pos); + +bail: + if ( comStatus_ ) { + setIntegerParam(c_p_->motorStatusProblem_, 1); + setIntegerParam(c_p_->motorStatusCommsError_, 1); + callParamCallbacks(); + } + return comStatus_; +} + +/* iocsh wrapping and registration business (stolen from ACRMotorDriver.cpp) */ +static const iocshArg cc_a0 = {"Port name [string]", iocshArgString}; +static const iocshArg cc_a1 = {"I/O port name [string]", iocshArgString}; +static const iocshArg cc_a2 = {"Number of axes [int]", iocshArgInt}; +static const iocshArg cc_a3 = {"Moving poll period (s) [double]", iocshArgDouble}; +static const iocshArg cc_a4 = {"Idle poll period (s) [double]", iocshArgDouble}; + +static const iocshArg * const cc_as[] = {&cc_a0, &cc_a1, &cc_a2, &cc_a3, &cc_a4}; + +static const iocshFuncDef cc_def = {"smarActMCSCreateController", sizeof(cc_as)/sizeof(cc_as[0]), cc_as}; + +extern "C" void * +smarActMCSCreateController( + const char *motorPortName, + const char *ioPortName, + int numAxes, + double movingPollPeriod, + double idlePollPeriod) +{ +void *rval = 0; + // the asyn stuff doesn't seem to be prepared for exceptions. I get segfaults + // if constructing a controller (or axis) incurs an exception even if its + // caught (IMHO asyn should behave as if the controller/axis never existed...) +#ifdef ASYN_CANDO_EXCEPTIONS + try { +#endif + rval = new SmarActMCSController(motorPortName, ioPortName, numAxes, movingPollPeriod, idlePollPeriod); +#ifdef ASYN_CANDO_EXCEPTIONS + } catch (SmarActMCSException &e) { + epicsPrintf("smarActMCSCreateController failed (exception caught):\n%s\n", e.what()); + rval = 0; + } +#endif + + return rval; +} + +static void cc_fn(const iocshArgBuf *args) +{ + smarActMCSCreateController( + args[0].sval, + args[1].sval, + args[2].ival, + args[3].dval, + args[4].dval); +} + + +static const iocshArg ca_a0 = {"Controller Port name [string]", iocshArgString}; +static const iocshArg ca_a1 = {"Axis number [int]", iocshArgInt}; +static const iocshArg ca_a2 = {"Channel [int]", iocshArgInt}; + +static const iocshArg * const ca_as[] = {&ca_a0, &ca_a1, &ca_a2}; + +/* iocsh wrapping and registration business (stolen from ACRMotorDriver.cpp) */ +/* smarActMCSCreateAxis called to create each axis of the smarActMCS controller*/ +static const iocshFuncDef ca_def = {"smarActMCSCreateAxis", 3, ca_as}; + +extern "C" void * +smarActMCSCreateAxis( + const char *controllerPortName, + int axisNumber, + int channel) +{ +void *rval = 0; + +SmarActMCSController *pC; +SmarActMCSAxis *pAxis; +asynMotorAxis *pAsynAxis; + + // the asyn stuff doesn't seem to be prepared for exceptions. I get segfaults + // if constructing a controller (or axis) incurs an exception even if its + // caught (IMHO asyn should behave as if the controller/axis never existed...) +#ifdef ASYN_CANDO_EXCEPTIONS + try { +#endif +// rval = new SmarActMCSAxis(, axisNumber, channel); + pC = (SmarActMCSController*) findAsynPortDriver(controllerPortName); + if (!pC) { + printf("smarActMCSCreateAxis: Error port %s not found\n", controllerPortName); + rval = 0; + return rval; + } + // check if axis number already exists + pAsynAxis = pC->getAxis(axisNumber); + if (pAsynAxis != NULL) { // axis already exists + epicsPrintf("SmarActMCSCreateAxis failed:axis %u already exists\n", axisNumber); +#ifdef ASYN_CANDO_EXCEPTIONS + THROW_(SmarActMCSException(MCSCommunicationError, "axis %u already exists", axisNumber)); +#endif + rval = 0; + return rval; + } + pC->lock(); + pAxis = new SmarActMCSAxis(pC, axisNumber, channel); + pAxis = NULL; + pC->unlock(); + +#ifdef ASYN_CANDO_EXCEPTIONS + } catch (SmarActMCSException &e) { + epicsPrintf("SmarActMCSAxis failed (exception caught):\n%s\n", e.what()); + rval = 0; + } +#endif + + return rval; +} + +static void ca_fn(const iocshArgBuf *args) +{ + smarActMCSCreateAxis( + args[0].sval, + args[1].ival, + args[2].ival); +} + +static void smarActMCSMotorRegister(void) +{ + iocshRegister(&cc_def, cc_fn); // smarActMCSCreateController + iocshRegister(&ca_def, ca_fn); // smarActMCSCreateAxis +} + +extern "C" { +epicsExportRegistrar(smarActMCSMotorRegister); +} diff --git a/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.h b/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.h new file mode 100644 index 00000000..c42ad18d --- /dev/null +++ b/motorApp/SmarActMCSSrc/smarActMCSMotorDriver.h @@ -0,0 +1,93 @@ +#ifndef SMARACT_MCS_MOTOR_DRIVER_H +#define SMARACT_MCS_MOTOR_DRIVER_H + +/* Motor driver support for smarAct MCS RS-232 Controller */ + +/* Derived from ACRMotorDriver.cpp by Mark Rivers, 2011/3/28 */ + +/* Author: Till Straumann , 9/11 */ + +#ifdef __cplusplus + +#include +#include +#include +#include + +enum SmarActMCSExceptionType { + MCSUnknownError, + MCSConnectionError, + MCSCommunicationError, +}; + +class SmarActMCSException : public std::exception { +public: + SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, ...); + SmarActMCSException(SmarActMCSExceptionType t) + : t_(t) + { str_[0] = 0; } + SmarActMCSException() + : t_(MCSUnknownError) + { str_[0] = 0; } + SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, va_list ap); + SmarActMCSExceptionType getType() + const { return t_; } + virtual const char *what() + const throw() {return str_ ;} +protected: + char str_[100]; + SmarActMCSExceptionType t_; +}; + + +class SmarActMCSAxis : public asynMotorAxis +{ +public: + SmarActMCSAxis(class SmarActMCSController *cnt_p, int axis, int channel); + asynStatus poll(bool *moving_p); + asynStatus move(double position, int relative, double min_vel, double max_vel, double accel); + asynStatus home(double min_vel, double max_vel, double accel, int forwards); + asynStatus stop(double acceleration); + asynStatus setPosition(double position); + asynStatus moveVelocity(double min_vel, double max_vel, double accel); + + virtual asynStatus getVal(const char *parm, int *val_p); + virtual asynStatus moveCmd(const char *cmd, ...); + virtual int getClosedLoop(); + + int getVel() const { return vel_; } + +protected: + asynStatus setSpeed(double velocity); + +private: + SmarActMCSController *c_p_; // pointer to asynMotorController for this axis + asynStatus comStatus_; + int vel_; + unsigned holdTime_; + int channel_; + +friend class SmarActMCSController; +}; + +class SmarActMCSController : public asynMotorController +{ +public: + SmarActMCSController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod); + virtual asynStatus sendCmd(size_t *got_p, char *rep, int len, double timeout, const char *fmt, va_list ap); + virtual asynStatus sendCmd(size_t *got_p, char *rep, int len, double timeout, const char *fmt, ...); + virtual asynStatus sendCmd(size_t *got_p, char *rep, int len, const char *fmt, ...); + virtual asynStatus sendCmd(char *rep, int len, const char *fmt, ...); + + static int parseReply(const char *reply, int *ax_p, int *val_p); + +protected: + SmarActMCSAxis **pAxes_; + +private: + asynUser *asynUserMot_p_; +friend class SmarActMCSAxis; +}; + +#endif // _cplusplus +#endif // SMARACT_MCS_MOTOR_DRIVER_H