From 8a0c3767ffaa7693b20550b6f492d8daef30c2fe Mon Sep 17 00:00:00 2001 From: smathis Date: Fri, 13 Mar 2026 10:19:00 +0100 Subject: [PATCH] Added switch for fixing an unhomed motor Incremental encoders may report a wrong position after a power cycle until they have been homed. Since the position of the motor is then unknown, the motor might hit the limit switches when moving. The newly introduced record $(INSTR)$(M):FixIfNotHomed provides a switch to prohibit / allow movement of motors with incremental encoder when they haven't been homed yet. --- README.md | 15 ++++++++++----- db/sinqMotor.db | 19 +++++++++++++++++-- src/sinqAxis.cpp | 43 +++++++++++++++++++++++------------------- src/sinqController.cpp | 14 ++++++++++++++ src/sinqController.h | 1 + 5 files changed, 66 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 284e440..f4f2b49 100644 --- a/README.md +++ b/README.md @@ -117,11 +117,11 @@ does not matter): file "$(SINQDBPATH)" { pattern -{ AXIS, M, DESC, EGU, DIR, MRES, ERRORMSGSIZE, ENABLEMOVWATCHDOG, LIMITSOFFSET, CANSETSPEED, ADAPTPOLL } -{ 1, "lin1", "Linear motor doing whatever", mm, Pos, 0.001, 200, 1, 1.0, 1, 1 } -{ 2, "rot1", "First rotary motor", degree, Neg, 0.001, 200, 0, 1.0, 0, 1 } -{ 3, "rot2", "Second rotary motor", degree, Pos, 0.001, 200, 0, 0.0, 1, 0 } -{ 5, "rot3", "Surprise: Third rotary motor", degree, Pos, 0.001, 200, 1, 2.0, 0, 0 } +{ AXIS, M, DESC, EGU, DIR, MRES, ERRORMSGSIZE, ENABLEMOVWATCHDOG, LIMITSOFFSET, CANSETSPEED, FIXIFNOTHOMED, ADAPTPOLL } +{ 1, "lin1", "Linear motor doing whatever", mm, Pos, 0.001, 200, 1, 1.0, 1, 1, 1 } +{ 2, "rot1", "First rotary motor", degree, Neg, 0.001, 200, 0, 1.0, 0, 1, 1 } +{ 3, "rot2", "Second rotary motor", degree, Pos, 0.001, 200, 0, 0.0, 1, 1, 0 } +{ 5, "rot3", "Surprise: Third rotary motor", degree, Pos, 0.001, 200, 1, 2.0, 0, 1, 0 } } ``` The variable `SINQDBPATH` has been set in "mcu1.cmd" before calling `dbLoadTemplate`. @@ -157,6 +157,11 @@ read-out limits are [-10.0 10.0], the EPICS limits are set to [-9.0 9.0]. This parameter uses engineering units (EGU). Defaults to 0.0. - `CANSETSPEED`: If set to 1, the motor speed can be modified by the user. Defaults to 0. +- `FIXIFNOTHOMED`: Incremental encoders may report a wrong position after a +power cycle until they have been homed. Since the position of the motor is then +unknown, the motor might hit the limit switches when moving. If this parameter +is set to 1 (or any other nonzero-value), movement is blocked until the motor +has been homed. Defaults to 0. - `ADAPTPOLL`: If set to any value other than 0, adaptive polling is enabled for this particular axis. Adaptive polling is designed to reduce the communication load in case some axis is moving. By default, if at least one axis is moving, diff --git a/db/sinqMotor.db b/db/sinqMotor.db index a314aec..7d5e10e 100755 --- a/db/sinqMotor.db +++ b/db/sinqMotor.db @@ -150,8 +150,9 @@ record(longin, "$(INSTR)$(M):CanDisable") { } # For some motors, the user might be allowed to adjust the speed within the -# limits specified in the motor record as VBAS and VMAX. This functionality can -# be enabled by setting CANSETSPEED to 1. It is disabled by default. +# limits specified in the motor record as VBAS and VMAX. This functionality can +# be enabled by setting CANSETSPEED in the substitution file to 1. It is +# disabled by default. # This record is coupled to the parameter library via motorCanSetSpeed -> MOTOR_CAN_SET_SPEED. record(longout, "$(INSTR)$(M):CanSetSpeed") { field(DTYP, "asynInt32") @@ -161,6 +162,20 @@ record(longout, "$(INSTR)$(M):CanSetSpeed") { field(VAL, "$(CANSETSPEED=0)") } +# If a motor has an incremental encoder, the position value given by the encoder +# might be wrong unless the motor is known to being homed since the last +# controller startup. As a safety feature, movement of the motor is be disabled +# in such a case if this record has a value other than zero. The record value +# can be set either by setting FIXIFNOTHOMED in the substitution file to to 1 or +# by changing the value of this record at runtime. It is disabled by default. +# This record is coupled to the parameter library via motorFixIfNotHomed -> MOTOR_FIX_IF_NOT_HOMED. +record(longout, "$(INSTR)$(M):FixIfNotHomed") { + field(DTYP, "asynInt32") + field(OUT, "@asyn($(CONTROLLER),$(AXIS),1) MOTOR_FIX_IF_NOT_HOMED") + field(PINI, "YES") + field(VAL, "$(FIXIFNOTHOMED=0)") +} + # If this PV has a value other than 0, adaptive polling for this axis is enabled. # The standard motor record behaviour is to poll all axis with the busy / move poll # period if at least one of the axes is moving. Adaptive polling changes this so diff --git a/src/sinqAxis.cpp b/src/sinqAxis.cpp index ba0284f..63fdc0f 100644 --- a/src/sinqAxis.cpp +++ b/src/sinqAxis.cpp @@ -369,28 +369,33 @@ asynStatus sinqAxis::move(double position, int relative, double minVelocity, asynStatus status = asynSuccess; double motorRecRes = 0.0; char encType[pC_->MAXBUF_] = {0}; - int motorStatHomed = 0; + int motStatusHomed = 0; + int motFixIfNotHomed = 0; // ========================================================================= - /* - Check if the motor is allowed to move: If the motor hasn't been homed in the - past and has an incremental encoder, it needs to be homed first! - */ - getAxisParamChecked(this, encoderType, &encType); - getAxisParamChecked(this, motorStatusHomed, &motorStatHomed); - if (strcmp(encType, IncrementalEncoder) == 0 && motorStatHomed == 0) { - asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, - "Controller \"%s\", axis %d => %s, line " - "%d:\nAxis needs to be homed first.\n", - pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__); - setAxisParamChecked(this, motorErrorMessage, - "Motor needs to be homed / referenced first."); - setAxisParamChecked(this, motorStatusProblem, true); - epicsTimeStamp ts; - epicsTimeGetCurrent(&ts); - pSinqA_->errorInitialAppearance = ts; - return pC_->callParamCallbacks(); + // Check if the motor is set to be fixed if it hasn't been homed + getAxisParamChecked(this, motorFixIfNotHomed, &motFixIfNotHomed); + if (motFixIfNotHomed) { + /* + If the motor hasn't been homed in + the past and has an incremental encoder, it needs to be homed first! + */ + getAxisParamChecked(this, encoderType, &encType); + getAxisParamChecked(this, motorStatusHomed, &motStatusHomed); + if (strcmp(encType, IncrementalEncoder) == 0 && motStatusHomed == 0) { + asynPrint(pC_->pasynUser(), ASYN_TRACE_ERROR, + "Controller \"%s\", axis %d => %s, line " + "%d:\nAxis needs to be homed first.\n", + pC_->portName, axisNo(), __PRETTY_FUNCTION__, __LINE__); + setAxisParamChecked(this, motorErrorMessage, + "Motor needs to be homed / referenced first."); + setAxisParamChecked(this, motorStatusProblem, true); + epicsTimeStamp ts; + epicsTimeGetCurrent(&ts); + pSinqA_->errorInitialAppearance = ts; + return pC_->callParamCallbacks(); + } } // Store the target position internally diff --git a/src/sinqController.cpp b/src/sinqController.cpp index e54c08d..0ad9a87 100644 --- a/src/sinqController.cpp +++ b/src/sinqController.cpp @@ -83,6 +83,7 @@ struct sinqControllerImpl { int motorCanDisable; int motorEnableMovWatchdog; int motorCanSetSpeed; + int motorFixIfNotHomed; int motorLimitsOffset; int motorForceStop; int motorConnected; @@ -137,6 +138,7 @@ sinqController::sinqController(const char *portName, .motorCanDisable = 0, .motorEnableMovWatchdog = 0, .motorCanSetSpeed = 0, + .motorFixIfNotHomed = 0, .motorLimitsOffset = 0, .motorForceStop = 0, .motorConnected = 0, @@ -243,6 +245,17 @@ sinqController::sinqController(const char *portName, exit(-1); } + status = createParam("MOTOR_FIX_IF_NOT_HOMED", asynParamInt32, + &pSinqC_->motorFixIfNotHomed); + if (status != asynSuccess) { + asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, + "Controller \"%s\" => %s, line %d:\nFATAL ERROR (creating a " + "parameter failed with %s).\nTerminating IOC", + portName, __PRETTY_FUNCTION__, __LINE__, + stringifyAsynStatus(status)); + exit(-1); + } + status = createParam("MOTOR_LIMITS_OFFSET", asynParamFloat64, &pSinqC_->motorLimitsOffset); if (status != asynSuccess) { @@ -718,6 +731,7 @@ int sinqController::motorEnableMovWatchdog() { return pSinqC_->motorEnableMovWatchdog; } int sinqController::motorCanSetSpeed() { return pSinqC_->motorCanSetSpeed; } +int sinqController::motorFixIfNotHomed() { return pSinqC_->motorFixIfNotHomed; } int sinqController::motorLimitsOffset() { return pSinqC_->motorLimitsOffset; } int sinqController::motorForceStop() { return pSinqC_->motorForceStop; } int sinqController::motorConnected() { return pSinqC_->motorConnected; } diff --git a/src/sinqController.h b/src/sinqController.h index 166d727..23293a2 100644 --- a/src/sinqController.h +++ b/src/sinqController.h @@ -355,6 +355,7 @@ class HIDDEN sinqController : public asynMotorController { int motorCanDisable(); int motorEnableMovWatchdog(); int motorCanSetSpeed(); + int motorFixIfNotHomed(); int motorLimitsOffset(); int motorForceStop(); int motorConnected();