diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp
index 2137772..896c050 100644
--- a/src/masterMacsAxis.cpp
+++ b/src/masterMacsAxis.cpp
@@ -272,7 +272,7 @@ asynStatus masterMacsAxis::init() {
                                           __PRETTY_FUNCTION__, __LINE__);
     }
 
-    status = setVeloFields(motVelocity, 0.0, motVmax);
+    status = setVeloFields(abs(motVelocity), 0.0, motVmax);
     if (status != asynSuccess) {
         return status;
     }
@@ -535,26 +535,10 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
         return rwStatus;
     }
 
-    // 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.
-    if (pMasterMacsA_->targetReachedUninitialized) {
-        *moving = false;
-    } else {
-        if (targetReached() || !switchedOn()) {
-            *moving = false;
-        } else {
-            *moving = true;
-        }
-    }
-
-    if (targetReached()) {
-        pMasterMacsA_->targetReachedUninitialized = false;
-    }
-
     // If moving in velocity mode, read out the current speed. Otherwise, read
     // out the current position
-    if (pMasterMacsA_->lastMoveCommand == velocityMode && *moving) {
+    if (pMasterMacsA_->lastMoveCommand == velocityMode && !speedEqualZero()) {
+
         rwStatus = pC_->read(axisNo_, 14, response);
         if (rwStatus != asynSuccess) {
             return rwStatus;
@@ -565,7 +549,14 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
                                               __PRETTY_FUNCTION__, __LINE__);
         }
 
+        // Write the actual velocity to the paramLib
+        setAxisParamChecked(this, motorVelocity, actualVelocity);
+
+        // Motor is moving in velocity mode
+        *moving = true;
+
     } else {
+
         rwStatus = pC_->read(axisNo_, 12, response);
         if (rwStatus != asynSuccess) {
             return rwStatus;
@@ -575,6 +566,28 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
             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.
+        if (pMasterMacsA_->targetReachedUninitialized) {
+            *moving = false;
+        } else {
+            if (targetReached() || !switchedOn()) {
+                *moving = false;
+            } else {
+                *moving = true;
+            }
+        }
+
+        if (targetReached()) {
+            pMasterMacsA_->targetReachedUninitialized = false;
+        }
     }
 
     /*
@@ -789,14 +802,6 @@ asynStatus masterMacsAxis::doPoll(bool *moving) {
     setAxisParamChecked(this, motorStatusDone, !(*moving));
     setAxisParamChecked(this, motorStatusDirection, direction);
 
-    plStatus = setMotorPosition(currentPosition);
-    if (plStatus != asynSuccess) {
-        return plStatus;
-    }
-
-    // Write the actual velocity to the paramLib
-    setAxisParamChecked(this, motorVelocity, actualVelocity);
-
     return poll_status;
 }
 
@@ -1352,6 +1357,8 @@ bool masterMacsAxis::remoteMode() { return pMasterMacsA_->axisStatus[9]; }
 
 bool masterMacsAxis::targetReached() { return pMasterMacsA_->axisStatus[10]; }
 
+bool masterMacsAxis::speedEqualZero() { return pMasterMacsA_->axisStatus[12]; }
+
 bool masterMacsAxis::internalLimitActive() {
     return pMasterMacsA_->axisStatus[11];
 }
diff --git a/src/masterMacsAxis.h b/src/masterMacsAxis.h
index 559a633..eff68cf 100644
--- a/src/masterMacsAxis.h
+++ b/src/masterMacsAxis.h
@@ -250,6 +250,12 @@ class HIDDEN masterMacsAxis : public sinqAxis {
      */
     bool internalLimitActive();
 
+    /**
+     * @brief Read the property from axisStatus (see masterMacsAxisImpl
+     * redefinition in masterMacsAxis.cpp)
+     */
+    bool speedEqualZero();
+
     // Bits 12 and 13 are unused
 
     /**