diff --git a/src/masterMacsAxis.cpp b/src/masterMacsAxis.cpp
index f23115a..254040e 100644
--- a/src/masterMacsAxis.cpp
+++ b/src/masterMacsAxis.cpp
@@ -1179,40 +1179,25 @@ asynStatus masterMacsAxis::enable(bool on) {
 }
 
 asynStatus masterMacsAxis::setMode(int mode) {
-    // masterMacs can be disabled
-    if (pasynUser->reason == motorSetMode()) {
 
-        // First call to the sinqController function. It checks whether it is
-        // possible to set the mode and whether the given value is valid.
-        asynStatus status = sinqController::writeInt32(pasynUser, value);
-        if (status == asynSuccess) {
+    char command[pC_->MAXBUF_] = {0};
 
-            // Now write to the hardware
-            char command[MAXBUF_];
-            int axisNo;
-            getAddress(pasynUser, &axisNo);
-
-            // Map the EPICS value to MasterMACS values (see
-            // MasterMACS_manual.pdf).
-            int adjustedValue = 0;
-            if (value == 0) {
-                adjustedValue = 1;
-            } else if (value == 1) {
-                adjustedValue = 3;
-            } else {
-                // This branch is unreachable, as it is is already checked
-                // within sinqController::writeInt32 that value is either 0
-                // or 1.
-                return asynError;
-            }
-
-            snprintf(command, sizeof(value), "%d", adjustedValue);
-            return write(axisNo, 07, command);
-        }
-        return status;
+    // Map the EPICS value to MasterMACS values (see
+    // MasterMACS_manual.pdf).
+    int adjustedMode = 0;
+    if (mode == 0) {
+        adjustedMode = 1;
+    } else if (mode == 1) {
+        adjustedMode = 3;
     } else {
-        return sinqController::writeInt32(pasynUser, value);
+        // This branch is unreachable, as it is is already checked
+        // within sinqController::writeInt32 that value is either 0
+        // or 1.
+        return asynError;
     }
+
+    snprintf(command, sizeof(command), "%d", adjustedMode);
+    return pC_->write(axisNo(), 07, command);
 }
 
 bool masterMacsAxis::needInit() { return pMasterMacsA_->needInit; }
diff --git a/src/masterMacsAxis.h b/src/masterMacsAxis.h
index 5759479..13f1dc5 100644
--- a/src/masterMacsAxis.h
+++ b/src/masterMacsAxis.h
@@ -137,7 +137,7 @@ class HIDDEN masterMacsAxis : public sinqAxis {
      * @param mode
      * @return asynStatus
      */
-    asynStatus masterMacsAxis::setMode(int mode);
+    asynStatus setMode(int mode);
 
     /**
      * @brief Read the encoder type (incremental or absolute) for this axis