lin-epics-modules/motorHuber
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add getAxisInitialStatus

Browse Source
This commit is contained in:
brambilla_m
2022-03-19 14:32:59 +01:00
parent 100daf7036
commit ca8f0c2bf1
1 changed files with 100 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SMC9300Driver.cpp
+100 -24
View File
@@ -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",