diff --git a/src/turboPmacAxis.cpp b/src/turboPmacAxis.cpp index 56ac49d..8989472 100644 --- a/src/turboPmacAxis.cpp +++ b/src/turboPmacAxis.cpp @@ -27,6 +27,18 @@ struct turboPmacAxisImpl { // The axis status is used when enabling / disabling the motor int axisStatus; bool needInit; + + /* + Deadband is defined in enconder units / 16 (a value of 16 is 1 actual + encoder count, see Turbo PMAC Manula, p.160) + */ + double deadband; + + /* + Scaling factor Ixx08, see Turbo PMAC User Manual, p. 268f: + [Ixx08] = counts / engineering units (eu) + */ + double eu_to_counts; }; /* @@ -70,12 +82,15 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo, axes.push_back(this); } - pTurboPmacA_ = std::make_unique( - (turboPmacAxisImpl){.waitForHandshake = false, - .enableDisable = false, - .timeAtHandshake = 0, - .axisStatus = 0, - .needInit = false}); + pTurboPmacA_ = std::make_unique((turboPmacAxisImpl){ + .waitForHandshake = false, + .enableDisable = false, + .timeAtHandshake = 0, + .axisStatus = 0, + .needInit = false, + .deadband = 0.0, + .eu_to_counts = 0.0, + }); // Provide initial values for some parameter library entries status = pC_->setIntegerParam(axisNo_, pC_->rereadEncoderPosition(), 0); @@ -163,12 +178,16 @@ asynStatus turboPmacAxis::init() { /* Read out the axis status, the current position, current and maximum speed, - acceleration and the air cushion delay. + acceleration, the air cushion delay, the scaling factor between counts and + engineering units ([Ixx08] = counts / engineering units) and the deadband + Ixx65. */ snprintf(command, sizeof(command), - "P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22", axisNo_, - axisNo_, axisNo_, axisNo_, axisNo_, axisNo_); - status = pC_->writeRead(axisNo_, command, response, 6); + "P%2.2d00 Q%2.2d10 Q%2.2d03 Q%2.2d04 Q%2.2d06 P%2.2d22 I%2.2d08 " + "I%2.2d65", + axisNo_, axisNo_, axisNo_, axisNo_, axisNo_, axisNo_, axisNo_, + axisNo_); + status = pC_->writeRead(axisNo_, command, response, 8); if (status != asynSuccess) { asynPrint( pC_->pasynUser(), ASYN_TRACE_ERROR, @@ -180,8 +199,10 @@ asynStatus turboPmacAxis::init() { pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__); pTurboPmacA_->needInit = true; } - nvals = sscanf(response, "%d %lf %lf %lf %lf %d", &axStatus, &motorPos, - &motorVmax, &motorVelocity, &motorAccel, &acoDelay); + nvals = + sscanf(response, "%d %lf %lf %lf %lf %d %lf %lf", &axStatus, &motorPos, + &motorVmax, &motorVelocity, &motorAccel, &acoDelay, + &pTurboPmacA_->eu_to_counts, &pTurboPmacA_->deadband); // The acoDelay is given in milliseconds -> Convert to seconds, rounded up setOffsetMovTimeout(std::ceil(acoDelay / 1000.0)); @@ -191,7 +212,7 @@ asynStatus turboPmacAxis::init() { // here to mm/s^2. motorAccel = motorAccel * 1000; - if (nvals != 6) { + if (nvals != 8) { return pC_->couldNotParseResponse(command, response, axisNo_, __PRETTY_FUNCTION__, __LINE__); } @@ -752,7 +773,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative, char command[pC_->MAXBUF_] = {0}; char response[pC_->MAXBUF_] = {0}; - double motorCoordinatesPosition = 0.0; + double motorTargetPosition = 0.0; double motorRecResolution = 0.0; double motorVelocity = 0.0; int enabled = 0; @@ -773,13 +794,48 @@ asynStatus turboPmacAxis::doMove(double position, int relative, } // Convert from EPICS to user / motor units - motorCoordinatesPosition = position * motorRecResolution; + motorTargetPosition = position * motorRecResolution; motorVelocity = maxVelocity * motorRecResolution; - asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, + /* + Check if the new target position is within the range current position +/- + deadband. If that is the case, no movement command should be sent. + + First, the deadband is read out (Ixx65, see Turbo PMAC User Manual, p. + 160.). This value then needs to be converted into counts (by dividing it by + 16). After words, the deadband in counts then need to be converted into + engineering units. For this, the scaling factor Ixx08 needs to be applied + (see Turbo PMAC User Manual, p. 268f): + [Ixx08] = counts / engineering units (eu) + deadband_eu = deadband_counts / Ixx08 = Ixx65 / (16 * Ixx08) + + The values of Ixx08 and Ixx65 are read out during initialization and are + assumed to not change during operation. + */ + double motorCurrentPosition = 0.0; + motorPosition(&motorCurrentPosition); + double deadband_eu = pTurboPmacA_->deadband / (pTurboPmacA_->eu_to_counts); + double deadband_lower = motorCurrentPosition - deadband_eu; + double deadband_upper = motorCurrentPosition + deadband_eu; + + if (motorTargetPosition > deadband_lower && + motorTargetPosition < deadband_upper) { + + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line %d\nNew target " + "position %lf within deadband around current position %lf " + "(deadband %lf to %lf). No move command sent.\n", + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + motorTargetPosition, motorCurrentPosition, deadband_lower, + deadband_upper); + return asynSuccess; + } + + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, "Controller \"%s\", axis %d => %s, line %d\nStart of axis to " "position %lf.\n", - pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, position); + pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, + motorCurrentPosition); // Check if the speed is allowed to be changed getAxisParamChecked(this, motorCanSetSpeed, &motorCanSetSpeed); @@ -804,11 +860,11 @@ asynStatus turboPmacAxis::doMove(double position, int relative, if (relative) { snprintf(&command[writeOffset], sizeof(command) - writeOffset, "P%2.2d23=0 Q%2.2d02=%lf M%2.2d=2", axisNo_, axisNo_, - motorCoordinatesPosition, axisNo_); + motorTargetPosition, axisNo_); } else { snprintf(&command[writeOffset], sizeof(command) - writeOffset, "P%2.2d23=0 Q%2.2d01=%lf M%2.2d=1", axisNo_, axisNo_, - motorCoordinatesPosition, axisNo_); + motorTargetPosition, axisNo_); } // We don't expect an answer @@ -820,7 +876,7 @@ asynStatus turboPmacAxis::doMove(double position, int relative, "Controller \"%s\", axis %d => %s, line %d\nStarting movement to " "target position %lf failed.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - motorCoordinatesPosition); + motorTargetPosition); setAxisParamChecked(this, motorStatusProblem, true); return status; }