From b1859453bb1279e04faa08881019fd7746827315 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Mon, 11 Sep 2023 13:34:41 +0200 Subject: [PATCH] merge features of MCS driver that were lost from version 1.5.0 --- smarActApp/src/smarActMCS2MotorDriver.cpp | 37 +- smarActApp/src/smarActMCS2MotorDriver.h | 10 +- smarActApp/src/smarActMCSMotorDriver.cpp | 463 +++++++++++----------- smarActApp/src/smarActMCSMotorDriver.h | 39 +- 4 files changed, 278 insertions(+), 271 deletions(-) diff --git a/smarActApp/src/smarActMCS2MotorDriver.cpp b/smarActApp/src/smarActMCS2MotorDriver.cpp index 9b5a1f2..1b367f7 100644 --- a/smarActApp/src/smarActMCS2MotorDriver.cpp +++ b/smarActApp/src/smarActMCS2MotorDriver.cpp @@ -23,6 +23,12 @@ 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. @@ -46,13 +52,12 @@ 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(MCS2CalString, asynParamInt32, &this->cal_); + createParam(MCS2MclfString, asynParamInt32, &this->mclf_); createParam(MCS2HoldString, asynParamInt32, &this->hold_); - + createParam(MCS2CalString, asynParamInt32, &this->cal_); // Connect to MCS2 controller status = pasynOctetSyncIO->connect(MCS2PortName, 0, &pasynUserController_, NULL); @@ -215,16 +220,16 @@ asynStatus MCS2Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) * status at the end, but that's OK */ status = setIntegerParam(pAxis->axisNo_, function, value); - 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 == ptyp_) { + if (function == ptyp_) { // set positioner type sprintf(pAxis->pC_->outString_, ":CHAN%d:PTYP %d", pAxis->axisNo_, value); status = pAxis->pC_->writeController(); } + 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 sprintf(pAxis->pC_->outString_, ":CAL%d", pAxis->axisNo_); @@ -356,7 +361,9 @@ asynStatus MCS2Axis::move(double position, int relative, double minVelocity, dou if(hasEnc) { - printf("MCS2Axis::move closeloop: position: %g rel: %d\n",position,relative); + 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 @@ -377,7 +384,7 @@ asynStatus MCS2Axis::move(double position, int relative, double minVelocity, dou if(!relative) position=position-curPos; curPos+=position; - printf("MCS2Axis::move openloop: new_pos: %g rel_move: %g\n",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); @@ -385,8 +392,10 @@ asynStatus MCS2Axis::move(double position, int relative, double minVelocity, dou sprintf(pC_->outString_, ":CHAN%d:MMOD %d", channel_, 4); status = pC_->writeController(); - sprintf(pC_->outString_, ":MOVE%d %f", channel_, position * PULSES_PER_STEP); + DBG_PRINTF("MCS2Axis::move: %s ->status:%d\n",pC_->outString_,status); + sprintf(pC_->outString_, ":MOVE%d %f", channel_, position); status = pC_->writeController(); + DBG_PRINTF("MCS2Axis::move: %s ->status:%d\n",pC_->outString_,status); } return status; } @@ -403,7 +412,7 @@ asynStatus MCS2Axis::home(double minVelocity, double maxVelocity, double acceler // Set default reference options - direction and autozero sprintf(pC_->outString_, ":CHAN%d:REF:OPT %d", channel_, refOpt); - printf("MCS2Axis::home: %s",pC_->outString_); + DBG_PRINTF("MCS2Axis::home: %s",pC_->outString_); status = pC_->writeController(); pC_->clearErrors(); @@ -438,7 +447,7 @@ asynStatus MCS2Axis::setPosition(double position) asynStatus status = asynSuccess; sprintf(pC_->outString_, ":CHAN%d:POS %f", channel_, position * PULSES_PER_STEP); - printf("MCS2Axis::setPosition: %s",pC_->outString_); + DBG_PRINTF("MCS2Axis::setPosition: %s",pC_->outString_); status = pC_->writeController(); return status; } diff --git a/smarActApp/src/smarActMCS2MotorDriver.h b/smarActApp/src/smarActMCS2MotorDriver.h index aa03f3b..c54238e 100644 --- a/smarActApp/src/smarActMCS2MotorDriver.h +++ b/smarActApp/src/smarActMCS2MotorDriver.h @@ -67,12 +67,12 @@ const unsigned short STOP_ON_REF_FOUND = 0x0020; #define HOLD_FOREVER 0xffffffff /** drvInfo strings for extra parameters that the MCS2 controller supports */ -#define MCS2MclfString "MCLF" #define MCS2PtypString "PTYP" #define MCS2PtypRbString "PTYP_RB" #define MCS2PstatString "PSTAT" -#define MCS2CalString "CAL" +#define MCS2MclfString "MCLF" #define MCS2HoldString "HOLD" +#define MCS2CalString "CAL" class epicsShareClass MCS2Axis : public asynMotorAxis { @@ -109,13 +109,13 @@ class epicsShareClass MCS2Controller : public asynMotorController MCS2Axis *getAxis(int axisNo); protected: - int mclf_; /**< MCL frequency */ -#define FIRST_MCS2_PARAM mclf_ int ptyp_; /**< positioner type */ +#define FIRST_MCS2_PARAM ptyp_ int ptyprb_; /**< positioner type readback */ int pstatrb_; /**< positoner status word readback */ - 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) diff --git a/smarActApp/src/smarActMCSMotorDriver.cpp b/smarActApp/src/smarActMCSMotorDriver.cpp index 4ce4a2b..54ca434 100644 --- a/smarActApp/src/smarActMCSMotorDriver.cpp +++ b/smarActApp/src/smarActMCSMotorDriver.cpp @@ -24,15 +24,18 @@ #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__ @@ -44,13 +47,10 @@ static double rint(double x) 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 @@ -86,8 +86,7 @@ SmarActMCSException::SmarActMCSException(SmarActMCSExceptionType t, const char * va_start(ap, fmt); epicsVsnprintf(str_, sizeof(str_), fmt, ap); va_end(ap); - } - else { + } else { str_[0] = 0; } }; @@ -156,15 +155,14 @@ int SmarActMCSController::parseAngle(const char *reply, int *ax_p, int *val_p, i } 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]; @@ -175,11 +173,17 @@ SmarActMCSController::SmarActMCSController(const char *portName, const char *IOP if (disableSpeed_) epicsPrintf("SmarActMCSController(%s): WARNING - The speed set commands have been disabled for this controller\n", portName); + createParam(MCSPtypString, asynParamInt32, &this->ptyp_); + 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 ) { - { + if (status) { asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, - "SmarActMCSController:SmarActMCSController: cannot connect to MCS controller\n"); + "SmarActMCSController:SmarActMCSController: cannot connect to MCS controller\n"); THROW_(SmarActMCSException(MCSConnectionError, "SmarActMCSController: unable to connect serial channel")); } @@ -187,7 +191,7 @@ SmarActMCSController::SmarActMCSController(const char *portName, const char *IOP // 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 ) { + 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); } @@ -219,8 +223,8 @@ SmarActMCSController::sendCmd(size_t *got_p, char *rep, int len, double timeout, status = pasynOctetSyncIO->writeRead(asynUserMot_p_, buf, strlen(buf), rep, len, timeout, &nwrite, got_p, &eomReason); + //DBG_PRINTF("SmarActMCSController::sendCmd: %s -> %d\n", buf,status); // asynPrint(c_p_->pasynUserSelf, ASYN_TRACEIO_DRIVER, "sendCmd()=%s", buf); - return status; } @@ -256,76 +260,120 @@ asynStatus SmarActMCSController::sendCmd(char *rep, int len, const char *fmt, .. 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; + // 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 + DBG_PRINTF("SmarActMCSAxis::SmarActMCSAxis: GCLS %u returned %i\n", axis, comStatus_); 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; - } + setIntegerParam(c_p_->autoZero_, 1); + setIntegerParam(c_p_->holdTime_, 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; - } + checkType(); // Query the sensor type if ((comStatus_ = getVal("GST", &sensorType_))) goto bail; - if (isRot_ == 1 && asynSuccess == getAngle(&angle, &rev)) { + 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)) { + else if (isRot_ == 0 && asynSuccess == getVal("GP", &val)) { setIntegerParam(c_p_->motorStatusHasEncoder_, 1); setIntegerParam(c_p_->motorStatusGainSupport_, 1); } @@ -336,7 +384,7 @@ SmarActMCSAxis::SmarActMCSAxis(class SmarActMCSController *cnt_p, int axis, int callParamCallbacks(); - if (comStatus_) { + if (comStatus_) { THROW_(SmarActMCSException(MCSCommunicationError, "SmarActMCSAxis::SmarActMCSAxis -- channel %u ASYN error %i", axis, comStatus_)); } } @@ -390,32 +438,24 @@ SmarActMCSAxis::getAngle(int *val_p, int *rev_p) asynStatus SmarActMCSAxis::poll(bool *moving_p) { - int val; - int angle; + epicsInt32 pos; + epicsInt32 val; + epicsInt32 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; - } + if (isRot_){ + if ((comStatus_ = getAngle(&angle, &rev))) + goto bail; + // Convert angle and revs to total angle + pos = rev * UDEG_PER_REV + angle; } else { - val = stepCount_; + if ((comStatus_ = getVal("GP", (int *)&pos))) + goto bail; } - setDoubleParam(c_p_->motorEncoderPosition_, (double)val); - setDoubleParam(c_p_->motorPosition_, (double)val); -#ifdef DEBUG - printf("POLL (position %d)", val); -#endif + setDoubleParam(c_p_->motorEncoderPosition_, (double)pos); + setDoubleParam(c_p_->motorPosition_, (double)pos); if ((comStatus_ = getVal("GS", &val))) goto bail; @@ -423,19 +463,6 @@ SmarActMCSAxis::poll(bool *moving_p) 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: @@ -444,6 +471,11 @@ SmarActMCSAxis::poll(bool *moving_p) case FindRefMark: *moving_p = true; break; + + case Holding: + default: + *moving_p = false; + break; } setIntegerParam(c_p_->motorStatusDone_, !*moving_p); @@ -456,18 +488,20 @@ SmarActMCSAxis::poll(bool *moving_p) setIntegerParam(c_p_->motorStatusHomed_, val ? 1 : 0); -#ifdef DEBUG - printf(" status %u", status); -#endif + // Get currently set positioner type + if ((comStatus_ = getVal("GST", &val))) + goto bail; + setIntegerParam(c_p_->ptyprb_, val); + + 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 callParamCallbacks(); + //DBG_PRINTF("SmarActMCSAxis::poll: position:%d status:%u", pos,status); return comStatus_; } @@ -494,9 +528,6 @@ SmarActMCSAxis::moveCmd(const char *fmt, ...) } bail: -#ifdef DEBUG - printf("\n"); -#endif return comStatus_; } @@ -504,14 +535,14 @@ SmarActMCSAxis::moveCmd(const char *fmt, ...) asynStatus SmarActMCSAxis::setSpeed(double velocity) { - long vel; + epicsInt32 vel; asynStatus status; // ignore set speed commands if flag is set if (c_p_->disableSpeed_) return asynSuccess; - if ((vel = (long)rint(fabs(velocity))) != vel_) { + if ((vel = (epicsInt32)rint(fabs(velocity))) != vel_) { /* change speed */ if (asynSuccess == (status = moveCmd(":SCLS%u,%ld", channel_, vel))) { vel_ = vel; @@ -524,79 +555,43 @@ SmarActMCSAxis::setSpeed(double velocity) asynStatus SmarActMCSAxis::move(double position, int relative, double min_vel, double max_vel, double accel) { + 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_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; + long int rpos; + epicsInt32 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; + 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; - /* cache 'closed-loop' setting until next move */ - holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + rpos = lround(position); - rpos = rint(position); + c_p_->getIntegerParam(axisNo_, c_p_->holdTime_, &holdTime); - 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_); + 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); } - 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)); -#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_) { + if (comStatus_){ setIntegerParam(c_p_->motorStatusProblem_, 1); setIntegerParam(c_p_->motorStatusCommsError_, 1); callParamCallbacks(); @@ -607,26 +602,17 @@ SmarActMCSAxis::move(double position, int relative, double min_vel, double max_v 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; + if ((comStatus_ = setSpeed(max_vel))) + goto bail; - /* cache 'closed-loop' setting until next move */ - holdTime_ = getClosedLoop() ? HOLD_FOREVER : 0; + 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_, isRot_ ? 1 : 0); - } - else - { - // no encoder, can't home. So just set current step count to 0 - stepCount_ = 0; - } + comStatus_ = moveCmd(":FRM%u,%u,%d,%d", channel_, forwards ? 0 : 1, holdTime, autoZero); bail: if (comStatus_) { @@ -640,9 +626,7 @@ SmarActMCSAxis::home(double min_vel, double max_vel, double accel, int forwards) asynStatus SmarActMCSAxis::stop(double acceleration) { -#ifdef DEBUG - printf("Stop\n"); -#endif + DBG_PRINTF("SmarActMCSAxis::stop:\n"); comStatus_ = moveCmd(":S%u", channel_); if (comStatus_) { @@ -659,25 +643,19 @@ 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); + + 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); } - else - { - stepCount_ = rpos; - } + if (comStatus_) { setIntegerParam(c_p_->motorStatusProblem_, 1); setIntegerParam(c_p_->motorStatusCommsError_, 1); @@ -689,18 +667,17 @@ SmarActMCSAxis::setPosition(double position) asynStatus SmarActMCSAxis::moveVelocity(double min_vel, double max_vel, double accel) { - long speed = (long)rint(fabs(max_vel)); - long tgt_pos = FAR_AWAY; + 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. */ -#ifdef DEBUG - printf("moveVelocity (%f - %f)\n", min_vel, max_vel); -#endif + DBG_PRINTF("SmarActMCSAxis::moveVelocity: min_vel:%g max_vel:%g\n", min_vel, max_vel); - if (0 == speed) { + 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. @@ -711,14 +688,20 @@ SmarActMCSAxis::moveVelocity(double min_vel, double max_vel, double accel) return asynSuccess; } - if (max_vel < 0) { - tgt_pos = -tgt_pos; + if (max_vel < 0){ + dir = -1; } 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_) { @@ -743,12 +726,12 @@ static const iocshFuncDef cc_def = {"smarActMCSCreateController", sizeof(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 @@ -759,8 +742,7 @@ smarActMCSCreateController( #endif rval = new SmarActMCSController(motorPortName, ioPortName, numAxes, movingPollPeriod, idlePollPeriod, disableSpeed); #ifdef ASYN_CANDO_EXCEPTIONS - } - catch (SmarActMCSException &e) { + } catch (SmarActMCSException &e) { epicsPrintf("smarActMCSCreateController failed (exception caught):\n%s\n", e.what()); rval = 0; } @@ -772,12 +754,12 @@ smarActMCSCreateController( 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); + 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}; @@ -792,14 +774,14 @@ 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; SmarActMCSController *pC; - // SmarActMCSAxis *pAxis; + //SmarActMCSAxis *pAxis; asynMotorAxis *pAsynAxis; // the asyn stuff doesn't seem to be prepared for exceptions. I get segfaults @@ -826,13 +808,12 @@ smarActMCSCreateAxis( return rval; } pC->lock(); - /*pAxis =*/new SmarActMCSAxis(pC, axisNumber, channel); - // pAxis = NULL; + /*pAxis =*/ new SmarActMCSAxis(pC, axisNumber, channel); + //pAxis = NULL; pC->unlock(); #ifdef ASYN_CANDO_EXCEPTIONS - } - catch (SmarActMCSException &e) { + } catch (SmarActMCSException &e) { epicsPrintf("SmarActMCSAxis failed (exception caught):\n%s\n", e.what()); rval = 0; } @@ -844,9 +825,9 @@ smarActMCSCreateAxis( static void ca_fn(const iocshArgBuf *args) { smarActMCSCreateAxis( - args[0].sval, - args[1].ival, - args[2].ival); + args[0].sval, + args[1].ival, + args[2].ival); } static void smarActMCSMotorRegister(void) diff --git a/smarActApp/src/smarActMCSMotorDriver.h b/smarActApp/src/smarActMCSMotorDriver.h index e23470d..e373c61 100644 --- a/smarActApp/src/smarActMCSMotorDriver.h +++ b/smarActApp/src/smarActMCSMotorDriver.h @@ -13,6 +13,15 @@ #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, @@ -20,21 +29,20 @@ enum SmarActMCSExceptionType { MCSCommunicationError, }; -class SmarActMCSException : public std::exception -{ +class SmarActMCSException : public std::exception { public: SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, ...); SmarActMCSException(SmarActMCSExceptionType t) - : t_(t) + : t_(t) { str_[0] = 0; } SmarActMCSException() - : t_(MCSUnknownError) + : t_(MCSUnknownError) { str_[0] = 0; } SmarActMCSException(SmarActMCSExceptionType t, const char *fmt, va_list ap); SmarActMCSExceptionType getType() - const { return t_; } + const { return t_; } virtual const char *what() - const throw() { return str_; } + const throw() { return str_; } protected: char str_[100]; @@ -55,8 +63,7 @@ class SmarActMCSAxis : public asynMotorAxis 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(); + virtual void checkType(); int getVel() const { return vel_; } @@ -66,12 +73,10 @@ class SmarActMCSAxis : public asynMotorAxis private: SmarActMCSController *c_p_; // pointer to asynMotorController for this axis asynStatus comStatus_; - int vel_; - unsigned holdTime_; + epicsInt32 vel_; int channel_; int sensorType_; int isRot_; - int stepCount_; // open loop current step count friend class SmarActMCSController; }; @@ -89,6 +94,7 @@ class SmarActMCSController : public asynMotorController 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_; @@ -96,6 +102,17 @@ class SmarActMCSController : public asynMotorController private: 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; };