From ab596ded48dbc8b7478bde274b458b13be49b7ae Mon Sep 17 00:00:00 2001 From: smathis Date: Thu, 14 Aug 2025 08:03:17 +0200 Subject: [PATCH] Lift and angle axis now move simultaneously Fixed a bug which caused the axes not to move in parallel, but after each other. --- src/detectorTowerAngleAxis.cpp | 33 ++++++++++++++-------------- src/detectorTowerAngleAxis.h | 17 +++++++------- src/detectorTowerController.cpp | 39 +++++++++++++++++++++------------ src/detectorTowerLiftAxis.cpp | 8 ++++--- src/detectorTowerLiftAxis.h | 4 ++++ 5 files changed, 58 insertions(+), 43 deletions(-) diff --git a/src/detectorTowerAngleAxis.cpp b/src/detectorTowerAngleAxis.cpp index 69cb300..126c111 100644 --- a/src/detectorTowerAngleAxis.cpp +++ b/src/detectorTowerAngleAxis.cpp @@ -34,17 +34,16 @@ static void deferredMovementCollectorLoop(void *drvPvt) { detectorTowerAngleAxis *axis = (detectorTowerAngleAxis *)drvPvt; while (1) { if (axis->receivedTarget_) { - // Wait for 100 ms and then start the movement with the information - // available - axis->startingDeferredMovement_ = true; + // Wait for the time span defined in deferredMovementWait_ and then + // start the movement with the information available epicsThreadSleep(axis->deferredMovementWait_); axis->startCombinedMove(); // After the movement command has been send, reset the flag axis->receivedTarget_ = false; } - // Limit this loop to an idle frequency of 1 kHz - epicsThreadSleep(0.001); + // Limit this loop to an idle frequency of 10 kHz + epicsThreadSleep(0.01); } } @@ -76,11 +75,10 @@ detectorTowerAngleAxis::detectorTowerAngleAxis(detectorTowerController *pC, } // Initialize all member variables - // Assumed to be not ready by default, this is overwritten in the next poll error_ = 0; + waitingForStart_ = false; receivedTarget_ = false; - startingDeferredMovement_ = false; - deferredMovementWait_ = 0.1; // seconds + deferredMovementWait_ = 0.5; // seconds // Will be populated in the init() method beamRadius_ = 0.0; @@ -199,7 +197,7 @@ asynStatus detectorTowerAngleAxis::doMove(double position, int relative, // started to the defined target position. setTargetPosition(position * motorRecResolution); receivedTarget_ = true; - + waitingForStart_ = true; return asynSuccess; } @@ -237,7 +235,6 @@ asynStatus detectorTowerAngleAxis::startCombinedMove() { setAxisParamChecked(this, motorStatusProblem, true); callParamCallbacks(); - startingDeferredMovement_ = false; return asynError; } @@ -293,9 +290,9 @@ asynStatus detectorTowerAngleAxis::stop(double acceleration) { } // Reset the deferred movement flags - startingDeferredMovement_ = false; - deferredMovementWait_ = false; - + receivedTarget_ = false; + waitingForStart_ = false; + liftAxis()->waitingForStart_ = false; return status; } @@ -422,8 +419,9 @@ asynStatus detectorTowerAngleAxis::doReset() { getAxisParamChecked(this, positionStateRBV, &positionState); // Reset the deferred movement flags - startingDeferredMovement_ = false; - deferredMovementWait_ = false; + receivedTarget_ = false; + waitingForStart_ = false; + liftAxis()->waitingForStart_ = false; /* Check which action should be performed: @@ -552,11 +550,12 @@ static void configDetectorTowerCreateAxisCallFunc(const iocshArgBuf *args) { // ============================================================================= /** - * @brief Set the setDeferredMovementWait (FFI implementation) + * @brief Set the setDeferredMovementWait time (FFI implementation) * * @param portName Name of the controller * @param axisNo Axis number - * @param scaleMovTimeout Scaling factor (in seconds) + * @param deferredMovementWait Time (in seconds) the driver waits to collect + * all movement commands from the different axes * @return asynStatus */ asynStatus setDeferredMovementWait(const char *portName, int axisNo, diff --git a/src/detectorTowerAngleAxis.h b/src/detectorTowerAngleAxis.h index fb78604..395fc8c 100644 --- a/src/detectorTowerAngleAxis.h +++ b/src/detectorTowerAngleAxis.h @@ -136,15 +136,15 @@ class HIDDEN detectorTowerAngleAxis : public turboPmacAxis { */ detectorTowerSupportAxis *supportAxis() { return supportAxis_; } + /** + * @brief Return a pointer to the axis controller + */ + virtual detectorTowerController *pController() override { return pC_; }; + // If true, either this axis or one of the detectorTowerLiftAxis // attached to it received a movement command. bool receivedTarget_; - // If set to true, the virtual axis is about to start a deferred movement - // (but is currently still collecting movement commands from its component - // axes) - bool startingDeferredMovement_; - /* The collector cycle waits until this time in seconds has passed before starting a deferred movement @@ -157,10 +157,9 @@ class HIDDEN detectorTowerAngleAxis : public turboPmacAxis { */ int error_; - /** - * @brief Return a pointer to the axis controller - */ - virtual detectorTowerController *pController() override { return pC_; }; + // True, if this axis has received a move command and the driver is + // currently starting a deferred movement. + bool waitingForStart_; protected: detectorTowerController *pC_; diff --git a/src/detectorTowerController.cpp b/src/detectorTowerController.cpp index 80c2589..3166dd9 100644 --- a/src/detectorTowerController.cpp +++ b/src/detectorTowerController.cpp @@ -418,15 +418,14 @@ asynStatus detectorTowerController::pollDetectorAxes( // Check if the tower is moving if (notInPosition == 1 || positionState > 2) { - // By now, the controller has actually started the movement - angleAxis->startingDeferredMovement_ = false; *moving = true; + + // Since the tower is now moving, the axis by definition do not wait for + // movement start anymore. + liftAxis->waitingForStart_ = false; + angleAxis->waitingForStart_ = false; } else { - if (angleAxis->startingDeferredMovement_) { - *moving = true; - } else { - *moving = false; - } + *moving = false; } // Calculate the lift position /w consideration of the angle @@ -809,14 +808,26 @@ asynStatus detectorTowerController::pollDetectorAxes( setAxisParamChecked(liftAxis, motorEnableRBV, true); setAxisParamChecked(supportAxis, motorEnableRBV, true); - // Are the axes currently moving? - setAxisParamChecked(angleAxis, motorStatusMoving, *moving); - setAxisParamChecked(liftAxis, motorStatusMoving, *moving); - setAxisParamChecked(supportAxis, motorStatusMoving, *moving); + // If the axis is starting a movement, mark it as moving. Otherwise use the + // moving parameter. + if (angleAxis->waitingForStart_) { + setAxisParamChecked(angleAxis, motorStatusMoving, true); + setAxisParamChecked(angleAxis, motorStatusDone, false); + } else { + setAxisParamChecked(angleAxis, motorStatusMoving, *moving); + setAxisParamChecked(angleAxis, motorStatusDone, !(*moving)); + } - // Is the axis movement done? - setAxisParamChecked(angleAxis, motorStatusDone, !(*moving)); - setAxisParamChecked(liftAxis, motorStatusDone, !(*moving)); + if (liftAxis->waitingForStart_) { + setAxisParamChecked(liftAxis, motorStatusMoving, true); + setAxisParamChecked(liftAxis, motorStatusDone, false); + } else { + setAxisParamChecked(liftAxis, motorStatusMoving, *moving); + setAxisParamChecked(liftAxis, motorStatusDone, !(*moving)); + } + + // Support axis status is solely deduced from system movement state + setAxisParamChecked(supportAxis, motorStatusMoving, *moving); setAxisParamChecked(supportAxis, motorStatusDone, !(*moving)); // In which direction are the axes currently moving? diff --git a/src/detectorTowerLiftAxis.cpp b/src/detectorTowerLiftAxis.cpp index 7f4ee4f..ecdee17 100644 --- a/src/detectorTowerLiftAxis.cpp +++ b/src/detectorTowerLiftAxis.cpp @@ -70,6 +70,9 @@ detectorTowerLiftAxis::detectorTowerLiftAxis(detectorTowerController *pC, angleAxis_ = angleAxis; angleAxis->liftAxis_ = this; + + // Initialize the flag to false + waitingForStart_ = false; } detectorTowerLiftAxis::~detectorTowerLiftAxis(void) { @@ -157,7 +160,6 @@ asynStatus detectorTowerLiftAxis::doMove(double position, int relative, double min_velocity, double max_velocity, double acceleration) { - double motorRecResolution = 0.0; getAxisParamChecked(this, motorRecResolution, &motorRecResolution); @@ -166,7 +168,7 @@ asynStatus detectorTowerLiftAxis::doMove(double position, int relative, // defined target position. setTargetPosition(position * motorRecResolution); angleAxis_->receivedTarget_ = true; - + waitingForStart_ = true; return asynSuccess; } @@ -222,4 +224,4 @@ asynStatus detectorTowerLiftAxis::adjustOrigin(double newOrigin) { } return status; -} \ No newline at end of file +} diff --git a/src/detectorTowerLiftAxis.h b/src/detectorTowerLiftAxis.h index 4f6ffa4..4b35e90 100644 --- a/src/detectorTowerLiftAxis.h +++ b/src/detectorTowerLiftAxis.h @@ -110,6 +110,10 @@ class HIDDEN detectorTowerLiftAxis : public turboPmacAxis { */ virtual detectorTowerController *pController() override { return pC_; }; + // True, if this axis has received a move command and the driver is + // currently starting a deferred movement. + bool waitingForStart_; + protected: detectorTowerController *pC_; detectorTowerAngleAxis *angleAxis_;