From 35834d1f2b7522c6e975a467a1d828e242845e7e Mon Sep 17 00:00:00 2001 From: koennecke Date: Wed, 6 Sep 2023 12:06:57 +0200 Subject: [PATCH] Fixed communication with the huber controller --- Makefile | 3 +- SMC9300Driver.cpp | 165 +++++++++++++++++----------------------------- SMC9300Driver.h | 2 +- 3 files changed, 64 insertions(+), 106 deletions(-) diff --git a/Makefile b/Makefile index 1c81c93..84a1a8f 100644 --- a/Makefile +++ b/Makefile @@ -1,8 +1,9 @@ include /ioc/tools/driver.makefile MODULE=motorHuber -LIBVERSION=brambilla_m +LIBVERSION=koennecke BUILDCLASSES=Linux EPICS_VERSIONS=3.14.12 7.0.4.1 +ARCH_FILTER=RHEL7-x86_64 DBDS += devHuberMotor.dbd diff --git a/SMC9300Driver.cpp b/SMC9300Driver.cpp index 9175175..5073971 100644 --- a/SMC9300Driver.cpp +++ b/SMC9300Driver.cpp @@ -10,6 +10,9 @@ March 1, 2020 Michele Brambilla 17 March, 2022 +Trying to fix communication problems: Mark Koennecke +August, 2023 + */ #include "SMC9300Driver.h" @@ -22,13 +25,16 @@ Michele Brambilla #include #include #include - +#include #include +#include + #define STEPS_PER_EGU 10000. #define NINT(f) (int)((f) > 0 ? (f) + 0.5 : (f)-0.5) - +#define CHECK_BIT(var,pos) ((var) & (1 << pos)) +# /** Creates a new SMC9300Controller object. * \param[in] portName The name of the asyn port that will be created * for this driver \param[in] SMC9300PortName The name of the @@ -77,6 +83,18 @@ SMC9300Controller::SMC9300Controller(const char *portName, createParam(motorMessageTextString, asynParamOctet, &motorMessageText_); } +asynStatus SMC9300Controller::transact(void) +{ + size_t nread, in; + int reason; + float timeout = 5; + /* read with a short timeout in order to remove stray messages + * from the line. This also serves to slow down communication + */ + pasynOctetSyncIO->read(pasynUserController_, inString_, sizeof(inString_), .01, &in, &reason); + + return writeReadController(outString_, inString_, sizeof(inString_), &nread, timeout); +} /** Creates a new SMC9300Controller object. * Configuration command, called directly or from iocsh * \param[in] portName The name of the asyn port that will be created @@ -142,9 +160,10 @@ SMC9300Axis *SMC9300Controller::getAxis(int axisNo) { SMC9300Axis::SMC9300Axis(SMC9300Controller *pC, int axisNo) : asynMotorAxis(pC, axisNo), pC_(pC) { static const char *functionName = "SMC9300Axis::SMC9300Axis"; + bool moving; // Do an initial poll to get some values from the axis - if (getAxisInitialStatus() != asynSuccess) { + if (poll(&moving) != asynSuccess) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, "%s: getAxisInitialStatus failed to return asynSuccess. " "Controller: %s, Axis: %d.\n", @@ -217,6 +236,8 @@ asynStatus SMC9300Axis::move(double position, int relative, double minVelocity, sprintf(pC_->outString_, "goto%d:%f", axisNo_, position / STEPS_PER_EGU); } status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -233,6 +254,8 @@ asynStatus SMC9300Axis::home(double minVelocity, double maxVelocity, sprintf(pC_->outString_, "home%d:hs", axisNo_); } status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -256,6 +279,8 @@ asynStatus SMC9300Axis::moveVelocity(double minVelocity, double maxVelocity, sprintf(pC_->outString_, "move%d:-0.1", axisNo_); } status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -265,6 +290,8 @@ asynStatus SMC9300Axis::stop(double acceleration) { sprintf(pC_->outString_, "q%d", axisNo_); status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -274,6 +301,8 @@ asynStatus SMC9300Axis::setPosition(double position) { sprintf(pC_->outString_, "pos%d:%f", axisNo_, position / STEPS_PER_EGU); status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -283,6 +312,8 @@ asynStatus SMC9300Axis::setClosedLoop(bool closedLoop) { sprintf(pC_->outString_, "ecl%d:%d", axisNo_, closedLoop ? 0 : 1); status = pC_->writeController(); + // The controller needs a while before it can accept more commands + usleep(300); return status; } @@ -304,24 +335,9 @@ asynStatus SMC9300Axis::poll(bool *moving) { updateMsgTxtFromDriver(""); - // Read errors from controller - sprintf(pC_->outString_, "?err%d", axisNo_); - comStatus = pC_->writeReadController(); - nvals = sscanf(pC_->inString_, "%d:%d %[^NULL]", &axisNo, &value, errString); - if (comStatus || nvals != 3 || nvals != 2) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvSMC9300AxisGetStatus: Failed to read error message\nStatus: %d" - "\nCommand :%s\nResponse:%s\n", - comStatus, pC_->outString_, pC_->inString_); - } - if (axisNo == axisNo_) { - updateMsgTxtFromDriver(errString); - } - // Read the current motor position sprintf(pC_->outString_, "?p%d", axisNo_); - comStatus = pC_->writeReadController(); + comStatus = pC_->transact(); nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); if (comStatus || nvals != 2) { asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, @@ -335,7 +351,7 @@ asynStatus SMC9300Axis::poll(bool *moving) { // Read the current motor encoder position sprintf(pC_->outString_, "?e%d", axisNo_); - comStatus = pC_->writeReadController(); + comStatus = pC_->transact(); nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); if (comStatus || nvals != 2) { asynPrint( @@ -350,7 +366,7 @@ asynStatus SMC9300Axis::poll(bool *moving) { // Read the moving status of this motor sprintf(pC_->outString_, "?s%d", axisNo_); - comStatus = pC_->writeReadController(); + comStatus = pC_->transact(); nvals = sscanf(pC_->inString_, "%d:%d", &axisNo, &value); if (comStatus || nvals != 2) { asynPrint( @@ -370,6 +386,25 @@ asynStatus SMC9300Axis::poll(bool *moving) { setIntegerParam(pC_->motorStatusPowerOn_, value >> 7 & 0x1); setIntegerParam(pC_->motorStatusProblem_, 0); // setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0); + if(CHECK_BIT(value, 15)) { + // Read errors from controller + sprintf(pC_->outString_, "?err%d", axisNo_); + comStatus = pC_->transact(); + nvals = sscanf(pC_->inString_, "%d:%d %[^NULL]", &axisNo, &value, errString); + if (comStatus || (nvals != 3 && nvals != 2)) { + asynPrint( + pC_->pasynUserSelf, ASYN_TRACE_ERROR, + "drvSMC9300AxisGetStatus: Failed to read error message\nStatus: %d" + "\nCommand :%s\nResponse:%s\n", + comStatus, pC_->outString_, pC_->inString_); + } + if (axisNo == axisNo_) { + updateMsgTxtFromDriver(errString); + setIntegerParam(pC_->motorStatusProblem_, 1); + errlogPrintf("SMC9300 motor %d reported %s\n", axisNo_, errString); + return asynError; + } + } } comStatus = callParamCallbacks(); @@ -377,89 +412,11 @@ asynStatus SMC9300Axis::poll(bool *moving) { } asynStatus SMC9300Axis::updateMsgTxtFromDriver(const char *value) { - if (value && value[0]) { - return setStringParam(pC_->motorMessageText_, value); - } - return setStringParam(pC_->motorMessageText_, ""); -} - -asynStatus SMC9300Axis::getAxisInitialStatus(void) { - int axisNo; - int nvals; - int value; - double position; - asynStatus comStatus; - static const char *functionName = "SMC9300Controller::getAxisInitialStatus"; - char errString[1024]; - - updateMsgTxtFromDriver(""); - - // Read errors from controller - sprintf(pC_->outString_, "?err%d", axisNo_); - comStatus = pC_->writeReadController(); - nvals = sscanf(pC_->inString_, "%d:%d %[^NULL]", &axisNo, &value, errString); - if (comStatus || nvals != 3 || nvals != 2) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvSMC9300AxisGetStatus: Failed to read error message\nStatus: %d" - "\nCommand :%s\nResponse:%s\n", - comStatus, pC_->outString_, pC_->inString_); - } - if (axisNo == axisNo_) { - updateMsgTxtFromDriver(errString); - } - - // Read the current motor position - sprintf(pC_->outString_, "?p%d", axisNo_); - comStatus = pC_->writeReadController(); - nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); - if (comStatus || nvals != 2) { - asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvSMC9300AxisGetStatus: Failed to read position\nStatus: %d" - "\nCommand :%s\nResponse:%s\n", - comStatus, pC_->outString_, pC_->inString_); - } - if (axisNo == axisNo_) { - setDoubleParam(pC_->motorPosition_, position * STEPS_PER_EGU); - } - - // Read the current motor encoder position - sprintf(pC_->outString_, "?e%d", axisNo_); - comStatus = pC_->writeReadController(); - nvals = sscanf(pC_->inString_, "%d:%lf", &axisNo, &position); - if (comStatus || nvals != 2) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvSMC9300AxisGetStatus: Failed to read encoder position\nStatus: %d" - "\nCommand :%s\nResponse:%s\n", - comStatus, pC_->outString_, pC_->inString_); - } - if (axisNo == axisNo_) { - setDoubleParam(pC_->motorEncoderPosition_, position * STEPS_PER_EGU); - } - - // Read the moving status of this motor - sprintf(pC_->outString_, "?s%d", axisNo_); - comStatus = pC_->writeReadController(); - nvals = sscanf(pC_->inString_, "%d:%d", &axisNo, &value); - if (comStatus || nvals != 2) { - asynPrint( - pC_->pasynUserSelf, ASYN_TRACE_ERROR, - "drvSMC9300AxisGetStatus: Failed to read axis moving status\nStatus: %d" - "\nCommand :%s\nResponse:%s\n", - comStatus, pC_->outString_, pC_->inString_); - } - if (axisNo == axisNo_) { - setIntegerParam(pC_->motorStatusDone_, value & 0x1); - setIntegerParam(pC_->motorStatusHighLimit_, value >> 2 & 0x1); - setIntegerParam(pC_->motorStatusLowLimit_, value >> 3 & 0x1); - setIntegerParam(pC_->motorStatusAtHome_, 0); - setIntegerParam(pC_->motorStatusPowerOn_, value >> 7 & 0x1); - setIntegerParam(pC_->motorStatusProblem_, 0); - } - - comStatus = callParamCallbacks(); - return comStatus ? asynError : asynSuccess; + return asynSuccess; + // if (value && value[0]) { + // return setStringParam(pC_->motorMessageText_, value); + //} + //return setStringParam(pC_->motorMessageText_, ""); } /** Code for iocsh registration */ diff --git a/SMC9300Driver.h b/SMC9300Driver.h index acd225e..c86d943 100644 --- a/SMC9300Driver.h +++ b/SMC9300Driver.h @@ -30,7 +30,6 @@ public: asynStatus poll(bool *moving); asynStatus setPosition(double position); asynStatus setClosedLoop(bool closedLoop); - asynStatus getAxisInitialStatus(void); protected: @@ -52,6 +51,7 @@ public: protected: int motorMessageText_; + asynStatus transact(void); friend class SMC9300Axis; };