From ca8f0c2bf11c75a95303da96fd0d47800529936c Mon Sep 17 00:00:00 2001 From: brambilla_m Date: Sat, 19 Mar 2022 14:32:59 +0100 Subject: [PATCH] Add getAxisInitialStatus --- SMC9300Driver.cpp | 124 +++++++++++++++++++++++++++++++++++++--------- 1 file changed, 100 insertions(+), 24 deletions(-) diff --git a/SMC9300Driver.cpp b/SMC9300Driver.cpp index c30a6f8..9175175 100644 --- a/SMC9300Driver.cpp +++ b/SMC9300Driver.cpp @@ -5,6 +5,11 @@ USAGE... Motor driver support for the HUBER SMC9300 controller. Yang Dongliang March 1, 2020 + + +Michele Brambilla +17 March, 2022 + */ #include "SMC9300Driver.h" @@ -24,7 +29,6 @@ March 1, 2020 #define NINT(f) (int)((f) > 0 ? (f) + 0.5 : (f)-0.5) - /** Creates a new SMC9300Controller object. * \param[in] portName The name of the asyn port that will be created * for this driver \param[in] SMC9300PortName The name of the @@ -32,20 +36,26 @@ March 1, 2020 * 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 + * no axis is moving. */ + +/* Michele Brambilla + * Add \param[in] extraParams to the constructor. The default = 1 accounts for + * the motor message + */ + SMC9300Controller::SMC9300Controller(const char *portName, const char *SMC9300PortName, int numAxes, double movingPollPeriod, - double idlePollPeriod, + double idlePollPeriod, const int &extraParams = 1) : asynMotorController( - portName, numAxes, NUM_SMC9300_PARAMS+extraParams, - 0, // No additional interfaces beyond those in base class - 0, // No additional callback interfaces beyond those in base class + portName, numAxes, NUM_SMC9300_PARAMS + extraParams, + 0, // No additional interfaces beyond those in base class + 0, // No additional callback interfaces beyond those in base class ASYN_CANBLOCK | ASYN_MULTIDEVICE, - 1, // autoconnect - 0, 0) // Default priority and stack size + 1, // autoconnect + 0, 0) // Default priority and stack size { int axis; asynStatus status; @@ -284,8 +294,6 @@ asynStatus SMC9300Axis::setClosedLoop(bool closedLoop) { * indicating that the axis is moving (true) or done (false). */ asynStatus SMC9300Axis::poll(bool *moving) { int done; - int driveOn; - int limit; int axisNo; int nvals; int value; @@ -300,14 +308,15 @@ asynStatus SMC9300Axis::poll(bool *moving) { 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 (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); + updateMsgTxtFromDriver(errString); } // Read the current motor position @@ -319,7 +328,6 @@ asynStatus SMC9300Axis::poll(bool *moving) { "drvSMC9300AxisGetStatus: Failed to read position\nStatus: %d" "\nCommand :%s\nResponse:%s\n", comStatus, pC_->outString_, pC_->inString_); - // updateMsgTxtFromDriver("Cannot read Axis position"); } if (axisNo == axisNo_) { setDoubleParam(pC_->motorPosition_, position * STEPS_PER_EGU); @@ -335,7 +343,6 @@ asynStatus SMC9300Axis::poll(bool *moving) { "drvSMC9300AxisGetStatus: Failed to read encoder position\nStatus: %d" "\nCommand :%s\nResponse:%s\n", comStatus, pC_->outString_, pC_->inString_); - // updateMsgTxtFromDriver("Cannot read Axis encoder position"); } if (axisNo == axisNo_) { setDoubleParam(pC_->motorEncoderPosition_, position * STEPS_PER_EGU); @@ -351,7 +358,6 @@ asynStatus SMC9300Axis::poll(bool *moving) { "drvSMC9300AxisGetStatus: Failed to read axis moving status\nStatus: %d" "\nCommand :%s\nResponse:%s\n", comStatus, pC_->outString_, pC_->inString_); - // updateMsgTxtFromDriver("Cannot read Axis position"); } if (axisNo == axisNo_) { done = value & 0x1; @@ -370,21 +376,91 @@ asynStatus SMC9300Axis::poll(bool *moving) { return comStatus ? asynError : asynSuccess; } -asynStatus SMC9300Axis::updateMsgTxtFromDriver(const char *value) -{ +asynStatus SMC9300Axis::updateMsgTxtFromDriver(const char *value) { if (value && value[0]) { - return setStringParam(pC_->motorMessageText_,value); + return setStringParam(pC_->motorMessageText_, value); } - return setStringParam(pC_->motorMessageText_,""); + 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]; -asynStatus SMC9300Axis::getAxisInitialStatus(void) { return asynSuccess; } + 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; +} /** Code for iocsh registration */ static const iocshArg SMC9300CreateControllerArg0 = {"Port name",