diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp
index 740ba52..da11fa9 100644
--- a/src/masterMacsAxis.cpp
+++ b/src/masterMacsAxis.cpp
@@ -455,7 +455,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     double previousPosition = 0.0;
     double motorRecResolution = 0.0;
     double handshakePerformed = 0;
-    double actualVelocity = 0.0;
 
     // =========================================================================
 
@@ -532,10 +531,28 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
         return rwStatus;
     }
 
-    // If moving in velocity mode, read out the current speed. Otherwise, read
-    // out the current position
+    rwStatus = pC_->read(axisNo_, 12, response);
+    if (rwStatus != asynSuccess) {
+        return rwStatus;
+    }
+    nvals = sscanf(response, "%lf", &currentPosition);
+    if (nvals != 1) {
+        return pC_->couldNotParseResponse("R12", response, axisNo_,
+                                          __PRETTY_FUNCTION__, __LINE__);
+    }
+
+    plStatus = setMotorPosition(currentPosition);
+    if (plStatus != asynSuccess) {
+        return plStatus;
+    }
+    setAxisParamChecked(this, motorEncoderPosition, currentPosition);
+
     if (pMasterMacsA_->lastMoveCommand == velocityMode && !speedEqualZero()) {
 
+        // TODO: Not sure whether the RVEL field of the motor record does not
+        // work - has to be clarified
+        double actualVelocity = 0.0;
+
         rwStatus = pC_->read(axisNo_, 14, response);
         if (rwStatus != asynSuccess) {
             return rwStatus;
@@ -554,21 +571,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
 
     } else {
 
-        rwStatus = pC_->read(axisNo_, 12, response);
-        if (rwStatus != asynSuccess) {
-            return rwStatus;
-        }
-        nvals = sscanf(response, "%lf", &currentPosition);
-        if (nvals != 1) {
-            return pC_->couldNotParseResponse("R12", response, axisNo_,
-                                              __PRETTY_FUNCTION__, __LINE__);
-        }
-
-        plStatus = setMotorPosition(currentPosition);
-        if (plStatus != asynSuccess) {
-            return plStatus;
-        }
-
         // If we wait for a handshake, but the motor was moving in its last poll
         // cycle and has reached its target, it is not moving. Otherwise it is
         // considered moving, even if we're still waiting for the handshake.