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

Fixed moving after enable bug

Browse Source 
When a MasterMacs motor is powered for the first time, it does not have
a target set. Therefore, the targetReached bit is 0, which the driver
used to interpret as "moving". This is solved now by an additional flag
which checks if the motor did a handshake.

Additionally, the communication module was simplified and new utility
scripts were added. It is now made sure that the communication timeout
for enabling and sending move commands is now at least equal to
PowerCycleTimeout defined in src/masterMacsAxis.cpp.
This commit is contained in:
mathis_s
2025-04-17 16:34:14 +02:00
parent 699b588ba5
commit 60053244a1
7 changed files with 292 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
src/masterMacsAxis.cpp
View File

@@ -11,6 +11,14 @@
#include <string.h>
#include <unistd.h>
/*
A special communication timeout is used in the following two cases:
1) Enable command
2) First move command after enabling the motor
This is due to MasterMACS running a powerup cycle, which can delay the answer.
*/
#define PowerCycleTimeout 10.0 // Value in seconds
/*
Contains all instances of turboPmacAxis which have been created and is used in
the initialization hook function.
@@ -79,7 +87,7 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
stop completely, since this is a configuration error.
*/
if (axisNo >= pC->numAxes()) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:: FATAL ERROR: "
"Axis index %d must be smaller than the total number of axes "
"%d. Call the support.",
@@ -106,12 +114,13 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
// Flag for handshake waiting
waitForHandshake_ = false;
timeAtHandshake_ = 0;
targetReachedUninitialized_ = true;
// masterMacs motors can always be disabled
status = pC_->setIntegerParam(axisNo_, pC_->motorCanDisable(), 1);
if (status != asynSuccess) {
asynPrint(
pC_->asynUserSelf(), ASYN_TRACE_ERROR,
pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
"(setting a parameter value failed with %s)\n. Terminating IOC",
pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
@@ -123,7 +132,7 @@ masterMacsAxis::masterMacsAxis(masterMacsController *pC, int axisNo)
status = pC_->setIntegerParam(axisNo_, pC_->motorStatusMoving(), false);
if (status != asynSuccess) {
asynPrint(
pC_->asynUserSelf(), ASYN_TRACE_ERROR,
pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nFATAL ERROR "
"(setting a parameter value failed with %s)\n. Terminating IOC",
pC_->portName, axisNo, __PRETTY_FUNCTION__, __LINE__,
@@ -148,7 +157,7 @@ asynStatus masterMacsAxis::init() {
// Local variable declaration
asynStatus pl_status = asynSuccess;
char response[pC_->MAXBUF_];
char response[pC_->MAXBUF_] = {0};
int nvals = 0;
double motorRecResolution = 0.0;
double motorPosition = 0.0;
@@ -167,7 +176,7 @@ asynStatus masterMacsAxis::init() {
&motorRecResolution);
if (pl_status == asynParamUndefined) {
if (now + maxInitTime < time(NULL)) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d\nInitializing the parameter library failed.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__,
@@ -259,7 +268,7 @@ asynStatus masterMacsAxis::init() {
// If we can't communicate with the parameter library, it doesn't
// make sense to try and upstream this to the user -> Just log the
// error
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d:\ncallParamCallbacks failed with %s.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
@@ -281,7 +290,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char response[pC_->MAXBUF_];
char response[pC_->MAXBUF_] = {0};
int nvals = 0;
int direction = 0;
@@ -306,8 +315,8 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
if (pC_->getMsgPrintControl().shouldBePrinted(
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, timedOut,
pC_->asynUserSelf())) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
pC_->pasynUser())) {
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d\nAsked for a "
"handshake at %ld s and didn't get a positive reply yet "
"(current time is %ld s).\n",
@@ -341,6 +350,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
// Handshake has been performed successfully -> Continue with the
// poll
waitForHandshake_ = false;
targetReachedUninitialized_ = false;
} else {
// Still waiting for the handshake - try again in the next busy
// poll. This is already part of the movement procedure.
@@ -385,12 +395,18 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
return rw_status;
}
// If the motor is switched off or if it reached its target, it is not
// moving.
if (targetReached() || !switchedOn()) {
if (targetReachedUninitialized_) {
*moving = false;
} else {
*moving = true;
if (targetReached() || !switchedOn()) {
*moving = false;
} else {
*moving = true;
}
}
if (targetReached()) {
targetReachedUninitialized_ = false;
}
// Read the current position
@@ -420,9 +436,9 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
since this information is not reliable.
*/
if (communicationError()) {
if (pC_->getMsgPrintControl().shouldBePrinted(
keyError, true, pC_->asynUserSelf())) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
if (pC_->getMsgPrintControl().shouldBePrinted(keyError, true,
pC_->pasynUser())) {
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d\nCommunication error.%s\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
@@ -579,8 +595,8 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
if (strlen(shellMessage) > 0) {
if (pC_->getMsgPrintControl().shouldBePrinted(
keyError, true, pC_->asynUserSelf())) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
keyError, true, pC_->pasynUser())) {
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line "
"%d\n%s%s\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__,
@@ -597,7 +613,7 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
}
}
} else {
pC_->getMsgPrintControl().resetCount(keyError, pC_->asynUserSelf());
pC_->getMsgPrintControl().resetCount(keyError, pC_->pasynUser());
}
// Read out the limits, if the motor is not moving
@@ -750,7 +766,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
}
if (enabled == 0) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nAxis is "
"disabled.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
@@ -762,7 +778,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
motorVelocity = maxVelocity * motorRecResolution;
asynPrint(
pC_->asynUserSelf(), ASYN_TRACE_FLOW,
pC_->pasynUser(), ASYN_TRACE_FLOW,
"Controller \"%s\", axis %d => %s, line %d:\nMove to position %lf.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, position);
@@ -805,7 +821,7 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
return rw_status;
}
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_FLOW,
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
"Controller \"%s\", axis %d => %s, line %d:\nSetting speed "
"to %lf.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
@@ -825,11 +841,17 @@ asynStatus masterMacsAxis::doMove(double position, int relative,
return rw_status;
}
// If the motor has just been enabled, use Enable
double timeout = pC_->comTimeout();
if (targetReachedUninitialized_ && timeout < PowerCycleTimeout) {
timeout = PowerCycleTimeout;
}
// Start the move
if (relative) {
rw_status = pC_->write(axisNo_, 00, "2");
rw_status = pC_->write(axisNo_, 00, "2", timeout);
} else {
rw_status = pC_->write(axisNo_, 00, "1");
rw_status = pC_->write(axisNo_, 00, "1", timeout);
}
if (rw_status != asynSuccess) {
pl_status = setIntegerParam(pC_->motorStatusProblem(), true);
@@ -912,7 +934,7 @@ asynStatus masterMacsAxis::doHome(double min_velocity, double max_velocity,
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char response[pC_->MAXBUF_];
char response[pC_->MAXBUF_] = {0};
// =========================================================================
@@ -964,7 +986,8 @@ asynStatus masterMacsAxis::readEncoderType() {
// Status of parameter library operations
asynStatus pl_status = asynSuccess;
char command[pC_->MAXBUF_], response[pC_->MAXBUF_];
char command[pC_->MAXBUF_] = {0};
char response[pC_->MAXBUF_] = {0};
int nvals = 0;
int encoder_id = 0;
@@ -1016,6 +1039,13 @@ asynStatus masterMacsAxis::enable(bool on) {
// =========================================================================
/*
When the motor is changing its enable state, its targetReached bit is set to
0. In order to prevent the poll method in interpreting the motor state as
"moving", this flag is used. It is reset in the handshake.
*/
targetReachedUninitialized_ = true;
doPoll(&moving);
// If the axis is currently moving, it cannot be disabled. Ignore the
@@ -1023,7 +1053,7 @@ asynStatus masterMacsAxis::enable(bool on) {
// axis instead of "moving", since status -6 is also moving, but the
// motor can actually be disabled in this state!
if (moving) {
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nAxis is not "
"idle and can therefore not be disabled.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__);
@@ -1045,7 +1075,7 @@ asynStatus masterMacsAxis::enable(bool on) {
if ((readyToBeSwitchedOn() && switchedOn()) == on) {
asynPrint(
pC_->asynUserSelf(), ASYN_TRACE_WARNING,
pC_->pasynUser(), ASYN_TRACE_WARNING,
"Controller \"%s\", axis %d => %s, line %d:\nAxis is already %s.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
on ? "enabled" : "disabled");
@@ -1054,7 +1084,7 @@ asynStatus masterMacsAxis::enable(bool on) {
// Enable / disable the axis if it is not moving
snprintf(value, sizeof(value), "%d", on);
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_FLOW,
asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW,
"Controller \"%s\", axis %d => %s, line %d:\n%s axis.\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
on ? "Enable" : "Disable");
@@ -1072,7 +1102,12 @@ asynStatus masterMacsAxis::enable(bool on) {
// The answer to the enable command on MasterMACS might take some time,
// hence we wait for a custom timespan in seconds instead of
// pC_->comTimeout_
rw_status = pC_->write(axisNo_, 04, value, 1.0);
double timeout = pC_->comTimeout();
if (targetReachedUninitialized_ && timeout < PowerCycleTimeout) {
timeout = PowerCycleTimeout;
}
rw_status = pC_->write(axisNo_, 04, value, timeout);
if (rw_status != asynSuccess) {
return rw_status;
}
@@ -1099,7 +1134,7 @@ asynStatus masterMacsAxis::enable(bool on) {
// Failed to change axis status within timeout_enable_disable => Send a
// corresponding message
asynPrint(pC_->asynUserSelf(), ASYN_TRACE_ERROR,
asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR,
"Controller \"%s\", axis %d => %s, line %d:\nFailed to %s axis "
"within %d seconds\n",
pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__,
@@ -1132,7 +1167,7 @@ std::bitset<16> toBitset(float val) {
}
asynStatus masterMacsAxis::readAxisStatus() {
char response[pC_->MAXBUF_];
char response[pC_->MAXBUF_] = {0};
// =========================================================================
@@ -1153,7 +1188,7 @@ asynStatus masterMacsAxis::readAxisStatus() {
}
asynStatus masterMacsAxis::readAxisError() {
char response[pC_->MAXBUF_];
char response[pC_->MAXBUF_] = {0};
// =========================================================================