From bf3406be1c5d933c2d8b9115a1d42b202ca3f249 Mon Sep 17 00:00:00 2001 From: smathis Date: Tue, 9 Sep 2025 16:52:19 +0200 Subject: [PATCH] Updated to sinqMotor 1.5 to get deadband feature --- src/turboPmacAxis.cpp | 91 ++++++++++++++----------------------- src/turboPmacController.cpp | 2 +- 2 files changed, 36 insertions(+), 57 deletions(-) diff --git a/src/turboPmacAxis.cpp b/src/turboPmacAxis.cpp index 8989472..e398584 100644 --- a/src/turboPmacAxis.cpp +++ b/src/turboPmacAxis.cpp @@ -27,18 +27,6 @@ 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; }; /* @@ -88,8 +76,6 @@ turboPmacAxis::turboPmacAxis(turboPmacController *pC, int axisNo, .timeAtHandshake = 0, .axisStatus = 0, .needInit = false, - .deadband = 0.0, - .eu_to_counts = 0.0, }); // Provide initial values for some parameter library entries @@ -147,8 +133,16 @@ asynStatus turboPmacAxis::init() { double motorVelocity = 0.0; double motorVmax = 0.0; double motorAccel = 0.0; - int acoDelay = 0.0; // Offset time for the movement watchdog caused by - // the air cushions in milliseconds. + + // Conversion factor between engineering units (EU) and encoder counts + double eu_to_counts = 0.0; + + // Deadband in counts. Can be a fraction of a count, hence double. + double deadband_counts = 0.0; + + // Offset time for the movement watchdog caused by the air cushions in + // milliseconds. + int acoDelay = 0.0; int axStatus = 0; // The parameter library takes some time to be initialized. Therefore we @@ -199,10 +193,9 @@ asynStatus turboPmacAxis::init() { pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__); pTurboPmacA_->needInit = true; } - nvals = - sscanf(response, "%d %lf %lf %lf %lf %d %lf %lf", &axStatus, &motorPos, - &motorVmax, &motorVelocity, &motorAccel, &acoDelay, - &pTurboPmacA_->eu_to_counts, &pTurboPmacA_->deadband); + nvals = sscanf(response, "%d %lf %lf %lf %lf %d %lf %lf", &axStatus, + &motorPos, &motorVmax, &motorVelocity, &motorAccel, + &acoDelay, &eu_to_counts, &deadband_counts); // The acoDelay is given in milliseconds -> Convert to seconds, rounded up setOffsetMovTimeout(std::ceil(acoDelay / 1000.0)); @@ -236,6 +229,26 @@ asynStatus turboPmacAxis::init() { return status; } + /* + Check if the new target position is within the range current position +/- + deadband. If that is the case, no movement command should be sent. This + functionality is implemented within the motor record itself, we just need to + populate the field SPDP (see record PushSPDB2Field in db/turboPmac.db) + + 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. + */ + setAxisParamChecked(this, motorPositionDeadband, + deadband_counts / (16.0 * eu_to_counts)); + // Update the parameter library immediately status = callParamCallbacks(); if (status != asynSuccess) { @@ -797,45 +810,11 @@ asynStatus turboPmacAxis::doMove(double position, int relative, motorTargetPosition = position * motorRecResolution; motorVelocity = maxVelocity * motorRecResolution; - /* - 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, + asynPrint(pC_->pasynUser(), ASYN_TRACE_FLOW, "Controller \"%s\", axis %d => %s, line %d\nStart of axis to " "position %lf.\n", pC_->portName, axisNo_, __PRETTY_FUNCTION__, __LINE__, - motorCurrentPosition); + motorTargetPosition); // Check if the speed is allowed to be changed getAxisParamChecked(this, motorCanSetSpeed, &motorCanSetSpeed); diff --git a/src/turboPmacController.cpp b/src/turboPmacController.cpp index 15463fb..ce675ad 100644 --- a/src/turboPmacController.cpp +++ b/src/turboPmacController.cpp @@ -47,7 +47,7 @@ struct turboPmacControllerImpl { int flushHardware; int limFromHardware; }; -#define NUM_turboPmac_DRIVER_PARAMS 3 +#define NUM_turboPmac_DRIVER_PARAMS 5 turboPmacController::turboPmacController(const char *portName, const char *ipPortConfigName,