diff --git a/src/turboPmacAxis.cpp b/src/turboPmacAxis.cpp index 4dd1219..627576d 100644 --- a/src/turboPmacAxis.cpp +++ b/src/turboPmacAxis.cpp @@ -15,6 +15,14 @@ struct turboPmacAxisImpl { bool waitForHandshake; + /* + This flag is set to true, if the controller is currently enabling / + disabling the motor and false otherwise. This flag is used in the doPoll + method to check if the enableRBV PV should be set or not in order to prevent + a premature change of state (controller reports that the motor is already + enabled / disabled while it is not ready yet to receive new commands). + */ + bool enableDisable; time_t timeAtHandshake; // The axis status is used when enabling / disabling the motor int axisStatus; @@ -64,6 +72,7 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo, pTurboPmacA_ = std::make_unique( (turboPmacAxisImpl){.waitForHandshake = false, + .enableDisable = false, .timeAtHandshake = 0, .axisStatus = 0, .needInit = false}); @@ -253,9 +262,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { double lowLimit = 0.0; double limitsOffset = 0.0; - // Was the axis idle during the previous poll? - int previousStatusDone = 1; - // ========================================================================= if (pTurboPmacA_->needInit) { @@ -332,8 +338,6 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { return status; } - getAxisParamChecked(this, motorStatusDone, &previousStatusDone); - // Query the axis status snprintf(command, sizeof(command), "P%2.2d00 Q%2.2d10 P%2.2d01 Q%2.2d13 Q%2.2d14", axisNo_, axisNo_, @@ -369,9 +373,12 @@ asynStatus turboPmacAxis::doPoll(bool *moving) { // Store the axis status pTurboPmacA_->axisStatus = axStatus; - // Update the enablement PV - setAxisParamChecked(this, motorEnableRBV, - (axStatus != -3 && axStatus != -5)); + // Update the enablement PV, if we are not in the middle of a enabling / + // disabling procedure. + if (!(pTurboPmacA_->enableDisable)) { + setAxisParamChecked(this, motorEnableRBV, + (axStatus != -3 && axStatus != -5)); + } // Create the unique callsite identifier manually so it can be used later in // the shouldBePrinted calls. @@ -833,7 +840,6 @@ asynStatus turboPmacAxis::doMove(double position, int relative, asynStatus turboPmacAxis::stop(double acceleration) { - // Status of read-write-operations of ASCII commands to the controller asynStatus status = asynSuccess; char command[pC_->MAXBUF_] = {0}; @@ -857,6 +863,18 @@ asynStatus turboPmacAxis::stop(double acceleration) { // if we're currently inside it. pTurboPmacA_->waitForHandshake = false; + /* + Stopping the motor results in a movement and further move commands have to + wait until the stopping movement is done. Therefore, we need to wait until + the poller "sees" the changed state (otherwise, we risk issuing move + commands while the motor is stopping). To ensure that at least one poll is + done, this thread (which also runs move commands) is paused for twice the + idle poll period. + */ + unsigned int idlePollMicros = + (unsigned int)ceil(pC_->idlePollPeriod() * 1e6); + usleep(2 * idlePollMicros); + return status; } @@ -886,14 +904,8 @@ asynStatus turboPmacAxis::doReset() { // if we're currently inside it. pTurboPmacA_->waitForHandshake = false; - // Disable the axis, if it is not in a moving state - int axStat = pTurboPmacA_->axisStatus; - if (axStat == 0 || axStat == -6) { - snprintf(command, sizeof(command), "M%2.2d14=0", axisNo_); - status = pC_->writeRead(axisNo_, command, response, 0); - } - - return status; + // Disable the axis + return enable(false); } /* @@ -1052,7 +1064,7 @@ asynStatus turboPmacAxis::rereadEncoder() { asynStatus turboPmacAxis::enable(bool on) { - int timeout_enable_disable = 2; + int timeout_enable_disable = 5; char command[pC_->MAXBUF_] = {0}; char response[pC_->MAXBUF_] = {0}; int nvals = 0; @@ -1080,13 +1092,16 @@ asynStatus turboPmacAxis::enable(bool on) { axStatus == 5 || axStatus == 6 || axStatus == 7 || axStatus == 8 || axStatus == 9 || axStatus == 10 || axStatus == 11 || axStatus == 12 || axStatus == 13 || axStatus == 15 || axStatus == 16) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line %d\nAxis is not " - "idle and can therefore not be enabled / disabled.\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__); + + asynPrint( + pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nAxis is not " + "idle (status %d) and can therefore not be enabled / disabled.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, axStatus); setAxisParamChecked(this, motorMessageText, "Axis cannot be disabled while it is moving."); + pTurboPmacA_->enableDisable = false; return asynError; } @@ -1109,7 +1124,10 @@ asynStatus turboPmacAxis::enable(bool on) { } } - // Enable / disable the axis if it is not moving + // Now the actual enabling / disabling starts + pTurboPmacA_->enableDisable = true; + + // Enable / disable the axis snprintf(command, sizeof(command), "M%2.2d14=%d", axisNo_, on); asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, "Controller \"%s\", axis %d => %s, line %d\n%s axis\n", @@ -1127,10 +1145,14 @@ asynStatus turboPmacAxis::enable(bool on) { int startTime = time(NULL); while (time(NULL) < startTime + timeout_enable_disable) { + // Wait a bit between status updates from the controller. + usleep(10000); + // Read the axis status - usleep(100000); status = pC_->writeRead(axisNo_, command, response, 1); if (status != asynSuccess) { + // Enabling / disabling procedure failed + pTurboPmacA_->enableDisable = false; return status; } nvals = sscanf(response, "%d", &pTurboPmacA_->axisStatus); @@ -1140,9 +1162,13 @@ asynStatus turboPmacAxis::enable(bool on) { } if ((pTurboPmacA_->axisStatus != -3) == on) { + usleep(500000); + bool moving = false; - // Perform a poll to update the parameter library - poll(&moving); + forcedPoll(&moving); + + // Enabling / disabling procedure is completed (successfully) + pTurboPmacA_->enableDisable = false; return asynSuccess; } } @@ -1159,6 +1185,9 @@ asynStatus turboPmacAxis::enable(bool on) { snprintf(command, sizeof(command), "Failed to %s within %d seconds", on ? "enable" : "disable", timeout_enable_disable); setAxisParamChecked(this, motorMessageText, command); + + // Enabling / disabling procedure failed + pTurboPmacA_->enableDisable = false; return asynError; }