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

Fixed communication with the huber controller

Browse Source
This commit is contained in:
koennecke
2023-09-06 12:06:57 +02:00
parent 02cb60be88
commit 35834d1f2b
3 changed files with 64 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@@ -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

165
SMC9300Driver.cpp
View File

@@ -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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <bitset>
#include <errlog.h>
#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 */

2
SMC9300Driver.h
View File

@@ -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;
};