diff --git a/smarActApp/src/smarActMCS2MotorDriver.cpp b/smarActApp/src/smarActMCS2MotorDriver.cpp index 215cf83..a0e5ed1 100644 --- a/smarActApp/src/smarActMCS2MotorDriver.cpp +++ b/smarActApp/src/smarActMCS2MotorDriver.cpp @@ -8,7 +8,6 @@ Jan 19, 2019 */ - #include #include #include @@ -24,22 +23,28 @@ Jan 19, 2019 #include #include "smarActMCS2MotorDriver.h" +#ifdef DEBUG + #define DBG_PRINTF(...) printf(__VA_ARGS__) +#else + #define DBG_PRINTF(...) +#endif + static const char *driverName = "SmarActMCS2MotorDriver"; /** Creates a new MCS2Controller object. - * \param[in] portName The name of the asyn port that will be created for this driver - * \param[in] MCS2PortName The name of the drvAsynIPPPort that was created previously to connect to the MCS2 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 - */ -MCS2Controller::MCS2Controller(const char *portName, const char *MCS2PortName, int numAxes, + * \param[in] portName The name of the asyn port that will be created for this driver + * \param[in] MCS2PortName The name of the drvAsynIPPPort that was created previously to connect to the MCS2 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 + */ +MCS2Controller::MCS2Controller(const char *portName, const char *MCS2PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, int unusedMask) - : asynMotorController(portName, numAxes, NUM_MCS2_PARAMS, - 0, 0, - ASYN_CANBLOCK | ASYN_MULTIDEVICE, - 1, // autoconnect - 0, 0) // Default priority and stack size + : asynMotorController(portName, numAxes, NUM_MCS2_PARAMS, + 0, 0, + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, 0) // Default priority and stack size { int axis, axisMask = 0; asynStatus status; @@ -47,23 +52,23 @@ MCS2Controller::MCS2Controller(const char *portName, const char *MCS2PortName, i asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, "MCS2Controller::MCS2Controller: Creating controller\n"); // Create controller-specific parameters - createParam(MCS2MclfString, asynParamInt32, &this->mclf_); createParam(MCS2PtypString, asynParamInt32, &this->ptyp_); createParam(MCS2PtypRbString, asynParamInt32, &this->ptyprb_); - createParam(MCS2PstatString, asynParamInt32, &this->pstatrb_); // whole positioner status word - createParam(MCS2RefString, asynParamInt32, &this->ref_); + createParam(MCS2PstatString, asynParamInt32, &this->pstatrb_); // whole positioner status word + createParam(MCS2MclfString, asynParamInt32, &this->mclf_); + createParam(MCS2HoldString, asynParamInt32, &this->hold_); createParam(MCS2CalString, asynParamInt32, &this->cal_); - /* Connect to MCS2 controller */ + // Connect to MCS2 controller status = pasynOctetSyncIO->connect(MCS2PortName, 0, &pasynUserController_, NULL); - pasynOctetSyncIO->setInputEos (pasynUserController_, "\r\n", 2); + pasynOctetSyncIO->setInputEos(pasynUserController_, "\r\n", 2); pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r\n", 2); asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, "MCS2Controller::MCS2Controller: Connecting to controller\n"); if (status) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s:%s: cannot connect to MCS2 controller\n", - driverName, functionName); + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s:%s: cannot connect to MCS2 controller\n", + driverName, functionName); } asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, "MCS2Controller::MCS2Controller: Clearing error messages\n"); this->clearErrors(); @@ -71,40 +76,38 @@ MCS2Controller::MCS2Controller(const char *portName, const char *MCS2PortName, i sprintf(this->outString_, ":DEV:SNUM?"); status = this->writeReadController(); if (status) { - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "%s:%s: cannot connect to MCS2 controller\n", - driverName, functionName); + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "%s:%s: cannot connect to MCS2 controller\n", + driverName, functionName); } asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "MCS2Controller::MCS2Controller: Device Name: %s\n", this->inString_); this->clearErrors(); - // Create the axis objects asynPrint(this->pasynUserSelf, ASYN_TRACEIO_DRIVER, "MCS2Controller::MCS2Controller: Creating axes\n"); for(axis=0; axis> axis; - if(!axisMask) - new MCS2Axis(this, axis); + if (!axisMask) + new MCS2Axis(this, axis); } startPoller(movingPollPeriod, idlePollPeriod, 2); } - /** Creates a new MCS2Controller 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] MCS2PortName The name of the drvAsynIPPPort that was created previously to connect to the MCS2 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 MCS2CreateController(const char *portName, const char *MCS2PortName, int numAxes, + * 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] MCS2PortName The name of the drvAsynIPPPort that was created previously to connect to the MCS2 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 MCS2CreateController(const char *portName, const char *MCS2PortName, int numAxes, int movingPollPeriod, int idlePollPeriod, int unusedMask) { - new MCS2Controller(portName, MCS2PortName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000., unusedMask); - return(asynSuccess); + new MCS2Controller(portName, MCS2PortName, numAxes, movingPollPeriod / 1000., idlePollPeriod / 1000., unusedMask); + return (asynSuccess); } asynStatus MCS2Controller::clearErrors() @@ -121,89 +124,88 @@ asynStatus MCS2Controller::clearErrors() if (comStatus) goto skip; numErrorMsgs = atoi(this->inString_); for (int i=0; ioutString_, ":SYST:ERR?"); - comStatus = this->writeReadController(); + sprintf(this->outString_, ":SYST:ERR?"); + comStatus = this->writeReadController(); if (comStatus) goto skip; - printf("%s", this->inString_); - errorCode = atoi(this->inString_); + printf("%s", this->inString_); + errorCode = atoi(this->inString_); switch (errorCode){ - case 259: sprintf(errorMsg, "No sensor present"); - break; - case 34: sprintf(errorMsg, "Invalid channel index"); - break; - case 0: sprintf(errorMsg, "No error"); - break; - case -101: sprintf(errorMsg, "Invalid character"); - break; - case -103: sprintf(errorMsg, "Invalid seperator"); - break; - case -104: sprintf(errorMsg, "Data type error"); - break; - case -108: sprintf(errorMsg, "Parameter not allowed"); - break; - case -109: sprintf(errorMsg, "Missing parameter"); - break; - case -113: sprintf(errorMsg, "Command not exist"); - break; - case -151: sprintf(errorMsg, "Invalid string"); - break; - case -350: sprintf(errorMsg, "Queue overflow"); - break; - case -363: sprintf(errorMsg, "Buffer overrun"); - break; - default: sprintf(errorMsg, "Unable to decode %d", errorCode); - break; - } - asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "MCS2Controller::clearErrors: %s\n", errorMsg); + case 259: sprintf(errorMsg, "No sensor present"); + break; + case 34: sprintf(errorMsg, "Invalid channel index"); + break; + case 0: sprintf(errorMsg, "No error"); + break; + case -101: sprintf(errorMsg, "Invalid character"); + break; + case -103: sprintf(errorMsg, "Invalid seperator"); + break; + case -104: sprintf(errorMsg, "Data type error"); + break; + case -108: sprintf(errorMsg, "Parameter not allowed"); + break; + case -109: sprintf(errorMsg, "Missing parameter"); + break; + case -113: sprintf(errorMsg, "Command not exist"); + break; + case -151: sprintf(errorMsg, "Invalid string"); + break; + case -350: sprintf(errorMsg, "Queue overflow"); + break; + case -363: sprintf(errorMsg, "Buffer overrun"); + break; + default: sprintf(errorMsg, "Unable to decode %d", errorCode); + break; + } + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "MCS2Controller::clearErrors: %s\n", errorMsg); } - skip: - setIntegerParam(this->motorStatusProblem_, comStatus ? 1:0); +skip: + setIntegerParam(this->motorStatusProblem_, comStatus ? 1 : 0); callParamCallbacks(); return comStatus ? asynError : 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 calls asynMotorController::report() - */ + * \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 calls asynMotorController::report() + */ void MCS2Controller::report(FILE *fp, int level) { - fprintf(fp, "MCS2 motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", - this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_); + fprintf(fp, "MCS2 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 MCS2MotorAxis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] pasynUser asynUser structure that encodes the axis index number. */ -MCS2Axis* MCS2Controller::getAxis(asynUser *pasynUser) + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] pasynUser asynUser structure that encodes the axis index number. */ +MCS2Axis *MCS2Controller::getAxis(asynUser *pasynUser) { - return static_cast(asynMotorController::getAxis(pasynUser)); + return static_cast(asynMotorController::getAxis(pasynUser)); } /** Returns a pointer to an MCS2MotorAxis object. - * Returns NULL if the axis number encoded in pasynUser is invalid. - * \param[in] axisNo Axis index number. */ -MCS2Axis* MCS2Controller::getAxis(int axisNo) + * Returns NULL if the axis number encoded in pasynUser is invalid. + * \param[in] axisNo Axis index number. */ +MCS2Axis *MCS2Controller::getAxis(int axisNo) { - return static_cast(asynMotorController::getAxis(axisNo)); + return static_cast(asynMotorController::getAxis(axisNo)); } /** Called when asyn clients call pasynInt32->write(). - * Extracts the function and axis number from pasynUser. - * Sets the value in the parameter library. - * For all other functions it calls asynMotorController::writeInt32. - * Calls any registered callbacks for this pasynUser->reason and address. - * \param[in] pasynUser asynUser structure that encodes the reason and address. - * \param[in] value Value to write. */ + * Extracts the function and axis number from pasynUser. + * Sets the value in the parameter library. + * For all other functions it calls asynMotorController::writeInt32. + * Calls any registered callbacks for this pasynUser->reason and address. + * \param[in] pasynUser asynUser structure that encodes the reason and address. + * \param[in] value Value to write. */ asynStatus MCS2Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) { int function = pasynUser->reason; @@ -211,155 +213,157 @@ asynStatus MCS2Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) MCS2Axis *pAxis = getAxis(pasynUser); static const char *functionName = "writeInt32"; - /* Check if axis exists */ + // Check if axis exists if(!pAxis) return asynError; /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the * status at the end, but that's OK */ status = setIntegerParam(pAxis->axisNo_, function, value); - if (function == mclf_) { - /* set MCLF */ - sprintf(pAxis->pC_->outString_, ":CHAN%d:MCLF:CURR %d", pAxis->axisNo_, value); + if (function == ptyp_) { + // set positioner type + sprintf(pAxis->pC_->outString_, ":CHAN%d:PTYP %d", pAxis->axisNo_, value); status = pAxis->pC_->writeController(); } - else if (function == ptyp_) { - /* set positioner type */ - sprintf(pAxis->pC_->outString_, ":CHAN%d:PTYP %d", pAxis->axisNo_, value); + else if (function == mclf_) { + // set piezo MaxClockFreq + sprintf(pAxis->pC_->outString_, ":CHAN%d:MCLF:CURR %d", pAxis->axisNo_, value); status = pAxis->pC_->writeController(); } else if (function == cal_) { - /* send calibration command */ + // send calibration command sprintf(pAxis->pC_->outString_, ":CAL%d", pAxis->axisNo_); status = pAxis->pC_->writeController(); } + else if (function == hold_) { + // set holding time + sprintf(pAxis->pC_->outString_, ":CHAN%d:HOLD %d", pAxis->axisNo_, value); + puts(pAxis->pC_->outString_); + status = pAxis->pC_->writeController(); + } else { - /* Call base class method */ + // Call base class method status = asynMotorController::writeInt32(pasynUser, value); } - /* Do callbacks so higher layers see any changes */ + // Do callbacks so higher layers see any changes callParamCallbacks(pAxis->axisNo_); if (status) asynPrint(pasynUser, ASYN_TRACE_ERROR, - "%s:%s: error, status=%d function=%d, value=%d\n", - driverName, functionName, status, function, value); + "%s:%s: error, status=%d function=%d, value=%d\n", + driverName, functionName, status, function, value); else asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, - "%s:%s: function=%d, value=%d\n", - driverName, functionName, function, value); + "%s:%s: function=%d, value=%d\n", + driverName, functionName, function, value); return status; } // These are the MCS2Axis methods /** Creates a new MCS2Axis object. - * \param[in] pC Pointer to the ACRController to which this axis belongs. - * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. - * - * Initializes register numbers, etc. - */ + * \param[in] pC Pointer to the ACRController to which this axis belongs. + * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1. + * + * Initializes register numbers, etc. + */ MCS2Axis::MCS2Axis(MCS2Controller *pC, int axisNo) - : asynMotorAxis(pC, axisNo), - pC_(pC) + : asynMotorAxis(pC, axisNo), + pC_(pC) { - asynStatus status; - asynPrint(pC->pasynUserSelf, ASYN_TRACEIO_DRIVER, "MCS2Axis::MCS2Axis: Creating axis %u\n", axisNo); channel_ = axisNo; - - // Set hold time - sprintf(pC_->outString_, ":CHAN%d:HOLD %d", channel_, HOLD_FOREVER); - status = pC_->writeController(); pC_->clearErrors(); callParamCallbacks(); } /** 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 calls asynMotorController::report() - */ + * \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 calls asynMotorController::report() + */ void MCS2Axis::report(FILE *fp, int level) { if (level > 0) { - int pcode; - char pname[256]; - int channelState; - int vel; - int acc; - int mclf; + int pcode; + char pname[256]; + int channelState; + int vel; + int acc; + int mclf; int followError; - int error; - int temp; - - asynStatus status; - - sprintf(pC_->outString_, ":CHAN%d:PTYP?", channel_); - status = pC_->writeReadController(); - pcode = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:PTYP:NAME?", channel_); - status = pC_->writeReadController(); - strcpy(pC_->inString_, pname); - sprintf(pC_->outString_, ":CHAN%d:STAT?", channel_); - status = pC_->writeReadController(); - channelState = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:VEL?", channel_); - status = pC_->writeReadController(); - vel = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:ACC?", channel_); - status = pC_->writeReadController(); - acc = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:MCLF?", channel_); - status = pC_->writeReadController(); - mclf = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:FERR?", channel_); - status = pC_->writeReadController(); - followError = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:ERR?", channel_); - status = pC_->writeReadController(); - error = atoi(pC_->inString_); - sprintf(pC_->outString_, ":CHAN%d:TEMP?", channel_); - status = pC_->writeReadController(); - temp = atoi(pC_->inString_); - - - + int error; + int temp; + + asynStatus status; + + sprintf(pC_->outString_, ":CHAN%d:PTYP?", channel_); + status = pC_->writeReadController(); + pcode = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:PTYP:NAME?", channel_); + status = pC_->writeReadController(); + strcpy(pC_->inString_, pname); + sprintf(pC_->outString_, ":CHAN%d:STAT?", channel_); + status = pC_->writeReadController(); + channelState = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:VEL?", channel_); + status = pC_->writeReadController(); + vel = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:ACC?", channel_); + status = pC_->writeReadController(); + acc = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:MCLF?", channel_); + status = pC_->writeReadController(); + mclf = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:FERR?", channel_); + status = pC_->writeReadController(); + followError = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:ERR?", channel_); + status = pC_->writeReadController(); + error = atoi(pC_->inString_); + sprintf(pC_->outString_, ":CHAN%d:TEMP?", channel_); + status = pC_->writeReadController(); + temp = atoi(pC_->inString_); + + if (status) + fprintf(fp, " axis %d -> asyn status error %d\n", axisNo_, status); fprintf(fp, " axis %d\n" - " positioner type %d\n" - " positioner name %s\n" - " state %d\n" - " velocity %d\n" - " acceleration %d\n" - " max closed loop frequency %d\n" - " following error %d\n" - " error %d\n" - " temp %d\n", + " positioner type %d\n" + " positioner name %s\n" + " state %d\n" + " velocity %d\n" + " acceleration %d\n" + " max closed loop frequency %d\n" + " following error %d\n" + " error %d\n" + " temp %d\n", axisNo_, pcode, pname, channelState, vel, - acc, mclf, followError, error, temp); - pC_->clearErrors(); + acc, mclf, followError, error, temp); + pC_->clearErrors(); } // Call the base class method asynMotorAxis::report(fp, level); } - asynStatus MCS2Axis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration) { asynStatus status = asynSuccess; - static const char *functionName = "move"; - - /* MCS2 move mode is: - * - absolute=0 - * - relative=1 - * - step=4 - */ - if(sensorPresent_) { - // closed loop move - // Set hold time + + // MCS2 MMOD (move mode) values: + // 0: absolute (close loop) + // 1 relative (close loop) + // 4 relative steps (open loop) + int hasEnc; + pC_->getIntegerParam(channel_, pC_->motorStatusHasEncoder_, &hasEnc); + + if(hasEnc) + { + printf("MCS2Axis::move: closeloop: position: %g rel: %d\n",position,relative); + + DBG_PRINTF("MCS2Axis::move: closeloop: position: %g rel: %d\n",position,relative); sprintf(pC_->outString_, ":CHAN%d:MMOD %d", channel_, relative > 0 ? 1 : 0); status = pC_->writeController(); // Set acceleration @@ -371,57 +375,53 @@ asynStatus MCS2Axis::move(double position, int relative, double minVelocity, dou // Do move sprintf(pC_->outString_, ":MOVE%d %f", channel_, position * PULSES_PER_STEP); status = pC_->writeController(); - } else { - // open loop move - PositionType dtg = position - stepTarget_; // distance to go - stepTarget_ = (PositionType)position; // store position in global scope - // Set mode; 4 == STEP - sprintf(pC_->outString_, ":CHAN%d:MMOD 4", channel_); - status = pC_->writeController(); - // Set frequency; range 1..20000 Hz - unsigned short frequency = (unsigned short)maxVelocity; - if(frequency >= MAX_FREQUENCY) { - frequency = MAX_FREQUENCY; - } - sprintf(pC_->outString_, ":CHAN%d:STEP:FREQ %u", channel_, frequency); + } + else + { // move relative in open loop + double curPos; + pC_->getDoubleParam(channel_, pC_->motorEncoderPosition_,&curPos); + //pC_->getDoubleParam(channel_, pC_->motorPosition_, &curPos); + if(!relative) + position=position-curPos; + curPos+=position; + DBG_PRINTF("MCS2Axis::move: openloop: new_pos: %g rel_move: %g\n",curPos,position); + //:STEP:FREQuency 1..20000, default:1000 Hz. + //:STEP:AMPLitude 1..65535, default:65535 (100 V). + setDoubleParam(pC_->motorEncoderPosition_, curPos); + setDoubleParam(pC_->motorPosition_, curPos); + + sprintf(pC_->outString_, ":CHAN%d:MMOD %d", channel_, 4); status = pC_->writeController(); - // Do move - sprintf(pC_->outString_, ":MOVE%d %lld", channel_, dtg); + DBG_PRINTF("MCS2Axis::move: %s ->status:%d\n",pC_->outString_,status); + sprintf(pC_->outString_, ":MOVE%d %f", channel_, position); status = pC_->writeController(); - setDoubleParam(pC_->motorPosition_, (double)stepTarget_); + DBG_PRINTF("MCS2Axis::move: %s ->status:%d\n",pC_->outString_,status); } - return status; } asynStatus MCS2Axis::home(double minVelocity, double maxVelocity, double acceleration, int forwards) { - asynStatus status=asynSuccess; - static const char *functionName = "homeAxis"; - printf("Home command received %d\n", forwards); + asynStatus status = asynSuccess; unsigned short refOpt = 0; if (forwards==0){ - refOpt |= START_DIRECTION; + refOpt |= START_DIRECTION; } refOpt |= AUTO_ZERO; // Set default reference options - direction and autozero - printf("ref opt: %d\n", refOpt); sprintf(pC_->outString_, ":CHAN%d:REF:OPT %d", channel_, refOpt); - status = pC_->writeController(); - pC_->clearErrors(); + DBG_PRINTF("MCS2Axis::home: %s",pC_->outString_); - // Set hold time - sprintf(pC_->outString_, ":CHAN%d:HOLD %d", channel_, HOLD_FOREVER); status = pC_->writeController(); pC_->clearErrors(); // Set acceleration - sprintf(pC_->outString_, ":CHAN%d:ACC %f", channel_, acceleration*PULSES_PER_STEP); + sprintf(pC_->outString_, ":CHAN%d:ACC %f", channel_, acceleration * PULSES_PER_STEP); status = pC_->writeController(); pC_->clearErrors(); // Set velocity - sprintf(pC_->outString_, ":CHAN%d:VEL %f", channel_, maxVelocity*PULSES_PER_STEP); + sprintf(pC_->outString_, ":CHAN%d:VEL %f", channel_, maxVelocity * PULSES_PER_STEP); status = pC_->writeController(); pC_->clearErrors(); // Begin move @@ -432,10 +432,9 @@ asynStatus MCS2Axis::home(double minVelocity, double maxVelocity, double acceler return status; } -asynStatus MCS2Axis::stop(double acceleration ) +asynStatus MCS2Axis::stop(double acceleration) { asynStatus status; - static const char *functionName = "stopAxis"; sprintf(pC_->outString_, ":STOP%d", channel_); status = pC_->writeController(); @@ -445,23 +444,23 @@ asynStatus MCS2Axis::stop(double acceleration ) asynStatus MCS2Axis::setPosition(double position) { - asynStatus status=asynSuccess; + asynStatus status = asynSuccess; - printf("Set position receieved\n"); - sprintf(pC_->outString_, ":CHAN%d:POS %f", channel_, position*PULSES_PER_STEP); + sprintf(pC_->outString_, ":CHAN%d:POS %f", channel_, position * PULSES_PER_STEP); + DBG_PRINTF("MCS2Axis::setPosition: %s",pC_->outString_); status = pC_->writeController(); return status; } /** Polls the axis. - * This function reads the controller position, encoder position, the limit status, the moving status, - * the drive power-on status and positioner type. It does not current detect following error, etc. - * but this could be added. - * 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 (1) or done (0). */ + * This function reads the controller position, encoder position, the limit status, the moving status, + * the drive power-on status and positioner type. It does not current detect following error, etc. + * but this could be added. + * 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 (1) or done (0). */ asynStatus MCS2Axis::poll(bool *moving) -{ +{ int done; int chanState; int closedLoop; @@ -477,6 +476,7 @@ asynStatus MCS2Axis::poll(bool *moving) double theoryPosition; int driveOn; int mclf; + int hold; asynStatus comStatus = asynSuccess; // Read the channel state @@ -485,21 +485,25 @@ asynStatus MCS2Axis::poll(bool *moving) if (comStatus) goto skip; chanState = atoi(pC_->inString_); setIntegerParam(pC_->pstatrb_, chanState); - done = (chanState & ACTIVELY_MOVING)?0:1; - closedLoop = (chanState & CLOSED_LOOP_ACTIVE)?1:0; - sensorPresent = (chanState & SENSOR_PRESENT)?1:0; - isCalibrated = (chanState & IS_CALIBRATED)?1:0; - isReferenced = (chanState & IS_REFERENCED)?1:0; - endStopReached = (chanState & END_STOP_REACHED)?1:0; - followLimitReached = (chanState & FOLLOWING_LIMIT_REACHED)?1:0; - movementFailed = (chanState & MOVEMENT_FAILED)?1:0; - refMark = (chanState & REFERENCE_MARK)?1:0; - - *moving = done ? false:true; + done = (chanState & ACTIVELY_MOVING) ? 0 : 1; + closedLoop = (chanState & CLOSED_LOOP_ACTIVE) ? 1 : 0; + sensorPresent = (chanState & SENSOR_PRESENT) ? 1 : 0; + isCalibrated = (chanState & IS_CALIBRATED) ? 1 : 0; + isReferenced = (chanState & IS_REFERENCED) ? 1 : 0; + endStopReached = (chanState & END_STOP_REACHED) ? 1 : 0; + followLimitReached = (chanState & FOLLOWING_LIMIT_REACHED) ? 1 : 0; + movementFailed = (chanState & MOVEMENT_FAILED) ? 1 : 0; + refMark = (chanState & REFERENCE_MARK) ? 1 : 0; + //if (channel_==0) + //{ //debug code + // int hasEnc; pC_->getIntegerParam(channel_, pC_->motorStatusHasEncoder_, &hasEnc); + // printf("chanState:0x%x hasEnc:%d closedLoop:%d\n",chanState,hasEnc,closedLoop); + //} + *moving = done ? false : true; + //https://epics.anl.gov/bcda/synApps/motor/R7-2-1/motorRecord.html#Fields_status setIntegerParam(pC_->motorStatusDone_, done); - setIntegerParam(pC_->motorClosedLoop_, closedLoop); setIntegerParam(pC_->motorStatusHasEncoder_, sensorPresent); - setIntegerParam(pC_->motorStatusGainSupport_, sensorPresent); + setIntegerParam(pC_->motorStatusGainSupport_, closedLoop); setIntegerParam(pC_->motorStatusHomed_, isReferenced); setIntegerParam(pC_->motorStatusHighLimit_, endStopReached); setIntegerParam(pC_->motorStatusLowLimit_, endStopReached); @@ -507,9 +511,10 @@ asynStatus MCS2Axis::poll(bool *moving) setIntegerParam(pC_->motorStatusProblem_, movementFailed); setIntegerParam(pC_->motorStatusAtHome_, refMark); - // Read the current encoder position, if the positioner has a sensor - sensorPresent_ = sensorPresent; - if(sensorPresent){ + if (sensorPresent) + { + // avoid pollong POS? if there is no sensor present. This would not return a value and block the polling for 1-2 sec + // Read the current encoder position sprintf(pC_->outString_, ":CHAN%d:POS?", channel_); comStatus = pC_->writeReadController(); if (comStatus) goto skip; @@ -530,9 +535,8 @@ asynStatus MCS2Axis::poll(bool *moving) sprintf(pC_->outString_, ":CHAN%d:AMPL?", channel_); comStatus = pC_->writeReadController(); if (comStatus) goto skip; - driveOn = atoi(pC_->inString_) ? 1:0; + driveOn = atoi(pC_->inString_) ? 1 : 0; setIntegerParam(pC_->motorStatusPowerOn_, driveOn); - setIntegerParam(pC_->motorStatusProblem_, 0); // Read the currently selected positioner type sprintf(pC_->outString_, ":CHAN%d:PTYP?", channel_); @@ -541,20 +545,24 @@ asynStatus MCS2Axis::poll(bool *moving) positionerType = atoi(pC_->inString_); setIntegerParam(pC_->ptyprb_, positionerType); - // Read CAL/REF status and MCLF when idle - if(done) + // Read CAL status MCLF HOLD when idle + if (done) { - setIntegerParam(pC_->cal_, isCalibrated); - setIntegerParam(pC_->ref_, isReferenced); - sprintf(pC_->outString_, ":CHAN%d:MCLF?", channel_); - comStatus = pC_->writeReadController(); + setIntegerParam(pC_->cal_, isCalibrated); + sprintf(pC_->outString_, ":CHAN%d:MCLF?", channel_); + comStatus = pC_->writeReadController(); if (comStatus) goto skip; - mclf = atoi(pC_->inString_); - setIntegerParam(pC_->mclf_, mclf); + mclf = atoi(pC_->inString_); + setIntegerParam(pC_->mclf_, mclf); + sprintf(pC_->outString_, ":CHAN%d:HOLD?", channel_); + comStatus = pC_->writeReadController(); + if (comStatus) goto skip; + hold = atoi(pC_->inString_); + setIntegerParam(pC_->hold_, hold); } - skip: - setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0); +skip: + setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0); callParamCallbacks(); return comStatus ? asynError : asynSuccess; } @@ -566,12 +574,12 @@ static const iocshArg MCS2CreateControllerArg2 = {"Number of axes", iocshArgInt} static const iocshArg MCS2CreateControllerArg3 = {"Moving poll period (ms)", iocshArgInt}; static const iocshArg MCS2CreateControllerArg4 = {"Idle poll period (ms)", iocshArgInt}; static const iocshArg MCS2CreateControllerArg5 = {"Unused bit mask", iocshArgInt}; -static const iocshArg * const MCS2CreateControllerArgs[] = {&MCS2CreateControllerArg0, - &MCS2CreateControllerArg1, - &MCS2CreateControllerArg2, - &MCS2CreateControllerArg3, - &MCS2CreateControllerArg4, - &MCS2CreateControllerArg5}; +static const iocshArg *const MCS2CreateControllerArgs[] = {&MCS2CreateControllerArg0, + &MCS2CreateControllerArg1, + &MCS2CreateControllerArg2, + &MCS2CreateControllerArg3, + &MCS2CreateControllerArg4, + &MCS2CreateControllerArg5}; static const iocshFuncDef MCS2CreateControllerDef = {"MCS2CreateController", 6, MCS2CreateControllerArgs}; static void MCS2CreateContollerCallFunc(const iocshArgBuf *args) { @@ -584,6 +592,5 @@ static void MCS2MotorRegister(void) } extern "C" { -epicsExportRegistrar(MCS2MotorRegister); + epicsExportRegistrar(MCS2MotorRegister); } - diff --git a/smarActApp/src/smarActMCS2MotorDriver.h b/smarActApp/src/smarActMCS2MotorDriver.h index 3bd24e3..dfde31b 100644 --- a/smarActApp/src/smarActMCS2MotorDriver.h +++ b/smarActApp/src/smarActMCS2MotorDriver.h @@ -8,14 +8,14 @@ Jan 19, 2019 Note: The MCS2 controller uses 64-bit int for the encoder and target positions. The motor record is limited -to 32 bit int for RMP (https://github.com/epics-modules/motor/issues/8, +to 32 bit int for RMP (https://github.com/epics-modules/motor/issues/8, https://epics.anl.gov/tech-talk/2018/msg00087.php) which effectively limits the travel -range to +/- 2.1mm. +range to +/- 2.1mm. Since it doesn't seem the motor record will update to using 64bit int the choices I can see are: * 1 - using a non-standard motor support * 2 - rescaling the minimum resolution to 1nm to effectively increase the range to 2.1m -I chose option 2. +I chose option 2. 1 step = 1nm Someone with more experience may have a better solution. @@ -38,8 +38,6 @@ The two that may be of significant interest are: * If this scaling was not implemented the maximum range would be ~2.147 mm/deg, now it's ~2147 mm/deg */ #define PULSES_PER_STEP 1000 -typedef long long PositionType; - /** MCS2 Axis status flags **/ const unsigned short ACTIVELY_MOVING = 0x0001; const unsigned short CLOSED_LOOP_ACTIVE = 0x0002; @@ -65,16 +63,12 @@ const unsigned short ABORT_ON_END_STOP = 0x0008; const unsigned short CONTINUE_ON_REF_FOUND = 0x0010; const unsigned short STOP_ON_REF_FOUND = 0x0020; -/** MCS2 Axis constants **/ -#define HOLD_FOREVER 0xffffffff -#define MAX_FREQUENCY 20000 - /** drvInfo strings for extra parameters that the MCS2 controller supports */ -#define MCS2MclfString "MCLF" #define MCS2PtypString "PTYP" #define MCS2PtypRbString "PTYP_RB" #define MCS2PstatString "PSTAT" -#define MCS2RefString "REF" +#define MCS2MclfString "MCLF" +#define MCS2HoldString "HOLD" #define MCS2CalString "CAL" class epicsShareClass MCS2Axis : public asynMotorAxis @@ -90,17 +84,15 @@ class epicsShareClass MCS2Axis : public asynMotorAxis asynStatus setPosition(double position); private: - MCS2Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. - * Abbreviated because it is used very frequently */ + MCS2Controller *pC_; /**< Pointer to the asynMotorController to which this axis belongs. + * Abbreviated because it is used very frequently */ int channel_; - int sensorPresent_; - PositionType stepTarget_ = 0; - asynStatus comStatus_; -friend class MCS2Controller; + friend class MCS2Controller; }; -class epicsShareClass MCS2Controller : public asynMotorController { +class epicsShareClass MCS2Controller : public asynMotorController +{ public: MCS2Controller(const char *portName, const char *MCS2PortName, int numAxes, double movingPollPeriod, double idlePollPeriod, int unusedMask = 0); virtual asynStatus clearErrors(); @@ -110,20 +102,19 @@ class epicsShareClass MCS2Controller : public asynMotorController { /* These are the methods that we override from asynMotorDriver */ void report(FILE *fp, int level); - MCS2Axis* getAxis(asynUser *pasynUser); - MCS2Axis* getAxis(int axisNo); + MCS2Axis *getAxis(asynUser *pasynUser); + MCS2Axis *getAxis(int axisNo); protected: - int mclf_; /**< MCL frequency */ -#define FIRST_MCS2_PARAM mclf_ - int ptyp_; /**< positioner type */ - int ptyprb_; /**< positioner type readback */ + int ptyp_; /**< positioner type */ +#define FIRST_MCS2_PARAM ptyp_ + int ptyprb_; /**< positioner type readback */ int pstatrb_; /**< positoner status word readback */ - int ref_; /**< reference command */ - int cal_; /**< calibration command */ + int mclf_; /**< MCL frequency */ + int hold_; /**< hold time */ + int cal_; /**< calibration command */ #define LAST_MCS2_PARAM cal_ #define NUM_MCS2_PARAMS (&LAST_MCS2_PARAM - &FIRST_MCS2_PARAM + 1) - -friend class MCS2Axis; -}; + friend class MCS2Axis; +}; diff --git a/smarActApp/src/smarActMCSMotorDriver.cpp b/smarActApp/src/smarActMCSMotorDriver.cpp index bd52cb0..73a9c61 100644 --- a/smarActApp/src/smarActMCSMotorDriver.cpp +++ b/smarActApp/src/smarActMCSMotorDriver.cpp @@ -24,81 +24,88 @@ #include /* Static configuration parameters (compile-time constants) */ -#undef DEBUG +#ifdef DEBUG + #define DBG_PRINTF(...) printf(__VA_ARGS__) +#else + #define DBG_PRINTF(...) +#endif #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*/ +#define FAR_AWAY_LIN 1000000000 /*nm*/ +#define FAR_AWAY_ROT 32767 /*revolutions*/ #define UDEG_PER_REV 360000000 #ifdef __MSC__ /* MSC may not have rint() function */ -#if(_MSC_VER < 1900) +#if (_MSC_VER < 1900) static double rint(double x) { - //middle value point test - if (ceil(x+0.5) == floor(x+0.5)) + // middle value point test + if (ceil(x + 0.5) == floor(x + 0.5)) { int a = (int)ceil(x); - if (a%2 == 0) return ceil(x); - else return floor(x); + if (a % 2 == 0) + return ceil(x); + else + return floor(x); } - else return floor(x+0.5); + else + return floor(x + 0.5); } #endif #endif /* The asyn motor driver apparently can't cope with exceptions */ -#undef ASYN_CANDO_EXCEPTIONS +#undef ASYN_CANDO_EXCEPTIONS /* Define this if exceptions should be thrown and it is OK to abort the application */ -#undef DO_THROW_EXCEPTIONS +#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()); +#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 + 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) + : t_(t) { -va_list ap; - if ( fmt ) { - va_start(ap, fmt); - epicsVsnprintf(str_, sizeof(str_), fmt, ap); - va_end(ap); - } else { - str_[0] = 0; - } + va_list ap; + if (fmt) { + va_start(ap, fmt); + epicsVsnprintf(str_, sizeof(str_), fmt, ap); + va_end(ap); + } + else { + str_[0] = 0; + } }; SmarActMCSException::SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, va_list ap) - : t_(t) + : t_(t) { - epicsVsnprintf(str_, sizeof(str_), fmt, ap); + epicsVsnprintf(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' @@ -114,20 +121,19 @@ SmarActMCSException::SmarActMCSException(SmarActMCSExceptionType t, const char * * - return code zero -> successful ACK or command reply; value * in *val_p. */ -int -SmarActMCSController::parseReply(const char *reply, int *ax_p, int *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; + 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; } /* Parse angle 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' @@ -143,201 +149,248 @@ char cmd[10]; * - return code zero -> successful ACK or command reply; value * in *val_p. */ -int -SmarActMCSController::parseAngle(const char *reply, int *ax_p, int *val_p, int *rot_p) +int SmarActMCSController::parseAngle(const char *reply, int *ax_p, int *val_p, int *rot_p) { -char cmd[10]; - if ( 4 != sscanf(reply, ":%10[A-Z]%i,%i,%i", cmd, ax_p, val_p, rot_p) ) - return -1; - // Will this ever get called? An error response fewer values than an angle response - return 'E' == cmd[0] ? *val_p : 0; + char cmd[10]; + if (4 != sscanf(reply, ":%10[A-Z]%i,%i,%i", cmd, ax_p, val_p, rot_p)) + return -1; + // Will this ever get called? An error response fewer values than an angle response + return 'E' == cmd[0] ? *val_p : 0; } SmarActMCSController::SmarActMCSController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod, int disableSpeed) - : 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) + : 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_); -disableSpeed_ = disableSpeed; -if (disableSpeed_) - epicsPrintf("SmarActMCSController(%s): WARNING - The speed set commands have been disabled for this controller\n", portName); - - 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; axptyp_); + createParam(MCSPtypRbString, asynParamInt32, &this->ptyprb_); + createParam(MCSAutoZeroString, asynParamInt32, &this->autoZero_); + createParam(MCSHoldTimeString, asynParamInt32, &this->holdTime_); + createParam(MCSSclfString, asynParamInt32, &this->sclf_); + createParam(MCSCalString, asynParamInt32, &this->cal_); + + 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); + status = pasynOctetSyncIO->writeRead(asynUserMot_p_, buf, strlen(buf), rep, len, timeout, &nwrite, got_p, &eomReason); - //asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "sendCmd()=%s", buf); - - return status; + //DBG_PRINTF("SmarActMCSController::sendCmd: %s -> %d\n", buf,status); + // 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; + 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; + 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; + 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 'motorClosedLoop_' parameter (which - * maps to the record's CNEN field) - */ -int SmarActMCSAxis::getClosedLoop() +/** Called when asyn clients call pasynInt32->write(). + * Extracts the function and axis number from pasynUser. + * Sets the value in the parameter library. + * For all other functions it calls asynMotorController::writeInt32. + * Calls any registered callbacks for this pasynUser->reason and address. + * \param[in] pasynUser asynUser structure that encodes the reason and address. + * \param[in] value Value to write. */ +asynStatus SmarActMCSController::writeInt32(asynUser *pasynUser, epicsInt32 value) { -int val; - c_p_->getIntegerParam(axisNo_, c_p_->motorClosedLoop_, &val); - return val; + int function = pasynUser->reason; + asynStatus status = asynSuccess; + char rep[REP_LEN]; + int val, ax; + SmarActMCSAxis *pAxis = static_cast(getAxis(pasynUser)); + + if (pAxis == NULL) { + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "SmarActMCSController:writeInt32: error, function: %i. Invalid axis number.\n", + function); + return asynError; + } + + /* Set the parameter and readback in the parameter library. This may be overwritten when we read back the + * status at the end, but that's OK */ + status = setIntegerParam(pAxis->axisNo_, function, value); + + if (function == ptyp_) { + // set positioner type + status = sendCmd(rep, sizeof(rep), ":SST%i,%i", pAxis->channel_, value); + if (status) return status; + if (parseReply(rep, &ax, &val)) return asynError; + pAxis->checkType(); + } + else if (function == cal_) { + // send calibration command + status = sendCmd(rep, sizeof(rep), ":CS%i", pAxis->channel_); + if (status) return status; + if (parseReply(rep, &ax, &val)) return asynError; + } + else if (function == sclf_) { + // set piezo MaxClockFreq + status = sendCmd(rep, sizeof(rep), ":SCLF%i,%i", pAxis->channel_, value); + if (status) return status; + if (parseReply(rep, &ax, &val)) return asynError; + } + else { + /* Call base class method */ + status = asynMotorController::writeInt32(pasynUser, value); + } + + /* Do callbacks so higher layers see any changes */ + callParamCallbacks(pAxis->channel_); + if (status) + asynPrint(pasynUser, ASYN_TRACE_ERROR, + "SmarActMCSController:writeInt32: error, status=%d function=%d, value=%d\n", + status, function, value); + else + asynPrint(pasynUser, ASYN_TRACEIO_DRIVER, + "SmarActMCSController:writeInt32: function=%d, value=%d\n", + function, value); + return status; } -/* - * return 1 if encoder exists. - * return 0 if encoder does not exist + +/* Check if the positioner type set on the controller + * is linear or rotary and set the isRot_ parameter correctly. */ -int SmarActMCSAxis::getEncoder() +void SmarActMCSAxis::checkType() { - int val; - c_p_->getIntegerParam(axisNo_, c_p_->motorStatusHasEncoder_, &val); - return val; + int val; + // Attempt to check linear position, if we receive + // an error, we're a rotary motor. + if ((comStatus_ = getVal("GP", &val))) { + isRot_ = 1; + } + else { + isRot_ = 0; + } + return; } SmarActMCSAxis::SmarActMCSAxis(class SmarActMCSController *cnt_p, int axis, int channel) - : asynMotorAxis(cnt_p, axis), c_p_(cnt_p) + : asynMotorAxis(cnt_p, axis), c_p_(cnt_p) { - int val; - int angle; - int rev; - channel_ = channel; - stepCount_ = 0; // initialize open loop step count to 0. Does it need to be restored from auto save? - asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "SmarActMCSAxis::SmarActMCSAxis -- creating axis %u\n", axis); - if (c_p_->disableSpeed_) - comStatus_ = asynSuccess; - else - 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; - } - - // Attempt to check linear position, if we receive - // an error, we're a rotary motor. - isRot_ = 0; - - if ( (comStatus_ = getVal("GP", &val)) ) { - isRot_ = 1; - } - - // Query the sensor type - if ( (comStatus_ = getVal("GST", &sensorType_)) ) - goto bail; - - if (isRot_ == 1 && asynSuccess == getAngle(&angle, &rev) ) { - setIntegerParam(c_p_->motorStatusHasEncoder_, 1); - setIntegerParam(c_p_->motorStatusGainSupport_, 1); - } - else if (isRot_ == 0 && asynSuccess == getVal("GP",&val) ) { - setIntegerParam(c_p_->motorStatusHasEncoder_, 1); - setIntegerParam(c_p_->motorStatusGainSupport_, 1); - } - + int val; + int angle; + int rev; + channel_ = channel; + + asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "SmarActMCSAxis::SmarActMCSAxis -- creating axis %u\n", axis); + + if (c_p_->disableSpeed_) + comStatus_ = asynSuccess; + else + comStatus_ = getVal("GCLS", &vel_); + DBG_PRINTF("SmarActMCSAxis::SmarActMCSAxis: GCLS %u returned %i\n", axis, comStatus_); + if (comStatus_) + goto bail; + if ((comStatus_ = getVal("GS", &val))) + goto bail; + + setIntegerParam(c_p_->autoZero_, 1); + setIntegerParam(c_p_->holdTime_, 0); + + checkType(); + + // Query the sensor type + if ((comStatus_ = getVal("GST", &sensorType_))) + goto bail; + + if (isRot_ == 1 && asynSuccess == getAngle(&angle, &rev)) { + setIntegerParam(c_p_->motorStatusHasEncoder_, 1); + setIntegerParam(c_p_->motorStatusGainSupport_, 1); + } + else if (isRot_ == 0 && 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(); + setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0); + setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0); - if ( comStatus_ ) { - THROW_(SmarActMCSException(MCSCommunicationError, "SmarActMCSAxis::SmarActMCSAxis -- channel %u ASYN error %i", axis, comStatus_)); - } + 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 @@ -351,17 +404,17 @@ SmarActMCSAxis::SmarActMCSAxis(class SmarActMCSController *cnt_p, int axis, int asynStatus SmarActMCSAxis::getVal(const char *parm_cmd, int *val_p) { -char rep[REP_LEN]; -asynStatus st; -int ax; + char rep[REP_LEN]; + asynStatus st; + int ax; - //asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "getVal() cmd=:%s%u", parm_cmd, this->channel_); + // 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; + // 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; } /* Read the position of rotation stage @@ -374,418 +427,350 @@ int ax; asynStatus SmarActMCSAxis::getAngle(int *val_p, int *rev_p) { -char rep[REP_LEN]; -asynStatus st; -int ax; + char rep[REP_LEN]; + asynStatus st; + int ax; - //asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "getAngle() cmd=:%s%u", parm_cmd, this->channel_); + // asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "getAngle() cmd=:%s%u", parm_cmd, this->channel_); - st = c_p_->sendCmd(rep, sizeof(rep), ":GA%u", this->channel_); - if ( st ) - return st; - return c_p_->parseAngle(rep, &ax, val_p, rev_p) ? asynError: asynSuccess; + st = c_p_->sendCmd(rep, sizeof(rep), ":GA%u", this->channel_); + if (st) + return st; + return c_p_->parseAngle(rep, &ax, val_p, rev_p) ? asynError : asynSuccess; } asynStatus -SmarActMCSAxis::poll(bool* moving_p) +SmarActMCSAxis::poll(bool *moving_p) { - int val; - int angle; - int rev; - enum SmarActMCSStatus status; - - if (getEncoder()) - { - if (isRot_) { - if ((comStatus_ = getAngle(&angle, &rev))) - goto bail; - // Convert angle and revs to total angle - val = rev * UDEG_PER_REV + angle; - } - else { - if ((comStatus_ = getVal("GP", &val))) - goto bail; - } - } - else { - val = stepCount_; - } - 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; - } + epicsInt32 pos; + epicsInt32 val; + epicsInt32 angle; + int rev; + enum SmarActMCSStatus status; + + if (isRot_){ + if ((comStatus_ = getAngle(&angle, &rev))) + goto bail; + // Convert angle and revs to total angle + pos = rev * UDEG_PER_REV + angle; + } + else { + if ((comStatus_ = getVal("GP", (int *)&pos))) + goto bail; + } + setDoubleParam(c_p_->motorEncoderPosition_, (double)pos); + setDoubleParam(c_p_->motorPosition_, (double)pos); + + if ((comStatus_ = getVal("GS", &val))) + goto bail; + + status = (enum SmarActMCSStatus)val; + + switch (status) { + case Stepping: + case Scanning: + case Targeting: + case MoveDelay: + case Calibrating: + case FindRefMark: + *moving_p = true; + break; + + case Holding: + default: + *moving_p = false; + break; + } - setIntegerParam(c_p_->motorStatusDone_, !*moving_p); + 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; - /* 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); - setIntegerParam(c_p_->motorStatusHomed_, val ? 1 : 0); + // Get currently set positioner type + if ((comStatus_ = getVal("GST", &val))) + goto bail; + setIntegerParam(c_p_->ptyprb_, val); -#ifdef DEBUG - printf(" status %u", status); -#endif + if ((comStatus_ = getVal("GST", &val))) + goto bail; bail: - setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0); - setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0); -#ifdef DEBUG - printf("\n"); -#endif + setIntegerParam(c_p_->motorStatusProblem_, comStatus_ ? 1 : 0); + setIntegerParam(c_p_->motorStatusCommsError_, comStatus_ ? 1 : 0); - callParamCallbacks(); + callParamCallbacks(); + //DBG_PRINTF("SmarActMCSAxis::poll: position:%d status:%u", pos,status); - return comStatus_; + 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; - } + 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_; + return comStatus_; } asynStatus SmarActMCSAxis::setSpeed(double velocity) { -long vel; -asynStatus status; - - //ignore set speed commands if flag is set - if(c_p_->disableSpeed_) - return asynSuccess; - - if ( (vel = (long)rint(fabs(velocity))) != vel_ ) { - /* change speed */ - if ( asynSuccess == (status = moveCmd(":SCLS%u,%ld", channel_, vel)) ) { - vel_ = vel; - } - return status; - } - return asynSuccess; + epicsInt32 vel; + asynStatus status; + + // ignore set speed commands if flag is set + if (c_p_->disableSpeed_) + return asynSuccess; + + if ((vel = (epicsInt32)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_rot = relative ? ":MAR%u,%ld,%d,%d" : ":MAA%u,%ld,%d,%d"; - const char* fmt_lin = relative ? ":MPR%u,%ld,%d" : ":MPA%u,%ld,%d"; - const char* fmt_step = ":MST%u,%ld,%d,%d"; // open loop move using step count, amplitude (0-4095; 0V-100V), frequency (1-18500 Hz) - const char* fmt; - const int MAX_FREQ = 18500; // max allowed frequency - const int MAX_VOLTAGE = 100; // max voltage 100V - const double STEP_PER_VOLT = 4095.0/MAX_VOLTAGE; // max voltage index, 4095=100V - double rpos; - long angle; - int rev; -#ifdef DEBUG - printf("Move to %f (speed %f - %f); accel %f\n", position, min_vel, max_vel, accel); -#endif - if (getEncoder()) - { - if (isRot_) { - fmt = fmt_rot; - } - else { - fmt = fmt_lin; - } - - - if ((comStatus_ = setSpeed(max_vel))) - goto bail; - - /* cache 'closed-loop' setting until next move */ - holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; - - rpos = rint(position); - - if (isRot_) { - angle = (long)rpos % UDEG_PER_REV; - rev = (int)(rpos / UDEG_PER_REV); - if (angle < 0) { - angle += UDEG_PER_REV; - rev -= 1; - } - comStatus_ = moveCmd(fmt, channel_, angle, rev, holdTime_); - } - else { - comStatus_ = moveCmd(fmt, channel_, (long)rpos, holdTime_); - } - } - else - { - fmt = fmt_step; - - rpos = rint(position); - if (relative == 0 ) // absolute move - { - int diff = rpos - stepCount_; - stepCount_ = rpos; - rpos = diff; // subtract current step count to produce steps for this move - } - else - { - // relative move. the position value is the number of steps intended - stepCount_ += rpos; - } - // overload min_vel as amplitude - double piezoVoltage = (min_vel > MAX_VOLTAGE) ? (MAX_VOLTAGE) : ((min_vel < 1) ? (1) : (min_vel)); - int amplitude = piezoVoltage * STEP_PER_VOLT; - // overload max_vel as frequency - int frequency = (max_vel> MAX_FREQ) ? (MAX_FREQ) : ((max_vel< 1) ? (1) : (max_vel)); + int holdTime; + const char *fmt_rot = relative ? ":MAR%u,%ld,%d,%d" : ":MAA%u,%ld,%d,%d"; + const char *fmt_lin = relative ? ":MPR%u,%ld,%d" : ":MPA%u,%ld,%d"; + const char *fmt; + long int rpos; + epicsInt32 angle; + int rev; + + if (isRot_) { + fmt = fmt_rot; + } else { + fmt = fmt_lin; + } + + DBG_PRINTF("SmarActMCSAxis::move: pos:%g min_vel:%g max_vel:%g\n", position, min_vel, max_vel); + + if ((comStatus_ = setSpeed(max_vel))) + goto bail; + + rpos = lround(position); + + c_p_->getIntegerParam(axisNo_, c_p_->holdTime_, &holdTime); + + if (isRot_) { + angle = (epicsInt32)rpos % UDEG_PER_REV; + rev = (int)(rpos / UDEG_PER_REV); + if (angle < 0){ + angle += UDEG_PER_REV; + rev -= 1; + } + comStatus_ = moveCmd(fmt, channel_, angle, rev, holdTime); + } else { + comStatus_ = moveCmd(fmt, channel_, rpos, holdTime); + } -#ifdef DEBUG - printf("Open loop Step to %ld (piezo voltage %d ,frequency %d)\n", (long)rpos, amplitude, frequency); -#endif - // overload accel as frequency - comStatus_ = moveCmd(fmt, channel_, (long)rpos, amplitude, frequency); - } bail: - if (comStatus_) { - setIntegerParam(c_p_->motorStatusProblem_, 1); - setIntegerParam(c_p_->motorStatusCommsError_, 1); - callParamCallbacks(); - } - return comStatus_; + 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) { + int holdTime; + int autoZero; + DBG_PRINTF("SmarActMCSAxis::home: forward:%u\n", forwards); -#ifdef DEBUG - printf("Home %u\n", forwards); -#endif - if (getEncoder()) - { - - 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,%d", channel_, forwards ? 0 : 1, holdTime_, isRot_ ? 1 : 0); - } - else - { - // no encoder, can't home. So just set current step count to 0 - stepCount_ = 0; - } + if ((comStatus_ = setSpeed(max_vel))) + goto bail; + + c_p_->getIntegerParam(axisNo_, c_p_->autoZero_, &autoZero); + c_p_->getIntegerParam(axisNo_, c_p_->holdTime_, &holdTime); + + comStatus_ = moveCmd(":FRM%u,%u,%d,%d", channel_, forwards ? 0 : 1, holdTime, autoZero); bail: - if ( comStatus_ ) { - setIntegerParam(c_p_->motorStatusProblem_, 1); - setIntegerParam(c_p_->motorStatusCommsError_, 1); - callParamCallbacks(); - } - return comStatus_; + 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_; + DBG_PRINTF("SmarActMCSAxis::stop:\n"); + 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) { - double rpos; - - rpos = rint(position); - if (getEncoder()) { - if (isRot_) { - // For rotation stages the revolution will always be set to zero - // Only set position if it is between zero an 360 degrees - if (rpos >= 0.0 && rpos < (double)UDEG_PER_REV) { - comStatus_ = moveCmd(":SP%u,%d", channel_, (long)rpos); - } - else { - comStatus_ = asynError; - } - } - else { - comStatus_ = moveCmd(":SP%u,%d", channel_, (long)rpos); - } - } - else - { - stepCount_ = rpos; - } - if ( comStatus_ ) { - setIntegerParam(c_p_->motorStatusProblem_, 1); - setIntegerParam(c_p_->motorStatusCommsError_, 1); - callParamCallbacks(); - } - return comStatus_; + double rpos; + + rpos = rint(position); + + if (isRot_){ + // For rotation stages the revolution will always be set to zero + // Only set position if it is between zero an 360 degrees + if (rpos >= 0.0 && rpos < (double)UDEG_PER_REV) { + comStatus_ = moveCmd(":SP%u,%d", channel_, (epicsInt32)rpos); + } else { + comStatus_ = asynError; + } + } else { + comStatus_ = moveCmd(":SP%u,%d", channel_, (epicsInt32)rpos); + } + + 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; - } + epicsInt32 speed = (epicsInt32)rint(fabs(max_vel)); + epicsInt32 tgt_pos; + signed char dir = 1; + + /* No MCS command we an use directly. Just use a 'relative move' to + * very far target. + */ + + DBG_PRINTF("SmarActMCSAxis::moveVelocity: min_vel:%g max_vel:%g\n", min_vel, max_vel); + + 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 (max_vel < 0){ + dir = -1; + } - if ( (comStatus_ = setSpeed(max_vel)) ) - goto bail; + if ((comStatus_ = setSpeed(max_vel))) + goto bail; - comStatus_ = moveCmd(":MPR%u,%ld,0", channel_, tgt_pos); + if (isRot_){ + tgt_pos = FAR_AWAY_ROT * dir; + comStatus_ = moveCmd(":MAR%u,0,%ld,0", channel_, tgt_pos); + } else { + tgt_pos = FAR_AWAY_LIN * dir; + 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_; + 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 cc_a5 = {"Disable speed cmds [int]", iocshArgInt}; +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 cc_a5 = {"Disable speed cmds [int]", iocshArgInt}; -static const iocshArg * const cc_as[] = {&cc_a0, &cc_a1, &cc_a2, &cc_a3, &cc_a4, &cc_a5}; +static const iocshArg *const cc_as[] = {&cc_a0, &cc_a1, &cc_a2, &cc_a3, &cc_a4, &cc_a5}; -static const iocshFuncDef cc_def = {"smarActMCSCreateController", sizeof(cc_as)/sizeof(cc_as[0]), cc_as}; +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, - int disableSpeed) + const char *motorPortName, + const char *ioPortName, + int numAxes, + double movingPollPeriod, + double idlePollPeriod, + int disableSpeed) { -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...) + 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 { + try { #endif - rval = new SmarActMCSController(motorPortName, ioPortName, numAxes, movingPollPeriod, idlePollPeriod, disableSpeed); + rval = new SmarActMCSController(motorPortName, ioPortName, numAxes, movingPollPeriod, idlePollPeriod, disableSpeed); #ifdef ASYN_CANDO_EXCEPTIONS - } catch (SmarActMCSException &e) { - epicsPrintf("smarActMCSCreateController failed (exception caught):\n%s\n", e.what()); - rval = 0; - } + } catch (SmarActMCSException &e) { + epicsPrintf("smarActMCSCreateController failed (exception caught):\n%s\n", e.what()); + rval = 0; + } #endif - return rval; + 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, - args[5].ival); + smarActMCSCreateController( + args[0].sval, + args[1].sval, + args[2].ival, + args[3].dval, + args[4].dval, + args[5].ival); } +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 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}; +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*/ @@ -793,68 +778,68 @@ static const iocshFuncDef ca_def = {"smarActMCSCreateAxis", 3, ca_as}; extern "C" void * smarActMCSCreateAxis( - const char *controllerPortName, - int axisNumber, - int channel) + const char *controllerPortName, + int axisNumber, + int channel) { -void *rval = 0; + void *rval = 0; -SmarActMCSController *pC; -SmarActMCSAxis *pAxis; -asynMotorAxis *pAsynAxis; + 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...) + // 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 { + 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); + // 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)); + 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(); + 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; - } + } catch (SmarActMCSException &e) { + epicsPrintf("SmarActMCSAxis failed (exception caught):\n%s\n", e.what()); + rval = 0; + } #endif - return rval; + return rval; } static void ca_fn(const iocshArgBuf *args) { - smarActMCSCreateAxis( - args[0].sval, - args[1].ival, - args[2].ival); + 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 + iocshRegister(&cc_def, cc_fn); // smarActMCSCreateController + iocshRegister(&ca_def, ca_fn); // smarActMCSCreateAxis } extern "C" { -epicsExportRegistrar(smarActMCSMotorRegister); + epicsExportRegistrar(smarActMCSMotorRegister); } diff --git a/smarActApp/src/smarActMCSMotorDriver.h b/smarActApp/src/smarActMCSMotorDriver.h index 3a9f7eb..e373c61 100644 --- a/smarActApp/src/smarActMCSMotorDriver.h +++ b/smarActApp/src/smarActMCSMotorDriver.h @@ -13,86 +13,107 @@ #include #include #include +#include + +/** drvInfo strings for extra parameters that the MCS2 controller supports */ +#define MCSPtypString "PTYP" +#define MCSPtypRbString "PTYP_RB" +#define MCSAutoZeroString "AUTO_ZERO" +#define MCSHoldTimeString "HOLD" +#define MCSSclfString "MCLF" +#define MCSCalString "CAL" enum SmarActMCSExceptionType { - MCSUnknownError, - MCSConnectionError, - MCSCommunicationError, + 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_ ;} + 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_; + 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 getAngle(int *val_p, int *rev_p); - virtual asynStatus moveCmd(const char *cmd, ...); - virtual int getClosedLoop(); - int getEncoder(); - - int getVel() const { return vel_; } - + 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 getAngle(int *val_p, int *rev_p); + virtual asynStatus moveCmd(const char *cmd, ...); + virtual void checkType(); + + int getVel() const { return vel_; } + protected: - asynStatus setSpeed(double velocity); + asynStatus setSpeed(double velocity); + private: - SmarActMCSController *c_p_; // pointer to asynMotorController for this axis - asynStatus comStatus_; - int vel_; - unsigned holdTime_; - int channel_; - int sensorType_; - int isRot_; - int stepCount_; // open loop current step count - -friend class SmarActMCSController; + SmarActMCSController *c_p_; // pointer to asynMotorController for this axis + asynStatus comStatus_; + epicsInt32 vel_; + int channel_; + int sensorType_; + int isRot_; + + friend class SmarActMCSController; }; class SmarActMCSController : public asynMotorController { public: - SmarActMCSController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod, int disableSpeed = 0); - 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, ...); + SmarActMCSController(const char *portName, const char *IOPortName, int numAxes, double movingPollPeriod, double idlePollPeriod, int disableSpeed = 0); + 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); + static int parseAngle(const char *reply, int *ax_p, int *val_p, int *rot_p); - static int parseReply(const char *reply, int *ax_p, int *val_p); - static int parseAngle(const char *reply, int *ax_p, int *val_p, int *rot_p); + /* These are the methods that we override from asynMotorDriver */ + asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); protected: - SmarActMCSAxis **pAxes_; + SmarActMCSAxis **pAxes_; private: - asynUser *asynUserMot_p_; - int disableSpeed_; -friend class SmarActMCSAxis; + asynUser *asynUserMot_p_; + int disableSpeed_; + + int ptyp_; /**< positioner type */ +#define FIRST_MCS_PARAM ptyp_ + int ptyprb_; /**< positioner type readback */ + int autoZero_; + int holdTime_; + int sclf_; /**< set maximum closed loop frequency */ + int cal_; /**< calibration command */ +#define LAST_MCS_PARAM cal_ +#define NUM_MCS_PARAMS (&LAST_MCS_PARAM - &FIRST_MCS_PARAM + 1) + + friend class SmarActMCSAxis; }; #endif // _cplusplus diff --git a/smarActApp/src/smarActSCUMotorDriver.cpp b/smarActApp/src/smarActSCUMotorDriver.cpp index 1c021cc..6566c26 100644 --- a/smarActApp/src/smarActSCUMotorDriver.cpp +++ b/smarActApp/src/smarActSCUMotorDriver.cpp @@ -24,63 +24,64 @@ #include /* Static configuration parameters (compile-time constants) */ -#undef DEBUG +#undef DEBUG #define CMD_LEN 50 #define REP_LEN 50 #define DEFAULT_TIMEOUT 2.0 -#define HOLD_FOREVER 60000 -#define HOLD_NEVER 0 -#define FAR_AWAY 1000000000 /*nm*/ -#define UDEG_PER_REV 360000000 +#define HOLD_FOREVER 60000 +#define HOLD_NEVER 0 +#define FAR_AWAY 1000000000 /*nm*/ +#define UDEG_PER_REV 360000000 #define STEPS_PER_EGU 1000. /* The asyn motor driver apparently can't cope with exceptions */ -#undef ASYN_CANDO_EXCEPTIONS +#undef ASYN_CANDO_EXCEPTIONS /* Define this if exceptions should be thrown and it is OK to abort the application */ -#undef DO_THROW_EXCEPTIONS +#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()); +#define THROW_(e) epicsPrintf("%s\n", e.what()); #endif enum SmarActSCUStatus { - Stopped = 0, - AmplSetting = 1, - Moving = 2, - Targeting = 3, - Holding = 4, - Calibrating = 5, - Referencing = 6, - Unknown = 7 + Stopped = 0, + AmplSetting = 1, + Moving = 2, + Targeting = 3, + Holding = 4, + Calibrating = 5, + Referencing = 6, + Unknown = 7 }; static SmarActSCUStatus parseMovingStatus(char statusChar) { - SmarActSCUStatus status = Unknown; - - if ('S' == statusChar) status = Stopped; - else if ('A' == statusChar) status = AmplSetting; - else if ('M' == statusChar) status = Moving; - else if ('T' == statusChar) status = Targeting; - else if ('H' == statusChar) status = Holding; - else if ('C' == statusChar) status = Calibrating; - else if ('R' == statusChar) status = Referencing; - return status; + SmarActSCUStatus status = Unknown; + + if ('S' == statusChar) status = Stopped; + else if ('A' == statusChar) status = AmplSetting; + else if ('M' == statusChar) status = Moving; + else if ('T' == statusChar) status = Targeting; + else if ('H' == statusChar) status = Holding; + else if ('C' == statusChar) status = Calibrating; + else if ('R' == statusChar) status = Referencing; + return status; } SmarActSCUException::SmarActSCUException(SmarActSCUExceptionType t, const char *fmt, ...) - : t_(t) + : t_(t) { -va_list ap; - if ( fmt ) { + va_list ap; + if (fmt) { va_start(ap, fmt); epicsVsnprintf(str_, sizeof(str_), fmt, ap); va_end(ap); - } else { + } + else { str_[0] = 0; } }; @@ -92,41 +93,39 @@ SmarActSCUException::SmarActSCUException(SmarActSCUExceptionType t, const char * } SmarActSCUController::SmarActSCUController(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 + : asynMotorController(portName, numAxes, + 0, // parameters + 0, // interface mask + 0, // interrupt mask + ASYN_CANBLOCK | ASYN_MULTIDEVICE, + 1, // autoconnect + 0, 0) // default priority and stack size { -asynStatus status; -pAxes_ = (SmarActSCUAxis **)(asynMotorController::pAxes_); + asynStatus status; + pAxes_ = (SmarActSCUAxis **)(asynMotorController::pAxes_); status = pasynOctetSyncIO->connect(IOPortName, 0, &pasynUserController_, NULL); - if ( status ) { + if (status) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, "SmarActSCUController:SmarActSCUController: cannot connect to SCU controller\n"); THROW_(SmarActSCUException(SCUConnectionError, "SmarActSCUController: unable to connect serial channel")); } - startPoller( movingPollPeriod, idlePollPeriod, 0 ); - + startPoller(movingPollPeriod, idlePollPeriod, 0); } /* Obtain value of the 'motorClosedLoop_' parameter (which * maps to the record's CNEN field) */ -int -SmarActSCUAxis::getClosedLoop() +int SmarActSCUAxis::getClosedLoop() { -int val; + int val; pC_->getIntegerParam(axisNo_, pC_->motorClosedLoop_, &val); return val; } SmarActSCUAxis::SmarActSCUAxis(class SmarActSCUController *cnt_p, int axis, int channel) - : asynMotorAxis(cnt_p, axis), pC_(cnt_p) + : asynMotorAxis(cnt_p, axis), pC_(cnt_p) { char moveStatus; double currentPosition; @@ -139,36 +138,37 @@ SmarActSCUAxis::SmarActSCUAxis(class SmarActSCUController *cnt_p, int axis, int #ifdef DEBUG printf("GCLF%u returned %i\n", axis, comStatus_); #endif - if ( comStatus_ ) + if (comStatus_) goto bail; - if ( (comStatus_ = getCharVal("M", &moveStatus)) ) + if ((comStatus_ = getCharVal("M", &moveStatus))) goto bail; - if ( 'H' == moveStatus ) { + if ('H' == moveStatus) { // 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 { + } + else { // initial value from 'closed-loop' property holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; } // Query the sensor type - if ( (comStatus_ = getIntegerVal("GST", &positionerType_)) ) + if ((comStatus_ = getIntegerVal("GST", &positionerType_))) goto bail; - // Determine if stage is a rotation stage + // Determine if stage is a rotation stage if (positionerType_ == 2 || - positionerType_ == 8 || - positionerType_ == 14 || - positionerType_ == 20 || - positionerType_ == 22 || - positionerType_ == 23 || + positionerType_ == 8 || + positionerType_ == 14 || + positionerType_ == 20 || + positionerType_ == 22 || + positionerType_ == 23 || (positionerType_ >= 25 && positionerType_ <= 29)) { - isRot_ = 1; - if ( asynSuccess == getAngle(¤tPosition, &rev) ) { + isRot_ = 1; + if (asynSuccess == getAngle(¤tPosition, &rev)) { setIntegerParam(pC_->motorStatusHasEncoder_, 1); setIntegerParam(pC_->motorStatusGainSupport_, 1); } @@ -181,34 +181,32 @@ SmarActSCUAxis::SmarActSCUAxis(class SmarActSCUController *cnt_p, int axis, int } } - bail: setIntegerParam(pC_->motorStatusProblem_, comStatus_ ? 1 : 0); setIntegerParam(pC_->motorStatusCommsError_, comStatus_ ? 1 : 0); callParamCallbacks(); - if ( comStatus_ ) { + if (comStatus_) { THROW_(SmarActSCUException(SCUCommunicationError, "SmarActSCUAxis::SmarActSCUAxis -- channel %u ASYN error %i", axis, comStatus_)); } - } /* Send a command to the controller and read the response. * Uses the toController_ and fromController_ strings in asynMotorController. - * + * * RETURNS: asynError if an error occurred, asynSuccess otherwise. */ asynStatus SmarActSCUAxis::sendCmd() { size_t replyLen; asynStatus status; - + status = pC_->writeReadController(toController_, fromController_, sizeof(fromController_), &replyLen, DEFAULT_TIMEOUT); if (status) asynPrint(pasynUser_, ASYN_TRACE_ERROR, "ERROR: sendCmd: status=%d, sent: %s, received: %s\n", status, toController_, fromController_); else - asynPrint(pasynUser_, ASYN_TRACEIO_DRIVER, "sendCmd: status=%d, sent: %s, received: %s\n", status, toController_, fromController_); + asynPrint(pasynUser_, ASYN_TRACEIO_DRIVER, "sendCmd: status=%d, sent: %s, received: %s\n", status, toController_, fromController_); return status; } @@ -216,7 +214,7 @@ asynStatus SmarActSCUAxis::sendCmd() * * parm_cmd: SCU command (w/o ':' char) to read parameter * val_p: where to store the value returned by the SCU - * + * * RETURNS: asynError if an error occurred, asynSuccess otherwise. */ asynStatus @@ -245,7 +243,7 @@ SmarActSCUAxis::getIntegerVal(const char *parm_cmd, int *val_p) * * parm_cmd: SCU command (w/o ':' char) to read parameter * val_p: where to store the value returned by the SCU - * + * * RETURNS: asynError if an error occurred, asynSuccess otherwise. */ asynStatus @@ -273,7 +271,7 @@ SmarActSCUAxis::getDoubleVal(const char *parm_cmd, double *val_p) * * parm_cmd: SCU command (w/o ':' char) to read parameter * val_p: where to store the value returned by the SCU - * + * * RETURNS: asynError if an error occurred, asynSuccess otherwise. */ asynStatus @@ -300,14 +298,14 @@ SmarActSCUAxis::getCharVal(const char *parm_cmd, char *val_p) * * parm_cmd: SCU command (w/o ':' char) to read parameter * val_p: where to store the value returned by the SCU - * + * * RETURNS: asynError if an error occurred, asynSuccess otherwise. */ asynStatus SmarActSCUAxis::getAngle(double *val_p, int *rev_p) { asynStatus status; - int axis; + int axis; epicsSnprintf(toController_, sizeof(toController_), ":GA%u", this->channel_); status = sendCmd(); @@ -325,12 +323,12 @@ SmarActSCUAxis::getAngle(double *val_p, int *rev_p) asynStatus SmarActSCUAxis::poll(bool *moving_p) { -double doubleVal; -int integerVal; -char charVal; -double angle; -int rev; -enum SmarActSCUStatus movingStatus; + double doubleVal; + int integerVal; + char charVal; + double angle; + int rev; + enum SmarActSCUStatus movingStatus; if (isRot_) { if ((comStatus_ = getAngle(&angle, &rev))) @@ -343,8 +341,8 @@ enum SmarActSCUStatus movingStatus; goto bail; } - setDoubleParam(pC_->motorEncoderPosition_, (doubleVal+positionOffset_)*STEPS_PER_EGU); - setDoubleParam(pC_->motorPosition_, (doubleVal+positionOffset_)*STEPS_PER_EGU); + setDoubleParam(pC_->motorEncoderPosition_, (doubleVal + positionOffset_) * STEPS_PER_EGU); + setDoubleParam(pC_->motorPosition_, (doubleVal + positionOffset_) * STEPS_PER_EGU); #ifdef DEBUG printf("POLL (position %f)", doubleVal); #endif @@ -355,45 +353,44 @@ enum SmarActSCUStatus movingStatus; movingStatus = parseMovingStatus(charVal); switch (movingStatus) { - default: - *moving_p = false; + 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; + /* 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 Targeting: - case Moving: - case Calibrating: - case Referencing: - *moving_p = true; + case Targeting: + case Moving: + case Calibrating: + case Referencing: + *moving_p = true; break; } - setIntegerParam(pC_->motorStatusDone_, ! *moving_p ); - + setIntegerParam(pC_->motorStatusDone_, !*moving_p); /* Check if the sensor 'knows' absolute position and * update the MSTA 'HOMED' bit. */ - if ((comStatus_ = getIntegerVal("GPPK", &integerVal))) + if ((comStatus_ = getIntegerVal("GPPK", &integerVal))) goto bail; - setIntegerParam(pC_->motorStatusHomed_, integerVal ? 1 : 0 ); + setIntegerParam(pC_->motorStatusHomed_, integerVal ? 1 : 0); #ifdef DEBUG printf(" status %u", status); #endif bail: - setIntegerParam(pC_->motorStatusProblem_, comStatus_ ? 1 : 0 ); - setIntegerParam(pC_->motorStatusCommsError_, comStatus_ ? 1 : 0 ); + setIntegerParam(pC_->motorStatusProblem_, comStatus_ ? 1 : 0); + setIntegerParam(pC_->motorStatusCommsError_, comStatus_ ? 1 : 0); #ifdef DEBUG printf("\n"); #endif @@ -403,7 +400,7 @@ enum SmarActSCUStatus movingStatus; return comStatus_; } -asynStatus +asynStatus SmarActSCUAxis::move(double position, int relative, double min_vel, double max_vel, double accel) { double rpos; @@ -415,11 +412,11 @@ SmarActSCUAxis::move(double position, int relative, double min_vel, double max_v #endif /* cache 'closed-loop' setting until next move */ - holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; rpos = (position / STEPS_PER_EGU) - positionOffset_; - if ( isRot_ ) { + if (isRot_) { angle = (long)rpos % UDEG_PER_REV; rev = (int)(rpos / UDEG_PER_REV); if (angle < 0) { @@ -434,7 +431,8 @@ SmarActSCUAxis::move(double position, int relative, double min_vel, double max_v epicsSnprintf(toController_, sizeof(toController_), ":MAA%uA%.3fR%dH%d:GP%u", this->channel_, angle, rev, holdTime_, this->channel_); comStatus_ = sendCmd(); } - } else { + } + else { if (relative) { epicsSnprintf(toController_, sizeof(toController_), ":MPR%uP%.3fH%d:GP%u", this->channel_, rpos, holdTime_, this->channel_); comStatus_ = sendCmd(); @@ -461,7 +459,7 @@ SmarActSCUAxis::home(double min_vel, double max_vel, double accel, int forwards) #endif /* cache 'closed-loop' setting until next move */ - holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; epicsSnprintf(toController_, sizeof(toController_), ":MTR%uH%dZ0:GP%u", this->channel_, holdTime_, this->channel_); comStatus_ = sendCmd(); @@ -503,7 +501,7 @@ SmarActSCUAxis::setPosition(double position) asynStatus SmarActSCUAxis::moveVelocity(double min_vel, double max_vel, double accel) { -double tgt_pos = FAR_AWAY; + double tgt_pos = FAR_AWAY; /* No SCU command we an use directly. Just use a 'relative move' to * very far target. @@ -513,37 +511,37 @@ double tgt_pos = FAR_AWAY; printf("moveVelocity (%f - %f)\n", min_vel, max_vel); #endif - if ( 0 == max_vel ) { + if (0 == max_vel) { return this->stop(0.); } - if ( max_vel < 0 ) { - tgt_pos = -tgt_pos; + if (max_vel < 0) { + tgt_pos = -tgt_pos; } return this->move(tgt_pos, 1, 0, max_vel, accel); } /* 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 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 iocshArg *const cc_as[] = {&cc_a0, &cc_a1, &cc_a2, &cc_a3, &cc_a4}; -static const iocshFuncDef cc_def = {"smarActSCUCreateController", sizeof(cc_as)/sizeof(cc_as[0]), cc_as}; +static const iocshFuncDef cc_def = {"smarActSCUCreateController", sizeof(cc_as) / sizeof(cc_as[0]), cc_as}; extern "C" void * smarActSCUCreateController( - const char *motorPortName, - const char *ioPortName, - int numAxes, - double movingPollPeriod, - double idlePollPeriod) + const char *motorPortName, + const char *ioPortName, + int numAxes, + double movingPollPeriod, + double idlePollPeriod) { -void *rval = 0; + 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...) @@ -552,7 +550,8 @@ void *rval = 0; #endif rval = new SmarActSCUController(motorPortName, ioPortName, numAxes, movingPollPeriod, idlePollPeriod); #ifdef ASYN_CANDO_EXCEPTIONS - } catch (SmarActSCUException &e) { + } + catch (SmarActSCUException &e) { epicsPrintf("smarActSCUCreateController failed (exception caught):\n%s\n", e.what()); rval = 0; } @@ -564,19 +563,18 @@ void *rval = 0; static void cc_fn(const iocshArgBuf *args) { smarActSCUCreateController( - args[0].sval, - args[1].sval, - args[2].ival, - args[3].dval, - args[4].dval); + 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}; +static const iocshArg *const ca_as[] = {&ca_a0, &ca_a1, &ca_a2}; /* iocsh wrapping and registration business (stolen from ACRMotorDriver.cpp) */ /* smarActSCUCreateAxis called to create each axis of the smarActSCU controller*/ @@ -584,14 +582,14 @@ static const iocshFuncDef ca_def = {"smarActSCUCreateAxis", 3, ca_as}; extern "C" void * smarActSCUCreateAxis( - const char *controllerPortName, - int axisNumber, - int channel) + const char *controllerPortName, + int axisNumber, + int channel) { -void *rval = 0; + void *rval = 0; -SmarActSCUController *pC; -asynMotorAxis *pAsynAxis; + SmarActSCUController *pC; + 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 @@ -599,8 +597,8 @@ asynMotorAxis *pAsynAxis; #ifdef ASYN_CANDO_EXCEPTIONS try { #endif -// rval = new SmarActSCUAxis(, axisNumber, channel); - pC = (SmarActSCUController*) findAsynPortDriver(controllerPortName); + // rval = new SmarActSCUAxis(, axisNumber, channel); + pC = (SmarActSCUController *)findAsynPortDriver(controllerPortName); if (!pC) { printf("smarActSCUCreateAxis: Error port %s not found\n", controllerPortName); rval = 0; @@ -621,7 +619,8 @@ asynMotorAxis *pAsynAxis; pC->unlock(); #ifdef ASYN_CANDO_EXCEPTIONS - } catch (SmarActSCUException &e) { + } + catch (SmarActSCUException &e) { epicsPrintf("SmarActSCUAxis failed (exception caught):\n%s\n", e.what()); rval = 0; } @@ -633,17 +632,17 @@ asynMotorAxis *pAsynAxis; static void ca_fn(const iocshArgBuf *args) { smarActSCUCreateAxis( - args[0].sval, - args[1].ival, - args[2].ival); + args[0].sval, + args[1].ival, + args[2].ival); } static void smarActSCUMotorRegister(void) { - iocshRegister(&cc_def, cc_fn); // smarActSCUCreateController - iocshRegister(&ca_def, ca_fn); // smarActSCUCreateAxis + iocshRegister(&cc_def, cc_fn); // smarActSCUCreateController + iocshRegister(&ca_def, ca_fn); // smarActSCUCreateAxis } extern "C" { -epicsExportRegistrar(smarActSCUMotorRegister); + epicsExportRegistrar(smarActSCUMotorRegister); } diff --git a/smarActApp/src/smarActSCUMotorDriver.h b/smarActApp/src/smarActSCUMotorDriver.h index d8e904c..2bab38a 100644 --- a/smarActApp/src/smarActSCUMotorDriver.h +++ b/smarActApp/src/smarActSCUMotorDriver.h @@ -24,38 +24,38 @@ class SmarActSCUException : public std::exception { public: SmarActSCUException(SmarActSCUExceptionType t, const char *fmt, ...); SmarActSCUException(SmarActSCUExceptionType t) - : t_(t) - { str_[0] = 0; } + : t_(t) + { str_[0] = 0; } SmarActSCUException() - : t_(SCUUnknownError) - { str_[0] = 0; } + : t_(SCUUnknownError) + { str_[0] = 0; } SmarActSCUException(SmarActSCUExceptionType t, const char *fmt, va_list ap); SmarActSCUExceptionType getType() - const { return t_; } + const { return t_; } virtual const char *what() - const throw() {return str_ ;} + const throw() { return str_; } + protected: - char str_[100]; + char str_[100]; SmarActSCUExceptionType t_; }; - class SmarActSCUAxis : public asynMotorAxis { public: SmarActSCUAxis(class SmarActSCUController *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); + 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 getCharVal(const char *parm, char *val_p); virtual asynStatus getIntegerVal(const char *parm, int *val_p); virtual asynStatus getDoubleVal(const char *parm, double *val_p); virtual asynStatus getAngle(double *val_p, int *rev_p); - virtual int getClosedLoop(); + virtual int getClosedLoop(); int getMaxFreq() const { return maxFreq_; } @@ -63,19 +63,19 @@ class SmarActSCUAxis : public asynMotorAxis asynStatus setSpeed(double velocity); private: - SmarActSCUController *pC_; // pointer to asynMotorController for this axis - asynStatus comStatus_; - int maxFreq_; - unsigned holdTime_; - int channel_; - int positionerType_; - int isRot_; - double positionOffset_; - asynStatus sendCmd(); + SmarActSCUController *pC_; // pointer to asynMotorController for this axis + asynStatus comStatus_; + int maxFreq_; + unsigned holdTime_; + int channel_; + int positionerType_; + int isRot_; + double positionOffset_; + asynStatus sendCmd(); char toController_[MAX_CONTROLLER_STRING_SIZE]; char fromController_[MAX_CONTROLLER_STRING_SIZE]; -friend class SmarActSCUController; + friend class SmarActSCUController; }; class SmarActSCUController : public asynMotorController @@ -90,7 +90,7 @@ class SmarActSCUController : public asynMotorController SmarActSCUAxis **pAxes_; private: -friend class SmarActSCUAxis; + friend class SmarActSCUAxis; }; #endif // _cplusplus