diff --git a/sinqEPICSApp/src/MasterMACSDriver.cpp b/sinqEPICSApp/src/MasterMACSDriver.cpp
index 24c7909..ca4f54a 100644
--- a/sinqEPICSApp/src/MasterMACSDriver.cpp
+++ b/sinqEPICSApp/src/MasterMACSDriver.cpp
@@ -3,7 +3,7 @@
  For documentation see the standard SINQ place for hardware documentation or 
  Marcel Schildt
 
- Mark Koennecke, March 2023
+ Mark Koennecke, March-August 2023
 */
 
 
@@ -13,6 +13,7 @@
 #include <math.h>
 #include <ctype.h>
 #include <time.h>
+#include <unistd.h>
 
 #include <iocsh.h>
 #include <epicsThread.h>
@@ -23,8 +24,6 @@
 #include "MasterMACSDriver.h"
 #include <epicsExport.h>
 
-#define IDLEPOLL 60
-
 #define CHECK_BIT(var,pos) ((var) & (1 << pos))
 #define ABS(x) (x < 0 ? -(x) : (x))
 
@@ -37,8 +36,8 @@
   */
 MasterMACSController::MasterMACSController(const char *portName,
 					   const char *MasterMACSPortName,
-					   int
-					   numAxes):SINQController
+					   int numAxes,
+					   int idlePoll, int busyPoll):SINQController
     (portName, MasterMACSPortName, numAxes)
 {
     asynStatus status;
@@ -66,8 +65,10 @@ MasterMACSController::MasterMACSController(const char *portName,
     createParam(EnableAxisString, asynParamInt32, &enableAxis_);
     createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
     callParamCallbacks();
-
-    startPoller(1000. / 1000., IDLEPOLL, 2);
+	    
+    startPoller(busyPoll/1000., idlePoll/1000., 1);
+    setIdlePollPeriod(idlePoll/1000.);
+    setMovingPollPeriod(busyPoll/1000.);
 }
 
 
@@ -76,13 +77,15 @@ MasterMACSController::MasterMACSController(const char *portName,
   * \param[in] portName          The name of the asyn port that will be created for this driver
   * \param[in] MasterMACSPortName       The name of the drvAsynIPPPort that was created previously to connect to the MasterMACS controller 
   * \param[in] numAxes           The number of axes that this controller supports 
+  * \param[in] idlePoll Poll interval when idle in microseconds
+  * \param[in] busyPoll Poll interval when moving in microSeconds
   */
 extern "C" int
 MasterMACSCreateController(const char *portName,
-			   const char *MasterMACSPortName, int numAxes)
+			   const char *MasterMACSPortName, int numAxes, int idlePoll, int busyPoll)
 {
     MasterMACSController *pMasterMACSController
-	= new MasterMACSController(portName, MasterMACSPortName, numAxes);
+	= new MasterMACSController(portName, MasterMACSPortName, numAxes, idlePoll, busyPoll);
     pMasterMACSController = NULL;
     return (asynSuccess);
 }
@@ -147,7 +150,7 @@ asynStatus
     /* read with a short timeout in order to remove duplicate messages 
      * from the line. This also serves to slow down communication
      */
-    pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .4, &in, &reason); 
+    pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .05, &in, &reason); 
 
     /* pack data for MasterMACS */
     len = strlen(command) + 6;
@@ -166,7 +169,9 @@ asynStatus
     /* 0x03 is appended by asyn */
 
     /* send the stuff away ... */
-    errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
+    if(debug) {
+    	errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
+    }
 
     status =
 	pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
@@ -175,8 +180,8 @@ asynStatus
     if (status != asynSuccess) {
 	if (axis != NULL) {
 	    errlogSevPrintf(errlogMajor,
-			    "Lost connection to motor controller, reason %d",
-			    reason);
+			    "Lost connection to motor controller,a axis %d, reason %d",
+			    axisNo, reason);
 	    axis->updateMsgTxtFromDriver
 		("Lost connection to motor controller");
 	    return status;
@@ -252,6 +257,8 @@ asynStatus
     char command[64] = { 0 };
     char response[64] = { 0 };
     int devStatus;
+    bool moving;
+    time_t startTime;
 
     pAxis = (MasterMACSAxis *) this->getAxis(pasynUser);
     if (!pAxis) {
@@ -268,9 +275,10 @@ asynStatus
         */
         devStatus = pAxis->readStatus();
         if(devStatus < 900){
+	    errlogPrintf("MMACS: Motor %d not ready to switch on\n", pAxis->axisNo_);		
             return asynError;
         }
-	    if (value == 1 && !pAxis->isOn(devStatus) ) {
+        if (value == 1 && !pAxis->isOn(devStatus) ) {
 	        /* download parameters, does not work as of now */
 	        /*
 	        sprintf(command, "%dS85=1.", pAxis->axisNo_);
@@ -280,64 +288,36 @@ asynStatus
 	        /*  actually enable */
 	        sprintf(command, "%dS04=1", pAxis->axisNo_);
 	        status = transactController(pAxis->axisNo_, command, response);
-	    } else {
-           if(pAxis->isOn(devStatus)) {
-                // only send command when necessary
-	            sprintf(command, "%dS04=0", pAxis->axisNo_);
-	            status = transactController(pAxis->axisNo_, command, response);
-            }
+        } else if(pAxis->isOn(devStatus)) {
+        	        // only send command when necessary
+	        	sprintf(command, "%dS04=0", pAxis->axisNo_);
+	            	status = transactController(pAxis->axisNo_, command, response);
+	} else {
+		// nothing to do
+		return status;
 	}
 	if (status == asynSuccess) {
 	    pAxis->updateMsgTxtFromDriver("");
 	} else {
-	    errlogPrintf("Failure to enable or disable axis %d",
+	    errlogPrintf("MMACS: Failure to enable or disable axis %d",
 			 pAxis->axisNo_);
 	}
+	startTime = time(NULL);
+	while(time(NULL) < startTime + 2.){
+		// wait for the change to happen
+		devStatus = pAxis->readStatus();
+		if(pAxis->isOn(devStatus) == value){
+            pAxis->active = true;
+			pAxis->poll(&moving); // to update the Enable_RBV field
+			return asynSuccess;
+		}
+                usleep(200);
+	}
+	errlogPrintf("MMACS: Failed to enable motor %d within 2 seconds\n", pAxis->axisNo_); 
     }
     return asynMotorController::writeInt32(pasynUser, value);
 }
 
-asynStatus
-    MasterMACSController::readInt32(asynUser * pasynUser,
-				    epicsInt32 * value)
-{
-
-    int function = pasynUser->reason;
-    MasterMACSAxis *pAxis = NULL;
-    char command[128] = { 0 }, reply[128] = {
-    0}, *pPtr;
-    float fval;
-    int devStatus, isOn, comStatus;
-
-    pAxis = (MasterMACSAxis *) (this->getAxis(pasynUser));
-    if (!pAxis) {
-	return asynError;
-    }
-    if (function == axisEnabled_) {
-	// Read the overall  status of this motor */
-	sprintf(command, "%dR10", pAxis->axisNo_);
-	comStatus = transactController(pAxis->axisNo_, command, reply);
-	if (comStatus == asynError) {
-	    return asynError;
-	}
-	pPtr = strstr(reply, "=");
-    if(pPtr) {
-	   sscanf(pPtr + 1, "%f", &fval);
-       devStatus = (int) fval;
-    } else {
-      devStatus = 0;
-      errlogPrintf(" Bad reply %s when trying to read motor status %d\n", reply, pAxis->axisNo_);      
-    }
-	isOn = pAxis->isOn(devStatus);
-	/* errlogPrintf("isOn in readInt32: %d, devStatus = %d\n", isOn, devStatus); */
-	setIntegerParam(axisEnabled_, isOn);
-	*value = isOn;
-	callParamCallbacks();
-	return asynSuccess;
-    }
-    return asynMotorController::readInt32(pasynUser, value);
-}
-
 
 // These are the MasterMACSAxis methods
 
@@ -351,9 +331,8 @@ MasterMACSAxis::MasterMACSAxis(MasterMACSController * pC, int axisNo):SINQAxis(p
 pC_
 (pC)
 {
-    next_poll = -1;
     hasStarted = false;
-    errorReported = 1;
+    active = false;
 }
 
 /** Reports on status of the axis
@@ -373,17 +352,27 @@ int MasterMACSAxis::readStatus()
 { 
     char command[COMLEN], reply[COMLEN], *pPtr;
     float fval;
-    int devStatus, status;
+    int status;
 
+    /*
+     * The MasterMACS sends invalid responses with a low frequency.
+     * Therefore I send cached status responses in such a case in order 
+     * to help the logic evrywhere else in the code.
+     */
     sprintf(command, "%dR10", axisNo_);
     status = pC_->transactController(axisNo_, command, reply);
     if (status == asynError) {
-	    return -1000;
+	    return oldStatus;
     }
     pPtr = strstr(reply, "=");
-    sscanf(pPtr + 1, "%f", &fval);
-    devStatus = (int) fval;
-    return devStatus;
+    if(pPtr) {
+        sscanf(pPtr + 1, "%f", &fval);
+        oldStatus = (int) fval;
+        return oldStatus;
+    } else { 
+        errlogPrintf("MMACS: invalid status reponse %s on axis %d\n", reply, axisNo_);
+        return oldStatus;
+    }
 }
 
 
@@ -463,9 +452,8 @@ asynStatus
 	return status;
     }
     hasStarted = true;
-    next_poll = -1;
     homing = 0;
-    errorReported = 0;
+    active = true;
 
     return status;
 }
@@ -496,14 +484,13 @@ asynStatus
 
     setIntegerParam(pC_->motorStatusProblem_, false);
     updateMsgTxtFromDriver("");
-    errorReported = 0;
 
 
     sprintf(command, "%dS00=9", axisNo_);
     homing = 1;
-    next_poll = -1;
     status = pC_->transactController(axisNo_, command, reply);
     hasStarted = true;
+    active = true;
     return status;
 }
 
@@ -525,16 +512,13 @@ asynStatus MasterMACSAxis::stop(double acceleration)
 
     memset(command, 0, COMLEN * sizeof(char));
 
-    if (errorReported == 0) {
-        devStatus = readStatus();
-        if(!CHECK_BIT(devStatus, 10)) {
-            // only try to stop when running ...    
-	        sprintf(command, "%dS00=8", axisNo_);
-	        status = pC_->transactController(axisNo_, command, reply);
-	        errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
-	        updateMsgTxtFromDriver("Axis interrupted");
-	        errorReported = 1;
-       }
+    devStatus = readStatus();
+    if(!CHECK_BIT(devStatus, 10)) {
+        // only try to stop when running ...    
+	    sprintf(command, "%dS00=8", axisNo_);
+	    status = pC_->transactController(axisNo_, command, reply);
+	    errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
+	    updateMsgTxtFromDriver("Axis interrupted");
     }
     return status;
 }
@@ -567,21 +551,33 @@ asynStatus MasterMACSAxis::setClosedLoop(bool closedLoop)
 asynStatus MasterMACSAxis::poll(bool * moving)
 {
     asynStatus comStatus = asynSuccess;
-    char command[COMLEN], reply[COMLEN], *pPtr;
+    char command[COMLEN], reply[COMLEN], *pPtr, buffer[80];
     float errStatus;
     unsigned int errCode, derCode, devStatus;
 
-    // protect against excessive polling
-    if (time(NULL) < next_poll) {
-	*moving = false;
-	return asynSuccess;
+    struct tm* tm_info;
+    time_t timer;
+
+    /* 
+     * EPICS always polls all motors on a controller when one motor is active.
+     * In order to reduce load on the controller we check if we are active and
+     * if not return old stuff
+     */
+    if(active == false && time(NULL) < lastPoll + pC_->idlePollPeriod_) {
+        *moving = false;
+        return asynSuccess;
     }
 
+    timer = time(NULL);
+    tm_info = localtime(&timer);
+
+    strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
+    errlogPrintf("poll called at %s on axis %d \n", 
+		 buffer, axisNo_ );
+
     setIntegerParam(pC_->motorStatusProblem_, false);
     memset(command, 0, COMLEN * sizeof(char));
 
-
-
     // Read the current motor position
     sprintf(command, "%dR12", axisNo_);
     comStatus = pC_->transactController(axisNo_, command, reply);
@@ -595,12 +591,20 @@ asynStatus MasterMACSAxis::poll(bool * moving)
     } else {
 	errlogPrintf("Received malformed reply: Axis %d, reply %s\n",
 		     axisNo_, reply + 4);
-	return asynError;
+	comStatus = asynError;
+	goto skip;
     }
 
     setDoubleParam(pC_->motorPosition_, position * 1000.);
     setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
 
+    /* 
+     * keep track of position in order to check for a stuck motor later
+     */
+    if(ABS(position - oldPosition) > 1){
+        oldPosition = position;
+        lastPositionUpdate = time(NULL);
+    }
 
     // Read the overall  status of this motor */
     devStatus = readStatus();
@@ -614,6 +618,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
 	             position, devStatus);
     }
 
+    setIntegerParam(pC_->axisEnabled_, isOn(devStatus));
     if (!isOn(devStatus)) {
 	    setIntegerParam(pC_->motorStatusProblem_, true);
 	    updateMsgTxtFromDriver("Motor disabled");
@@ -628,7 +633,6 @@ asynStatus MasterMACSAxis::poll(bool * moving)
     if (!hasStarted) {
 	*moving = false;
 	setIntegerParam(pC_->motorStatusDone_, true);
-	next_poll = time(NULL) + IDLEPOLL;
 	goto skip;
     }
 
@@ -636,18 +640,23 @@ asynStatus MasterMACSAxis::poll(bool * moving)
      * We may have a valid status bit...
      */
     if (!CHECK_BIT(devStatus, 10)) {
-	/* we are still creeping along .... */
-	*moving = true;
-	next_poll = -1;
-	setIntegerParam(pC_->motorStatusDone_, false);
-	goto skip;
+    	/* we are still creeping along .... */
+	    *moving = true;
+	    setIntegerParam(pC_->motorStatusDone_, false);
+        if(time(NULL) > lastPositionUpdate + 60) {
+            // we are detecting a stuck motor
+            errlogPrintf("MMACS: axis %d is STUCK!!\n", axisNo_);
+            updateMsgTxtFromDriver("Axis STUCK!!");
+	        setIntegerParam(pC_->motorStatusProblem_, true);
+        }
+	    goto skip;
     }
 
 
     /*we are done moving */
     *moving = false;
+    active = false;
     setIntegerParam(pC_->motorStatusDone_, true);
-    next_poll = time(NULL) + IDLEPOLL;
 
     /* when homing, set the proper flag */
     if (homing) {
@@ -655,7 +664,7 @@ asynStatus MasterMACSAxis::poll(bool * moving)
     }
 
     /* check for limit switches*/
-	setIntegerParam(pC_->motorStatusLowLimit_, false);
+    setIntegerParam(pC_->motorStatusLowLimit_, false);
     setIntegerParam(pC_->motorStatusHighLimit_, false);
     if (CHECK_BIT(devStatus, 11)) {
 	    setIntegerParam(pC_->motorStatusProblem_, true);
@@ -685,9 +694,15 @@ asynStatus MasterMACSAxis::poll(bool * moving)
 	        goto skip;
         }
         pPtr = strstr(reply, "=");
-        sscanf(pPtr + 1, "%f", &errStatus);
-        errCode = (unsigned int) errStatus;
-
+        if(pPtr) {
+            sscanf(pPtr + 1, "%f", &errStatus);
+            errCode = (unsigned int) errStatus;
+        } else {
+            errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
+            updateMsgTxtFromDriver("Invalid reply asking error code R11");
+            errCode = 0;
+            goto skip;
+        }
         sprintf(command, "%dR18", axisNo_);
         comStatus = pC_->transactController(axisNo_, command, reply);
         if (comStatus == asynError) {
@@ -695,8 +710,14 @@ asynStatus MasterMACSAxis::poll(bool * moving)
 	        goto skip;
         }
         pPtr = strstr(reply, "=");
-        sscanf(pPtr + 1, "%f", &errStatus);
-        derCode = (unsigned int) errStatus;
+        if(pPtr) {
+            sscanf(pPtr + 1, "%f", &errStatus);
+            derCode = (unsigned int) errStatus;
+        } else {
+            errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
+            derCode = 0;
+            updateMsgTxtFromDriver("Invalid reply asking error code R18");
+        }
 
         if(debug) {
 		errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
@@ -784,18 +805,28 @@ static const iocshArg
 static const iocshArg
     MasterMACSCreateControllerArg2 = { "Number of axes", iocshArgInt };
 
+static const iocshArg
+    MasterMACSCreateControllerArg3 = { "idlePoll", iocshArgInt };
+
+static const iocshArg
+    MasterMACSCreateControllerArg4 = { "busyPoll", iocshArgInt };
+
+
 static const iocshArg *const
     MasterMACSCreateControllerArgs[] = { &MasterMACSCreateControllerArg0,
     &MasterMACSCreateControllerArg1,
-    &MasterMACSCreateControllerArg2
+    &MasterMACSCreateControllerArg2,
+    &MasterMACSCreateControllerArg3,
+    &MasterMACSCreateControllerArg4
 };
 
 static const iocshFuncDef
     MasterMACSCreateControllerDef =
-    { "MasterMACSCreateController", 3, MasterMACSCreateControllerArgs };
+    { "MasterMACSCreateController", 5, MasterMACSCreateControllerArgs };
 static void MasterMACSCreateContollerCallFunc(const iocshArgBuf * args)
 {
-    MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival);
+    MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival, 
+		    	       args[3].ival, args[4].ival);
 }
 
 /**
diff --git a/sinqEPICSApp/src/MasterMACSDriver.h b/sinqEPICSApp/src/MasterMACSDriver.h
index e51430a..d63bd13 100644
--- a/sinqEPICSApp/src/MasterMACSDriver.h
+++ b/sinqEPICSApp/src/MasterMACSDriver.h
@@ -33,13 +33,18 @@ private:
   MasterMACSController *pC_;          /**< Pointer to the asynMotorController to which this axis belongs.
                                    *   Abbreviated because it is used very frequently */
   double position;
+  double oldPosition;
+  time_t lastPositionUpdate;
   int homing;
-  time_t next_poll;
-  int errorReported;
   int hasStarted; /* The motor status is invalid if the thing has not run once */
   int isOn(int axisStatus); 
   int readStatus();
   int errorCodeFound;
+  int oldStatus;
+
+  bool active;
+  time_t lastPoll;
+
 friend class MasterMACSController;
 };
 
@@ -49,7 +54,8 @@ friend class MasterMACSController;
 
 class MasterMACSController : public SINQController {
 public:
-  MasterMACSController(const char *portName, const char *MasterMACSPortName, int numAxes);
+  MasterMACSController(const char *portName, const char *MasterMACSPortName, 
+		       int numAxes, int idlePoll, int busyPoll);
 
   void report(FILE *fp, int level);
   MasterMACSAxis* getAxis(asynUser *pasynUser);
@@ -58,9 +64,6 @@ public:
   // overloaded because we want to enable/disable the motor
   asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
 
-  // overloaded because we want to read the axis state
-  asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
- 
 friend class MasterMACSAxis;
  private:
  asynUser *pasynUserController_;
diff --git a/sinqEPICSApp/src/NanotecDriver.cpp b/sinqEPICSApp/src/NanotecDriver.cpp
index e564be3..f630d17 100644
--- a/sinqEPICSApp/src/NanotecDriver.cpp
+++ b/sinqEPICSApp/src/NanotecDriver.cpp
@@ -377,7 +377,7 @@ asynStatus NanotecAxis::poll(bool *moving)
   asynStatus comStatus;
   char command[COMLEN], reply[COMLEN];
   char *pPtr; 
-  int posVal, statVal;
+  int posVal, statVal, count = 0;
   double lowLim, highLim;
 
 
@@ -393,11 +393,12 @@ asynStatus NanotecAxis::poll(bool *moving)
   if(comStatus) goto skip;
 
   pPtr = strchr(reply,'C');
-  pPtr++;
   if(pPtr){
-    posVal = atoi(pPtr);
-  } else {
-    errlogPrintf("Invalid response %s for #C received for axis %d\n", reply, axisNo_);
+    pPtr++;
+    count = sscanf(pPtr,"%d", &posVal); 	  
+  } 
+  if(pPtr == NULL || count < 1) {
+    errlogPrintf("Invalid response %s for #C received for axis %d, address %d\n", reply, axisNo_, busAddress);
     return asynError;
   }
 
@@ -412,11 +413,12 @@ asynStatus NanotecAxis::poll(bool *moving)
   if(comStatus) goto skip;
 
   pPtr = strchr(reply,'$');
-  pPtr++;
   if(pPtr) {
-    statVal = atoi(pPtr);
-  } else {
-     errlogPrintf("Invalid response %s for #$ received for axis %d\n", reply, axisNo_);
+    pPtr++;
+    count = sscanf(pPtr, "%d", &statVal);	  
+  } 
+  if(pPtr == NULL || count < 1) {
+     errlogPrintf("Invalid response %s for #$ received for axis %d busAddress %d\n", reply, axisNo_, busAddress);
      return asynError;
   }        
   //errlogPrintf("Axis %d, reply %s, statVal = %d\n", 
diff --git a/sinqEPICSApp/src/pmacAxis.cpp b/sinqEPICSApp/src/pmacAxis.cpp
index 3b19af2..f9afb08 100644
--- a/sinqEPICSApp/src/pmacAxis.cpp
+++ b/sinqEPICSApp/src/pmacAxis.cpp
@@ -185,7 +185,7 @@ asynStatus pmacAxis::getAxisInitialStatus(void) {
     cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
     if (cmdStatus) {
       asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
-                "%s: Error: enaabling axis %d failed.\n", functionName,
+                "%s: Error: enabling axis %d failed.\n", functionName,
                 axisNo_);
       return asynError;
     }
@@ -911,11 +911,6 @@ asynStatus pmacV3Axis::poll(bool *moving) {
   static const char *functionName = "pmacV3Axis::poll";
   char message[132];
 
-  // Protect against excessive polling
-  if (time(NULL) < next_poll) {
-    return asynSuccess;
-  }
-
   sprintf(message, "%s: Polling axis: %d", functionName, this->axisNo_);
   pC_->debugFlow(message);
 
@@ -927,12 +922,6 @@ asynStatus pmacV3Axis::poll(bool *moving) {
               functionName, pC_->portName, axisNo_);
   }
 
-  if (*moving) {
-    next_poll = time(NULL) + BUSYPOLL;
-  } else {
-    next_poll = time(NULL) + IDLEPOLL;
-  }
-
   callParamCallbacks();
   return status ? asynError : asynSuccess;
 }
@@ -966,7 +955,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
   pmacV3Controller *p3C_ = (pmacV3Controller *)pC_;
   IsEnable = axStat != -3;
 
-  asynStatus st = setIntegerParam(p3C_->axisEnabled_, axStat > -1);
+  asynStatus st = setIntegerParam(p3C_->axisEnabled_, IsEnable);
   cmdStatus = cmdStatus > st ? cmdStatus : st;
 
   int direction = 0;
@@ -993,7 +982,8 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
   previous_direction_ = direction;
 
   /* are we done? */
-  if ((axStat == 0 || axStat < 0) && starting == 0) {
+  /* if ((axStat == 0 || axStat < 0) && starting == 0) { */
+  if (axStat == 0 && starting == 0) {
     done = 1;
   } else {
     starting = 0;
diff --git a/sinqEPICSApp/src/pmacController.cpp b/sinqEPICSApp/src/pmacController.cpp
index 94c48ac..6109b4c 100644
--- a/sinqEPICSApp/src/pmacController.cpp
+++ b/sinqEPICSApp/src/pmacController.cpp
@@ -1,24 +1,24 @@
 /********************************************
- *  pmacController.cpp
- * 
- *  PMAC Asyn motor based on the 
- *  asynMotorController class.
- * 
- *  Matthew Pearson
- *  23 May 2012
- * 
- * 
- * Substantially modified for use at SINQ at PSI. 
- * The thing with the PMACS is that they can be programmed.
- * This affects also the commands they understand. 
- *
- * Mark Koennecke, February 2013 
- *
- * Updated to use the MsgTxt field for error messages as 
- * used at ESS and SINQ 
- *
- * Mark Koennecke, January 2019
- ********************************************/
+*  pmacController.cpp
+* 
+*  PMAC Asyn motor based on the 
+*  asynMotorController class.
+* 
+*  Matthew Pearson
+*  23 May 2012
+* 
+* 
+* Substantially modified for use at SINQ at PSI. 
+* The thing with the PMACS is that they can be programmed.
+* This affects also the commands they understand. 
+*
+* Mark Koennecke, February 2013 
+*
+* Updated to use the MsgTxt field for error messages as 
+* used at ESS and SINQ 
+*
+* Mark Koennecke, January 2019
+********************************************/
 
 
 
@@ -27,8 +27,8 @@
 #include <string.h>
 #include <math.h>
 #include <errno.h>
-
 #include <iostream>
+#include <unistd.h>
 using std::cout;
 using std::endl;
 
@@ -129,387 +129,389 @@ const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB2 = (PMAC_STATUS2_DESIRE
 
 //C function prototypes, for the functions that can be called on IOC shell
 extern "C" {
-  asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
-					 int numAxes, int movingPollPeriod, int idlePollPeriod);
-  asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
-					 int numAxes, int movingPollPeriod, int idlePollPeriod);
-  asynStatus pmacV3CreateController(const char *portName,
-                                  const char *lowLevelPortName,
-                                  int lowLevelPortAddress, int numAxes,
-                                  int movingPollPeriod, int idlePollPeriod);
-  asynStatus pmacCreateAxis(const char *pmacName, int axis);
-  asynStatus pmacCreateAxis(const char *pmacName, int numAxis);
-  
+asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
+				 int numAxes, int movingPollPeriod, int idlePollPeriod);
+asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
+				 int numAxes, int movingPollPeriod, int idlePollPeriod);
+asynStatus pmacV3CreateController(const char *portName,
+			  const char *lowLevelPortName,
+			  int lowLevelPortAddress, int numAxes,
+			  int movingPollPeriod, int idlePollPeriod);
+asynStatus pmacCreateAxis(const char *pmacName, int axis);
+asynStatus pmacCreateAxis(const char *pmacName, int numAxis);
+
 }
 
 pmacController::pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
-			       int numAxes, double movingPollPeriod, double idlePollPeriod, const int& extraParams)
-  : SINQController(portName, lowLevelPortName, numAxes+1, extraParams)
+		       int numAxes, double movingPollPeriod, double idlePollPeriod, const int& extraParams)
+: SINQController(portName, lowLevelPortName, numAxes+1, extraParams)
 {
-  static const char *functionName = "pmacController::pmacController";
+static const char *functionName = "pmacController::pmacController";
 
-  //Initialize non static data members
-  lowLevelPortUser_ = NULL;
-  debugFlag_ = 0;
+//Initialize non static data members
+lowLevelPortUser_ = NULL;
+debugFlag_ = 0;
 
-  pAxes_ = (pmacAxis **)(asynMotorController::pAxes_);
+pAxes_ = (pmacAxis **)(asynMotorController::pAxes_);
 
-  // Create controller-specific parameters
-  createParam(PMAC_C_CommsErrorString,         asynParamInt32, &PMAC_C_CommsError_);
+// Create controller-specific parameters
+createParam(PMAC_C_CommsErrorString,         asynParamInt32, &PMAC_C_CommsError_);
 
-  // Connect our Asyn user to the low level port that is a parameter to this constructor
-  if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", (char *)"\r") != asynSuccess) {
-    printf("%s: Failed to connect to low level asynOctetSyncIO port %s\n", functionName, lowLevelPortName);
-        setIntegerParam(PMAC_C_CommsError_, 1);
-  } else {
-    /* Create the poller thread for this controller
-     * NOTE: at this point the axis objects don't yet exist, but the poller tolerates this */
-    setIntegerParam(PMAC_C_CommsError_, 0);
-  }
-  startPoller(movingPollPeriod, idlePollPeriod, 10);
+// Connect our Asyn user to the low level port that is a parameter to this constructor
+if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", (char *)"\r") != asynSuccess) {
+printf("%s: Failed to connect to low level asynOctetSyncIO port %s\n", functionName, lowLevelPortName);
+setIntegerParam(PMAC_C_CommsError_, 1);
+} else {
+/* Create the poller thread for this controller
+* NOTE: at this point the axis objects don't yet exist, but the poller tolerates this */
+setIntegerParam(PMAC_C_CommsError_, 0);
+}
+startPoller(movingPollPeriod, idlePollPeriod, 10);
+
+callParamCallbacks();
 
-  callParamCallbacks();
- 
 }
 
 
 pmacController::~pmacController(void) 
 {
-  //Destructor
+//Destructor
 }
 
 
 /**
- * Connect to the underlying low level Asyn port that is used for comms.
- * This uses the asynOctetSyncIO interface, and also sets the input and output terminators.
- */
+* Connect to the underlying low level Asyn port that is used for comms.
+* This uses the asynOctetSyncIO interface, and also sets the input and output terminators.
+*/
 int pmacController::lowLevelPortConnect(const char *port, int addr, asynUser **ppasynUser, char *inputEos, char *outputEos)
 {
-  asynStatus status = asynSuccess;
- 
-  static const char *functionName = "pmacController::lowLevelPortConnect";
+asynStatus status = asynSuccess;
 
-  debugFlow(functionName);
+static const char *functionName = "pmacController::lowLevelPortConnect";
 
-  status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
-  if (status) {
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-	      "pmacController::motorAxisAsynConnect: unable to connect to port %s\n", 
-	      port);
-    return status;
-  }
+debugFlow(functionName);
 
-  //Do I want to disconnect below? If the IP address comes up, will the driver recover
-  //if the poller functions are running? Might have to use asynManager->isConnected to
-  //test connection status of low level port (in the pollers). But then autosave 
-  //restore doesn't work (and we would save wrong positions). So I need to 
-  //have a seperate function(s) to deal with connecting after IOC init.
+status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
+if (status) {
+asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+      "pmacController::motorAxisAsynConnect: unable to connect to port %s\n", 
+      port);
+return status;
+}
 
-  status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
-  if (status) {
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-	      "pmacController: unable to set input EOS on %s: %s\n", 
-	      port, (*ppasynUser)->errorMessage);
-    pasynOctetSyncIO->disconnect(*ppasynUser);
-    //Set my low level pasynUser pointer to NULL
-    *ppasynUser = NULL;
-    return status;
-  }
-  
-  status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
-  if (status) {
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-	      "pmacController: unable to set output EOS on %s: %s\n", 
-	      port, (*ppasynUser)->errorMessage);
-    pasynOctetSyncIO->disconnect(*ppasynUser);
-    //Set my low level pasynUser pointer to NULL
-    *ppasynUser = NULL;
-    return status;
-  }
-  
-  return status;
+//Do I want to disconnect below? If the IP address comes up, will the driver recover
+//if the poller functions are running? Might have to use asynManager->isConnected to
+//test connection status of low level port (in the pollers). But then autosave 
+//restore doesn't work (and we would save wrong positions). So I need to 
+//have a seperate function(s) to deal with connecting after IOC init.
+
+status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
+if (status) {
+asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+      "pmacController: unable to set input EOS on %s: %s\n", 
+      port, (*ppasynUser)->errorMessage);
+pasynOctetSyncIO->disconnect(*ppasynUser);
+//Set my low level pasynUser pointer to NULL
+*ppasynUser = NULL;
+return status;
+}
+
+status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
+if (status) {
+asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+      "pmacController: unable to set output EOS on %s: %s\n", 
+      port, (*ppasynUser)->errorMessage);
+pasynOctetSyncIO->disconnect(*ppasynUser);
+//Set my low level pasynUser pointer to NULL
+*ppasynUser = NULL;
+return status;
+}
+
+return status;
 }
 
 /**
- * Utilty function to print the connected status of the low level asyn port.
- */
+* Utilty function to print the connected status of the low level asyn port.
+*/
 asynStatus pmacController::printConnectedStatus()
 {
-  asynStatus status = asynSuccess;
-  int asynManagerConnected = 0;
-  
-  if (lowLevelPortUser_) {
-    status = pasynManager->isConnected(lowLevelPortUser_, &asynManagerConnected);
-      if (status) {
-      asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
-	      "pmacController: Error calling pasynManager::isConnected.\n");
-      return asynError;
-    } else {
-	printf("pmacController::printConnectedStatus: isConnected: %d\n", asynManagerConnected);
-    }
-  }
-  return asynSuccess;
+asynStatus status = asynSuccess;
+int asynManagerConnected = 0;
+
+if (lowLevelPortUser_) {
+status = pasynManager->isConnected(lowLevelPortUser_, &asynManagerConnected);
+if (status) {
+asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+      "pmacController: Error calling pasynManager::isConnected.\n");
+return asynError;
+} else {
+printf("pmacController::printConnectedStatus: isConnected: %d\n", asynManagerConnected);
+}
+}
+return asynSuccess;
 }
 
 /**
- * Wrapper for asynOctetSyncIO write/read functions.
- * @param command - String command to send.
- * @response response - String response back.
- */
+* Wrapper for asynOctetSyncIO write/read functions.
+* @param command - String command to send.
+* @response response - String response back.
+*/
 asynStatus pmacController::lowLevelWriteRead(int axisNo, const char *command, char *response)
 {
-  asynStatus status = asynSuccess;
+asynStatus status = asynSuccess;
 
-   int eomReason;
-   size_t nwrite = 0;
-   size_t nread = 0;
-   int commsError = 0;
-   static const char *functionName = "pmacController::lowLevelWriteRead";
-   pmacAxis *axis = getAxis(axisNo);
+int eomReason;
+size_t nwrite = 0;
+size_t nread = 0;
+int commsError = 0;
+static const char *functionName = "pmacController::lowLevelWriteRead";
+pmacAxis *axis = getAxis(axisNo);
 
-   debugFlow(functionName);
+debugFlow(functionName);
 
-   if (!lowLevelPortUser_) {
-     setIntegerParam(this->motorStatusCommsError_, 1);
-     return asynError;
-   }
+if (!lowLevelPortUser_) {
+setIntegerParam(this->motorStatusCommsError_, 1);
+return asynError;
+}
 
-   asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: command: %s\n", functionName, command);
-   debugFlow("Sending: ");
-   debugFlow(command);
+asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: command: %s\n", functionName, command);
+debugFlow("Sending: ");
+debugFlow(command);
 
-   //Make sure the low level port is connected before we attempt comms
-   //Use the controller-wide param PMAC_C_CommsError_
-   getIntegerParam(PMAC_C_CommsError_, &commsError);
+//Make sure the low level port is connected before we attempt comms
+//Use the controller-wide param PMAC_C_CommsError_
+getIntegerParam(PMAC_C_CommsError_, &commsError);
 
-   if (!commsError) {
-     status = pasynOctetSyncIO->writeRead(lowLevelPortUser_ ,
-					  command, strlen(command),
-					  response, PMAC_MAXBUF_,
-					  PMAC_TIMEOUT_,
-					  &nwrite, &nread, &eomReason );
-     
-     if (status) {
-       asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: Error from pasynOctetSyncIO->writeRead. command: %s\n", functionName, command);
-       setIntegerParam(this->motorStatusCommsError_, 1);
-       if(axis!= NULL){
-	 axis->updateMsgTxtFromDriver("Lost connection to motor controller");
-       }
-     } else {
-       setIntegerParam(this->motorStatusCommsError_, 0);
-     }
-   }
+if (!commsError) {
+status = pasynOctetSyncIO->writeRead(lowLevelPortUser_ ,
+				  command, strlen(command),
+				  response, PMAC_MAXBUF_,
+				  PMAC_TIMEOUT_,
+				  &nwrite, &nread, &eomReason );
 
-   asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: response: %s\n", functionName, response); 
-   debugFlow("Received: ");
-   debugFlow(response);
+if (status) {
+asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: Error from pasynOctetSyncIO->writeRead. command: %s\n", functionName, command);
+setIntegerParam(this->motorStatusCommsError_, 1);
+if(axis!= NULL){
+ axis->updateMsgTxtFromDriver("Lost connection to motor controller");
+}
+} else {
+setIntegerParam(this->motorStatusCommsError_, 0);
+}
+}
 
-  return status;
+usleep(20); // slow down communication somewhat 
+
+asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: response: %s\n", functionName, response); 
+debugFlow("Received: ");
+debugFlow(response);
+
+return status;
 }
 
 void pmacController::debugFlow(const char *message)
 {
-  if (debugFlag_ == 1) {
-    printf("  %s\n", message);
-  }
+if (debugFlag_ == 1) {
+printf("  %s\n", message);
+}
 
-  asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s\n", message);
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s\n", message);
 
 }
 
 
 void pmacController::report(FILE *fp, int level)
 {
-  int axis = 0;
-  pmacAxis *pAxis = NULL;
+int axis = 0;
+pmacAxis *pAxis = NULL;
 
-  fprintf(fp, "pmac motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", 
-          this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
+fprintf(fp, "pmac motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", 
+  this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);
 
-  if (level > 0) {
-    for (axis=0; axis<numAxes_; axis++) {
-      pAxis = getAxis(axis);
-      fprintf(fp, "  axis %d\n"
-                  "    scale = %lf\n", 
-              pAxis->axisNo_,
-              pAxis->scale_);
-    }
-  }
+if (level > 0) {
+for (axis=0; axis<numAxes_; axis++) {
+pAxis = getAxis(axis);
+fprintf(fp, "  axis %d\n"
+	  "    scale = %lf\n", 
+      pAxis->axisNo_,
+      pAxis->scale_);
+}
+}
 
-  // Call the base class method
-  asynMotorController::report(fp, level);
+// Call the base class method
+asynMotorController::report(fp, level);
 }
 
 
 asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
 {
-  int function = pasynUser->reason;
-  asynStatus status = asynError;
-  pmacAxis *pAxis = NULL;
-  char command[64] = {0};
-  char response[64] = {0};
-  char message[132];
+int function = pasynUser->reason;
+asynStatus status = asynError;
+pmacAxis *pAxis = NULL;
+char command[64] = {0};
+char response[64] = {0};
+char message[132];
+
+static const char *functionName = "pmacController::writeFloat64";
+
+sprintf(message,"%s, reason %d",  functionName, function);
+debugFlow(message);
+//debugFlow(functionName);
+
+pAxis = this->getAxis(pasynUser);
+if (!pAxis) {
+return asynError;
+}
+
+
+/* Set the parameter and readback in the parameter library. */
+status = pAxis->setDoubleParam(function, value);
+
+// if (function == motorPosition_) {
+//   /*Set position on motor axis.*/            
+//   epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);
+
+//   sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
+// 	     pAxis->axisNo_,
+// 	     pAxis->axisNo_, position, pAxis->axisNo_, 
+// 	     pAxis->axisNo_, position, pAxis->axisNo_ );
+
+//   asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
+// 	      "%s: Set axis %d on controller %s to position %f\n", 
+// 	      functionName, pAxis->axisNo_, portName, value);
+
+//   if ( command[0] != 0 && status == asynSuccess) {
+//     status = lowLevelWriteRead(command, response);
+//   }
 	
-  static const char *functionName = "pmacController::writeFloat64";
+//   sprintf(command, "#%dJ/", pAxis->axisNo_);
 
-  sprintf(message,"%s, reason %d",  functionName, function);
-  debugFlow(message);
-  //debugFlow(functionName);
+//   if (command[0] != 0 && status == asynSuccess) {
+//     status = lowLevelWriteRead(command, response);
+//   }
 
-  pAxis = this->getAxis(pasynUser);
-  if (!pAxis) {
-    return asynError;
-  }
+//   /*Now set position on encoder axis, if one is in use.*/
+	
+//   if (pAxis->encoder_axis_) {
+//     getDoubleParam(motorEncRatio_,  &encRatio);
+//     encposition = (epicsInt32) floor((position*encRatio) + 0.5);
+	  
+//     sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
+// 	      pAxis->encoder_axis_,
+// 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_, 
+// 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_ );
+	  
+//     asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
+// 		"%s: Set encoder axis %d on controller %s to position %f\n", 
+// 		functionName, pAxis->axisNo_, portName, value);
+	  
+//     if (command[0] != 0 && status == asynSuccess) {
+// 	status = lowLevelWriteRead(command, response);
+//     }
+	  
+//     sprintf(command, "#%dJ/", pAxis->encoder_axis_);
+//     //The lowLevelWriteRead will be done at the end of this function.
+//   }
 
+//   /*Now do an update, to get the new position from the controller.*/
+//   bool moving = true;
+//   pAxis->getAxisStatus(&moving);
+// } else 
+/*
+if (function == motorLowLimit_) {
+sprintf(command, "I%d14=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+      "%s: Setting low limit on controller %s, axis %d to %f\n",
+      functionName, portName, pAxis->axisNo_, value);
+} else if (function == motorHighLimit_) {
+sprintf(command, "I%d13=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+      "%s: Setting high limit on controller %s, axis %d to %f\n",
+      functionName, portName, pAxis->axisNo_, value);
+} 
 
-  /* Set the parameter and readback in the parameter library. */
-  status = pAxis->setDoubleParam(function, value);
+//Execute the command.
+if (command[0] != 0 && status == asynSuccess) {
+status = lowLevelWriteRead(pAxis->axisNo_,command, response);
+}
+*/
 
-  // if (function == motorPosition_) {
-  //   /*Set position on motor axis.*/            
-  //   epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);
-    
-  //   sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
-  // 	     pAxis->axisNo_,
-  // 	     pAxis->axisNo_, position, pAxis->axisNo_, 
-  // 	     pAxis->axisNo_, position, pAxis->axisNo_ );
+//Call base class method
+//This will handle callCallbacks even if the function was handled here.
+status = asynMotorController::writeFloat64(pasynUser, value);
 
-  //   asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
-  // 	      "%s: Set axis %d on controller %s to position %f\n", 
-  // 	      functionName, pAxis->axisNo_, portName, value);
-    
-  //   if ( command[0] != 0 && status == asynSuccess) {
-  //     status = lowLevelWriteRead(command, response);
-  //   }
-                
-  //   sprintf(command, "#%dJ/", pAxis->axisNo_);
-
-  //   if (command[0] != 0 && status == asynSuccess) {
-  //     status = lowLevelWriteRead(command, response);
-  //   }
-
-  //   /*Now set position on encoder axis, if one is in use.*/
-                
-  //   if (pAxis->encoder_axis_) {
-  //     getDoubleParam(motorEncRatio_,  &encRatio);
-  //     encposition = (epicsInt32) floor((position*encRatio) + 0.5);
-                  
-  //     sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
-  // 	      pAxis->encoder_axis_,
-  // 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_, 
-  // 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_ );
-                  
-  //     asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
-  // 		"%s: Set encoder axis %d on controller %s to position %f\n", 
-  // 		functionName, pAxis->axisNo_, portName, value);
-                  
-  //     if (command[0] != 0 && status == asynSuccess) {
-  // 	status = lowLevelWriteRead(command, response);
-  //     }
-                  
-  //     sprintf(command, "#%dJ/", pAxis->encoder_axis_);
-  //     //The lowLevelWriteRead will be done at the end of this function.
-  //   }
-
-  //   /*Now do an update, to get the new position from the controller.*/
-  //   bool moving = true;
-  //   pAxis->getAxisStatus(&moving);
-  // } else 
-  /*
-  if (function == motorLowLimit_) {
-    sprintf(command, "I%d14=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
-	      "%s: Setting low limit on controller %s, axis %d to %f\n",
-	      functionName, portName, pAxis->axisNo_, value);
-  } else if (function == motorHighLimit_) {
-    sprintf(command, "I%d13=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
-	      "%s: Setting high limit on controller %s, axis %d to %f\n",
-	      functionName, portName, pAxis->axisNo_, value);
-  } 
-
-  //Execute the command.
-  if (command[0] != 0 && status == asynSuccess) {
-    status = lowLevelWriteRead(pAxis->axisNo_,command, response);
-  }
-  */
-
-  //Call base class method
-  //This will handle callCallbacks even if the function was handled here.
-  status = asynMotorController::writeFloat64(pasynUser, value);
-
-  return status;
+return status;
 
 }
 
 
 asynStatus pmacController::writeInt32(asynUser *pasynUser, epicsInt32 value)
 {
-  int function = pasynUser->reason;
-  asynStatus status = asynError;
-  pmacAxis *pAxis = NULL;
-  static const char *functionName = "pmacController::writeInt32";
+int function = pasynUser->reason;
+asynStatus status = asynError;
+pmacAxis *pAxis = NULL;
+static const char *functionName = "pmacController::writeInt32";
 
-  debugFlow(functionName);
+debugFlow(functionName);
 
-  pAxis = this->getAxis(pasynUser);
-  if (!pAxis) {
-    return asynError;
-  }
-  
-  /* Set the parameter and readback in the parameter library.  This may be overwritten when we read back the
-   * status at the end, but that's OK */
-  status = pAxis->setIntegerParam(function, value);
+pAxis = this->getAxis(pasynUser);
+if (!pAxis) {
+return asynError;
+}
+
+/* Set the parameter and readback in the parameter library.  This may be overwritten when we read back the
+* status at the end, but that's OK */
+status = pAxis->setIntegerParam(function, value);
 
 
-  //Call base class method
-  //This will handle callCallbacks even if the function was handled here.
-  status = asynMotorController::writeInt32(pasynUser, value);
+//Call base class method
+//This will handle callCallbacks even if the function was handled here.
+status = asynMotorController::writeInt32(pasynUser, value);
 
-  return status;
+return status;
 
 }
 
 /** Returns a pointer to an pmacAxis object.
-  * Returns NULL if the axis number encoded in pasynUser is invalid.
-  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
+* Returns NULL if the axis number encoded in pasynUser is invalid.
+* \param[in] pasynUser asynUser structure that encodes the axis index number. */
 pmacAxis* pmacController::getAxis(asynUser *pasynUser)
 {
-  int axisNo = 0;
-    
-  getAddress(pasynUser, &axisNo);
-  return getAxis(axisNo);
+int axisNo = 0;
+
+getAddress(pasynUser, &axisNo);
+return getAxis(axisNo);
 }
 
 
 
 /** Returns a pointer to an pmacAxis object.
-  * Returns NULL if the axis number is invalid.
-  * \param[in] axisNo Axis index number. */
+* Returns NULL if the axis number is invalid.
+* \param[in] axisNo Axis index number. */
 pmacAxis* pmacController::getAxis(int axisNo)
 {
-  if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL;
-  return pAxes_[axisNo];
+if ((axisNo < 0) || (axisNo >= numAxes_)) return NULL;
+return pAxes_[axisNo];
 }
 
 
 /** Polls the controller, rather than individual axis.*/
 asynStatus pmacController::poll()
 {
-  static const char *functionName = "pmacController::poll";
+static const char *functionName = "pmacController::poll";
 
-  debugFlow(functionName);
+debugFlow(functionName);
 
-  if (!lowLevelPortUser_) {
-    setIntegerParam(this->motorStatusCommsError_, 1);
-    return asynError;
-  }
+if (!lowLevelPortUser_) {
+setIntegerParam(this->motorStatusCommsError_, 1);
+return asynError;
+}
 
-  asynMotorController::poll();
-  
-  callParamCallbacks();
+asynMotorController::poll();
 
-  return asynSuccess;
+callParamCallbacks();
+
+return asynSuccess;
 }
 
 
@@ -520,334 +522,336 @@ asynStatus pmacController::poll()
 extern "C" {
 
 /**
- * C wrapper for the pmacController constructor.
- * See pmacController::pmacController.
- *
- */
+* C wrapper for the pmacController constructor.
+* See pmacController::pmacController.
+*
+*/
 asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
-				int numAxes, int movingPollPeriod, int idlePollPeriod)
+			int numAxes, int movingPollPeriod, int idlePollPeriod)
 {
 
-    pmacController *ppmacController
-      = new pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
-    ppmacController = NULL;
+pmacController *ppmacController
+= new pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
+ppmacController = NULL;
 
-    return asynSuccess;
+return asynSuccess;
 }
 
 asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
-				int numAxes, int movingPollPeriod, int idlePollPeriod)
+			int numAxes, int movingPollPeriod, int idlePollPeriod)
 {
 
-    SeleneController *ppmacController
-      = new SeleneController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
-    ppmacController = NULL;
+SeleneController *ppmacController
+= new SeleneController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
+ppmacController = NULL;
 
-    return asynSuccess;
+return asynSuccess;
 }
 
 asynStatus pmacV3CreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
-				  int numAxes, int movingPollPeriod, int idlePollPeriod)
+			  int numAxes, int movingPollPeriod, int idlePollPeriod)
 {
 
-    pmacController *ppmacController = new pmacV3Controller(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
-    ppmacController = NULL;
+pmacController *ppmacController = new pmacV3Controller(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
+ppmacController = NULL;
 
-    return asynSuccess;
+return asynSuccess;
 }
 
-  /*---------------------------------------------------------------------------------------------------------------------*/
+/*---------------------------------------------------------------------------------------------------------------------*/
 SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
-				     int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName,
-														   lowLevelPortName,
-														   lowLevelPortAddress,
-														   numAxes,
-														   movingPollPeriod,
-														   idlePollPeriod, 3) {
-    static const char *functionName = "seleneController::seleneController";
-    createParam(MotorSetPositionString,                asynParamFloat64,         &setMotorPosition_);
-    callParamCallbacks();
+			     int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName,
+													   lowLevelPortName,
+													   lowLevelPortAddress,
+													   numAxes,
+													   movingPollPeriod,
+													   idlePollPeriod, 3) {
+static const char *functionName = "seleneController::seleneController";
+createParam(MotorSetPositionString,                asynParamFloat64,         &setMotorPosition_);
+callParamCallbacks();
 }
 
-  pmacV3Controller::pmacV3Controller(const char *portName,
-                                     const char *lowLevelPortName,
-                                     int lowLevelPortAddress, int numAxes,
-                                     double movingPollPeriod,
-                                     double idlePollPeriod,
-                                     const int &extraParams) :
-    pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) {
-      static const char *functionName = "pmacV3Controller::pmacV3Controller";
-      createParam(EnableAxisString, asynParamInt32, &enableAxis_);
-      createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
-      callParamCallbacks();
-  }
+pmacV3Controller::pmacV3Controller(const char *portName,
+			     const char *lowLevelPortName,
+			     int lowLevelPortAddress, int numAxes,
+			     double movingPollPeriod,
+			     double idlePollPeriod,
+			     const int &extraParams) :
+pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) {
+static const char *functionName = "pmacV3Controller::pmacV3Controller";
+createParam(EnableAxisString, asynParamInt32, &enableAxis_);
+createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
+callParamCallbacks();
+}
 
-  /**
-   * C wrapper for the pmacAxis constructor.
-   * See pmacAxis::pmacAxis.
-   *
-   */
-  asynStatus pmacCreateAxis(
-      const char *pmacName, /* specify which controller by port name */
-      int axis)             /* axis number (start from 1). */
-  {
-    pmacController *pC;
-    pmacAxis *pAxis;
-
-    static const char *functionName = "pmacCreateAxis";
-
-    pC = (pmacController *)findAsynPortDriver(pmacName);
-    if (!pC) {
-      printf("%s:%s: Error port %s not found\n", driverName, functionName,
-             pmacName);
-      return asynError;
-    }
-
-    pC->lock();
-    pAxis = new pmacAxis(pC, axis);
-    pAxis = NULL;
-    pC->unlock();
-    return asynSuccess;
-  }
 /**
- * C wrapper for the pmacHRPTAxis constructor.
- * See pmacHRPTAxis::pmacHRPTAxis.
- *
- */
+* C wrapper for the pmacAxis constructor.
+* See pmacAxis::pmacAxis.
+*
+*/
+asynStatus pmacCreateAxis(
+const char *pmacName, /* specify which controller by port name */
+int axis)             /* axis number (start from 1). */
+{
+pmacController *pC;
+pmacAxis *pAxis;
+
+static const char *functionName = "pmacCreateAxis";
+
+pC = (pmacController *)findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n", driverName, functionName,
+     pmacName);
+return asynError;
+}
+
+pC->lock();
+pAxis = new pmacAxis(pC, axis);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
+}
+/**
+* C wrapper for the pmacHRPTAxis constructor.
+* See pmacHRPTAxis::pmacHRPTAxis.
+*
+*/
 asynStatus pmacCreateHRPTAxis(const char *pmacName,         /* specify which controller by port name */
-			  int axis)                    /* axis number (start from 1). */
+		  int axis)                    /* axis number (start from 1). */
 {
-  pmacController *pC;
-  pmacAxis *pAxis;
+pmacController *pC;
+pmacAxis *pAxis;
 
-  static const char *functionName = "pmacCreateHRPTAxis";
+static const char *functionName = "pmacCreateHRPTAxis";
 
-  pC = (pmacController*) findAsynPortDriver(pmacName);
-  if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
-           driverName, functionName, pmacName);
-    return asynError;
-  }
-  
-  pC->lock();
-  pAxis = new pmacHRPTAxis(pC, axis);
-  pAxis = NULL;
-  pC->unlock();
-  return asynSuccess;
+pC = (pmacController*) findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n",
+   driverName, functionName, pmacName);
+return asynError;
+}
+
+pC->lock();
+pAxis = new pmacHRPTAxis(pC, axis);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
 }
 
 /**
- * C wrapper for the SeleneAxis constructor.
- * See SeleneAxis::SeleneAxis.
- *
- */
+* C wrapper for the SeleneAxis constructor.
+* See SeleneAxis::SeleneAxis.
+*
+*/
 asynStatus SeleneCreateAxis(const char *pmacName,         /* specify which controller by port name */
-			    int axis,        /* axis number (start from 1). */
-                            double limitTarget)
+		    int axis,        /* axis number (start from 1). */
+		    double limitTarget)
 {
-  SeleneController *pC;
-  SeleneAxis *pAxis;
+SeleneController *pC;
+SeleneAxis *pAxis;
 
-  static const char *functionName = "SeleneCreateAxis";
+static const char *functionName = "SeleneCreateAxis";
 
-  pC = (SeleneController*) findAsynPortDriver(pmacName);
-  if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
-           driverName, functionName, pmacName);
-    return asynError;
-  }
-  
-  pC->lock();
-  pAxis = new SeleneAxis(pC, axis, limitTarget);
-  pAxis = NULL;
-  pC->unlock();
-  return asynSuccess;
+pC = (SeleneController*) findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n",
+   driverName, functionName, pmacName);
+return asynError;
+}
+
+pC->lock();
+pAxis = new SeleneAxis(pC, axis, limitTarget);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
 }
 
 /**
- * C wrapper for the Selene LiftAxis constructor.
- * See LiftAxis::LiftAxis.
- *
- */
+* C wrapper for the Selene LiftAxis constructor.
+* See LiftAxis::LiftAxis.
+*
+*/
 asynStatus LiftCreateAxis(const char *pmacName,         /* specify which controller by port name */
-			  int axis)        /* axis number (start from 1). */
+		  int axis)        /* axis number (start from 1). */
 {
-  pmacController *pC;
-  LiftAxis *pAxis;
+pmacController *pC;
+LiftAxis *pAxis;
 
-  static const char *functionName = "LiftCreateAxis";
+static const char *functionName = "LiftCreateAxis";
 
-  pC = (pmacController*) findAsynPortDriver(pmacName);
-  if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
-           driverName, functionName, pmacName);
-    return asynError;
-  }
-  
-  pC->lock();
-  pAxis = new LiftAxis(pC, axis);
-  pAxis = NULL;
-  pC->unlock();
-  return asynSuccess;
+pC = (pmacController*) findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n",
+   driverName, functionName, pmacName);
+return asynError;
 }
 
-  /**
-   * C wrapper for the pmacV3Axis constructor.
-   * See pmacAxis::pmacV3Axis.
-   *
-   */
-  asynStatus pmacV3CreateAxis(
-      const char *pmacName, /* specify which controller by port name */
-      int axis)             /* axis number (start from 1). */
-  {
-    pmacController *pC;
-    pmacAxis *pAxis;
-
-    static const char *functionName = "pmacV3CreateAxis";
-
-    pC = (pmacController *)findAsynPortDriver(pmacName);
-    if (!pC) {
-      printf("%s:%s: Error port %s not found\n", driverName, functionName,
-             pmacName);
-      return asynError;
-    }
-
-    pC->lock();
-    pAxis = new pmacV3Axis(pC, axis);
-    pAxis = NULL;
-    pC->unlock();
-    return asynSuccess;
-  }
+pC->lock();
+pAxis = new LiftAxis(pC, axis);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
+}
 
 /**
- * C Wrapper function for pmacHRPTAxis constructor.
- * See pmacAxis::pmacAxis.
- * This function allows creation of multiple pmacAxis objects with axis numbers 1 to numAxes.
- * @param pmacName Asyn port name for the controller (const char *)
- * @param numAxes The number of axes to create, starting at 1.
- *
- */
+* C wrapper for the pmacV3Axis constructor.
+* See pmacAxis::pmacV3Axis.
+*
+*/
+asynStatus pmacV3CreateAxis(
+const char *pmacName, /* specify which controller by port name */
+int axis)             /* axis number (start from 1). */
+{
+pmacController *pC;
+pmacAxis *pAxis;
+
+static const char *functionName = "pmacV3CreateAxis";
+
+pC = (pmacController *)findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n", driverName, functionName,
+     pmacName);
+return asynError;
+}
+
+pC->lock();
+pAxis = new pmacV3Axis(pC, axis);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
+}
+
+/**
+* C Wrapper function for pmacHRPTAxis constructor.
+* See pmacAxis::pmacAxis.
+* This function allows creation of multiple pmacAxis objects with axis numbers 1 to numAxes.
+* @param pmacName Asyn port name for the controller (const char *)
+* @param numAxes The number of axes to create, starting at 1.
+*
+*/
 asynStatus pmacCreateAxes(const char *pmacName,        
-			  int numAxes)                   
+		  int numAxes)                   
 {
-  pmacController *pC;
-  pmacAxis *pAxis;
+pmacController *pC;
+pmacAxis *pAxis;
 
-  static const char *functionName = "pmacCreateAxis";
+static const char *functionName = "pmacCreateAxis";
 
-  pC = (pmacController*) findAsynPortDriver(pmacName);
-  if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
-           driverName, functionName, pmacName);
-    return asynError;
-  }
-  
-  pC->lock();
-  for (int axis=1; axis<=numAxes; axis++) {
-    pAxis = new pmacAxis(pC, axis);
-    pAxis = NULL;
-  }
-  pC->unlock();
-  return asynSuccess;
+pC = (pmacController*) findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n",
+   driverName, functionName, pmacName);
+return asynError;
+}
+
+pC->lock();
+for (int axis=1; axis<=numAxes; axis++) {
+pAxis = new pmacAxis(pC, axis);
+pAxis = NULL;
+}
+pC->unlock();
+return asynSuccess;
 }
 
 /**
- * C wrapper for the pmacAmorDetectorAxis constructor.
- * See pmacAmorDetectorAxis::pmacAmorDetectorAxis.
- *
- */
+* C wrapper for the pmacAmorDetectorAxis constructor.
+* See pmacAmorDetectorAxis::pmacAmorDetectorAxis.
+*
+*/
 asynStatus pmacCreateAmorDetectorAxis(const char *pmacName,         /* specify which controller by port name */
-			                        int axis,                      /* axis number (start from 1). */
-                                    int function_code)             /* AMOR detector axis function */
+					int axis,                      /* axis number (start from 1). */
+			    int function_code)             /* AMOR detector axis function */
 {     
-  pmacController *pC;
-  pmacAxis *pAxis;
+pmacController *pC;
+pmacAxis *pAxis;
 
-  static const char *functionName = "pmacCreateHRPTAxis";
+static const char *functionName = "pmacCreateHRPTAxis";
 
-  pC = (pmacController*) findAsynPortDriver(pmacName);
-  if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
-           driverName, functionName, pmacName);
-    return asynError;
-  }
-  
-  pC->lock();
-  pAxis = new AmorDetectorAxis(pC, axis, function_code);
-  pAxis = NULL;
-  pC->unlock();
-  return asynSuccess;
+pC = (pmacController*) findAsynPortDriver(pmacName);
+if (!pC) {
+printf("%s:%s: Error port %s not found\n",
+   driverName, functionName, pmacName);
+return asynError;
+}
+
+pC->lock();
+pAxis = new AmorDetectorAxis(pC, axis, function_code);
+pAxis = NULL;
+pC->unlock();
+return asynSuccess;
 }
 
 /*================================ SeleneController ===============================================*/
 
 asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
 {
-  int function = pasynUser->reason;
-  asynStatus status = asynError;
-  SeleneAxis *pAxis = NULL;
-  char command[64] = {0};
-  char response[64] = {0};
-  char message[132];
-	
-  static const char *functionName = "SeleneController::writeFloat64";
+int function = pasynUser->reason;
+asynStatus status = asynError;
+SeleneAxis *pAxis = NULL;
+char command[64] = {0};
+char response[64] = {0};
+char message[132];
 
-  sprintf(message,"%s, reason %d",  functionName, function);
+static const char *functionName = "SeleneController::writeFloat64";
 
-  pAxis = (SeleneAxis *)this->getAxis(pasynUser);
-  if (!pAxis) {
-    return asynError;
-  }
+sprintf(message,"%s, reason %d",  functionName, function);
+
+pAxis = (SeleneAxis *)this->getAxis(pasynUser);
+if (!pAxis) {
+return asynError;
+}
 
 
-  /* Set the parameter and readback in the parameter library. */
-  status = pAxis->setDoubleParam(function, value);
+/* Set the parameter and readback in the parameter library. */
+status = pAxis->setDoubleParam(function, value);
 
-  if (function == motorLowLimit_) {
-    sprintf(command, "Q%d54=%f", pAxis->axisNo_, (float)value/(float)MULT);
-    // sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
-	      "%s: Setting low limit on controller %s, axis %d to %f\n",
-	      functionName, portName, pAxis->axisNo_, value);
-    errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
-  } else if (function == motorHighLimit_) {
-  sprintf(command, "Q%d53=%f", pAxis->axisNo_, (float)value/(float)MULT);
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
-	      "%s: Setting high limit on controller %s, axis %d to %f\n",
-	      functionName, portName, pAxis->axisNo_, value);
-    errlogPrintf("Setting high limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
-  } else if(function == setMotorPosition_){
-    snprintf(command,sizeof(command),"Q%d59=%f", pAxis->axisNo_, value);
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
-	      "%s: Defining position of axis%d to %f\n",
-	      functionName, pAxis->axisNo_, value);
-    errlogPrintf("Defining position of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
-  }
+if (function == motorLowLimit_) {
+sprintf(command, "Q%d54=%f", pAxis->axisNo_, (float)value/(float)MULT);
+// sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+      "%s: Setting low limit on controller %s, axis %d to %f\n",
+      functionName, portName, pAxis->axisNo_, value);
+errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
+} else if (function == motorHighLimit_) {
+sprintf(command, "Q%d53=%f", pAxis->axisNo_, (float)value/(float)MULT);
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+      "%s: Setting high limit on controller %s, axis %d to %f\n",
+      functionName, portName, pAxis->axisNo_, value);
+errlogPrintf("Setting high limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
+} else if(function == setMotorPosition_){
+snprintf(command,sizeof(command),"Q%d59=%f", pAxis->axisNo_, value);
+asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+      "%s: Defining position of axis%d to %f\n",
+      functionName, pAxis->axisNo_, value);
+errlogPrintf("Defining position of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
+}
 
-  //Execute the command.
-  if (command[0] != 0 && status == asynSuccess) {
-    status = lowLevelWriteRead(pAxis->axisNo_,command, response);
-  }
+//Execute the command.
+if (command[0] != 0 && status == asynSuccess) {
+status = lowLevelWriteRead(pAxis->axisNo_,command, response);
+}
 
-  // What if now status != asynSuccess
+// What if now status != asynSuccess
 
-  //Call base class method
-  //This will handle callCallbacks even if the function was handled here.
-  status = asynMotorController::writeFloat64(pasynUser, value);
+//Call base class method
+//This will handle callCallbacks even if the function was handled here.
+status = asynMotorController::writeFloat64(pasynUser, value);
 
-  return status;
+return status;
 
 }
 
 
 asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
-  int function = pasynUser->reason;
-  asynStatus status = asynSuccess;
-  pmacV3Axis *pAxis = NULL;
-  char command[64] = {0};
-  char response[64] = {0};
-  int isOn;
-  static const char *functionName = "pmacV3Controller::writeInt32";
+int function = pasynUser->reason;
+asynStatus status = asynSuccess;
+pmacV3Axis *pAxis = NULL;
+char command[64] = {0};
+char response[64] = {0};
+int isOn;
+time_t startTime;
+bool moving;
+static const char *functionName = "pmacV3Controller::writeInt32";
 
   debugFlow(functionName);
 
@@ -864,18 +868,32 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
     // only send commands when a state change is necessary      
     snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
     this->lowLevelWriteRead(pAxis->axisNo_, command, response);
-    isOn = strtol(response, NULL, 10);
-    if(value == 1 && isOn == 0) {
-      sprintf(command, "M%2.2d=15\n", pAxis->axisNo_);
-      lowLevelWriteRead(pAxis->axisNo_, command, response);
+    isOn = strtol(response, NULL, 10) != -3;
+    if(value == 1 && isOn) {
+      sprintf(command, "M%2.2d=15", pAxis->axisNo_);
+      // lowLevelWriteRead(pAxis->axisNo_, command, response);
     }
-    sprintf(command, "M%2.2d14=%d\n", pAxis->axisNo_, value);
+    sprintf(command, "M%2.2d14=%d", pAxis->axisNo_, value);
     pAxis->updateMsgTxtFromDriver("");
     if(isOn != value) {
         asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
               "%s: Enable axis on controller %s, axis %d enable=%d\n",
               functionName, portName, pAxis->axisNo_, value);
         lowLevelWriteRead(pAxis->axisNo_, command, response);
+	startTime = time(NULL);
+	while(time(NULL) < startTime + 2.){
+	    snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
+            this->lowLevelWriteRead(pAxis->axisNo_, command, response);
+            isOn = strtol(response, NULL, 10) != -3;
+	    if(value == isOn){
+	    	pAxis->poll(&moving); // to update enable status
+		return asynSuccess;
+	    }
+	    usleep(100);
+       }
+       errlogPrintf("PMAC: failed to switch axis %d enable status within 2 seconds\n", 
+	            pAxis->axisNo_);
+       return asynError;
     }
   } 
 
@@ -884,7 +902,7 @@ asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
 
 asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
 
-  int function = pasynUser->reason;
+  int function = pasynUser->reason, axStat;
   asynStatus status = asynError;
   pmacV3Axis *pAxis = NULL;
   static const char *functionName = "pmacV3Controller::readInt32";
@@ -901,8 +919,8 @@ asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {
     snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
     status = this->lowLevelWriteRead(pAxis->axisNo_, command, response);
     *value = strtol(response, NULL, 10);
-    
-    int st = setIntegerParam(axisEnabled_, *value);
+    axStat = *value != -3;
+    int st = setIntegerParam(axisEnabled_, axStat);
     callParamCallbacks();
   }
   return pmacController::readInt32(pasynUser, value);
diff --git a/testhuber/db/asynRecord.db b/testhuber/db/asynRecord.db
new file mode 100644
index 0000000..6c50e6d
--- /dev/null
+++ b/testhuber/db/asynRecord.db
@@ -0,0 +1,9 @@
+record(asyn,"$(P)$(R)")
+{
+    field(DTYP,"asynRecordDevice")
+    field(PORT,"$(PORT)")
+    field(ADDR,"$(ADDR)")
+    field(OMAX,"$(OMAX)")
+    field(IMAX,"$(IMAX)")
+}
+
diff --git a/testhuber/db/basic_asyn_motor.db b/testhuber/db/basic_asyn_motor.db
new file mode 100644
index 0000000..fc19187
--- /dev/null
+++ b/testhuber/db/basic_asyn_motor.db
@@ -0,0 +1,23 @@
+record(motor,"$(P)$(M)")
+{
+	field(DESC,"$(DESC)")
+	field(DTYP,"$(DTYP)")
+	field(DIR,"$(DIR)")
+	field(VELO,"$(VELO)")
+	field(VBAS,"$(VBAS)")
+	field(ACCL,"$(ACCL)")
+	field(BDST,"$(BDST)")
+	field(BVEL,"$(BVEL)")
+	field(BACC,"$(BACC)")
+	field(OUT,"@asyn($(PORT),$(ADDR))")
+	field(MRES,"$(MRES)")
+	field(PREC,"$(PREC)")
+	field(EGU,"$(EGU)")
+	field(DHLM,"$(DHLM)")
+	field(DLLM,"$(DLLM)")
+	field(INIT,"$(INIT)")
+	field(TWV,"1")
+        field(RDBD,"$(RDBD)")
+        field(RTRY,"0")
+}
+
diff --git a/testhuber/db/basic_motor.db b/testhuber/db/basic_motor.db
new file mode 100644
index 0000000..98e55ec
--- /dev/null
+++ b/testhuber/db/basic_motor.db
@@ -0,0 +1,21 @@
+grecord(motor,"$(P)$(M)")
+{
+	field(DESC,"$(DESC)")
+	field(DTYP,"$(DTYP)")
+	field(DIR,"$(DIR)")
+	field(VELO,"$(VELO)")
+	field(VBAS,"$(VBAS)")
+	field(ACCL,"$(ACCL)")
+	field(BDST,"$(BDST)")
+	field(BVEL,"$(BVEL)")
+	field(BACC,"$(BACC)")
+	field(OUT,"#C$(C) S$(S) @")
+	field(MRES,"$(MRES)")
+	field(PREC,"$(PREC)")
+	field(EGU,"$(EGU)")
+	field(DHLM,"$(DHLM)")
+	field(DLLM,"$(DLLM)")
+	field(INIT,"$(INIT)")
+	field(TWV,"1")
+}
+
diff --git a/testhuber/db/motorMessage.db b/testhuber/db/motorMessage.db
new file mode 100644
index 0000000..6168c2b
--- /dev/null
+++ b/testhuber/db/motorMessage.db
@@ -0,0 +1,18 @@
+
+# The message text
+record(waveform, "$(P)$(M)-MsgTxt") {
+    field(DTYP, "asynOctetRead")
+    field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
+    field(FTVL, "CHAR")
+    field(NELM, "80")
+    field(SCAN, "I/O Intr")
+}
+
+record(ao,"$(P)m$(N)-Resolution"){
+	field(DESC,"m$(N) Resolution")
+        field(DOL,"$(P)m$(N).MRES CP MS")
+	field(OMSL,"closed_loop")
+	field(DTYP,"asynFloat64")
+	field(OUT,"@asyn($(PORT),$(N))MOTOR_RESOLUTION")
+	field(PREC,"3")
+}
diff --git a/testhuber/db/pmacV3.db b/testhuber/db/pmacV3.db
new file mode 100644
index 0000000..37c5b4f
--- /dev/null
+++ b/testhuber/db/pmacV3.db
@@ -0,0 +1,14 @@
+# enable axis
+record(longout, "$(P)$(M):Enable") {
+    field(DTYP, "asynInt32")
+    field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
+    field(PINI, "YES")
+}
+
+# enable axis
+record(longin, "$(P)$(M):Enable_RBV") {
+    field(DTYP, "asynInt32")
+    field(INP, "@asyn($(PORT),$(N),1) AXIS_ENABLED")
+    field(PINI, "YES")
+    field(SCAN, "1 second")
+}
diff --git a/testhuber/db/sinq_asyn_motor.db b/testhuber/db/sinq_asyn_motor.db
new file mode 100644
index 0000000..4d818ad
--- /dev/null
+++ b/testhuber/db/sinq_asyn_motor.db
@@ -0,0 +1,47 @@
+record(motor,"$(P)$(M)")
+{
+	field(DESC,"$(DESC)")
+	field(DTYP,"$(DTYP)")
+	field(DIR,"$(DIR)")
+	field(VELO,"$(VELO)")
+	field(VBAS,"$(VBAS)")
+	field(ACCL,"$(ACCL)")
+	field(BDST,"$(BDST)")
+	field(BVEL,"$(BVEL)")
+	field(BACC,"$(BACC)")
+	field(OUT,"@asyn($(PORT),$(ADDR))")
+	field(MRES,"$(MRES)")
+	field(PREC,"$(PREC)")
+	field(EGU,"$(EGU)")
+	field(DHLM,"$(DHLM)")
+	field(DLLM,"$(DLLM)")
+	field(INIT,"$(INIT)")
+	field(TWV,"1")
+	field(RDBD,"$(RDBD)")
+}
+
+
+# The message text
+record(waveform, "$(P)$(M)-MsgTxt") {
+    field(DTYP, "asynOctetRead")
+    field(INP, "@asyn($(PORT),$(N),1) MOTOR_MESSAGE_TEXT")
+    field(FTVL, "CHAR")
+    field(NELM, "80")
+    field(SCAN, "I/O Intr")
+}
+
+# enable axis
+record(longout, "$(P)$(M):Enable") {
+    field(DTYP, "asynInt32")
+    field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
+    field(PINI, "YES")
+}
+
+# enable axis
+record(longin, "$(P)$(M):Enable_RBV") {
+    field(DTYP, "asynInt32")
+    field(INP, "@asyn($(PORT),$(N),1) AXIS_ENABLED")
+    field(PINI, "YES")
+    field(SCAN, "5 second")
+}
+
diff --git a/testhuber/huber.cmd b/testhuber/huber.cmd
new file mode 100755
index 0000000..1479a1c
--- /dev/null
+++ b/testhuber/huber.cmd
@@ -0,0 +1,33 @@
+#!/ioc/tools/iocsh
+
+require motorHuber,brambilla_m
+
+# epicsEnvSet("TOP","/ioc/sinq-ioc/boa-ioc")
+
+epicsEnvSet("TOP","/afs/psi.ch/project/sinqdev/sinqepicsapp/testhuber")
+
+epicsEnvSet("STREAM_PROTOCOL_PATH","./db")
+epicsEnvSet("MOTOR",".")
+
+# require autosave, koennecke
+
+# Configure IP Port
+drvAsynIPPortConfigure("Huber1", "129.129.195.128:1234",0,0,0)
+
+# Configure Controller
+asynOctetSetInputEos( "Huber1", -1, "\r")
+asynOctetSetOutputEos("Huber1", -1, "\r")
+
+SMC9300CreateController("SMC1", "Huber1", 5, 50, 2000)
+
+dbLoadTemplate "motor.substitutions.smc9300"
+
+dbLoadRecords("$(TOP)/db/asynRecord.db","P=SQ:BOA:optics:,R=mcu,PORT=Huber1,ADDR=0,OMAX=80,IMAX=80")
+
+
+#--------------------------------- restore autosave
+#set_requestfile_path("$(TOP)", "")
+#set_savefile_path("$(TOP)/autosave")
+#save_restoreSet_Debug(0)
+#set_pass0_restoreFile("$(TOP)/autosave/huber.sav","")
+#set_pass1_restoreFile("$(TOP)/autosave/huber.sav","")
diff --git a/testhuber/motor.substitutions.smc9300 b/testhuber/motor.substitutions.smc9300
new file mode 100644
index 0000000..1a94457
--- /dev/null
+++ b/testhuber/motor.substitutions.smc9300
@@ -0,0 +1,19 @@
+file "$(MOTOR)/db/basic_asyn_motor.db"
+{
+pattern
+{P,             N,   M,      DTYP,         PORT,  ADDR,DESC,           EGU,   DIR,   VELO,  VBAS, ACCL, BDST, BVEL, BACC,    MRES, PREC,   DHLM, DLLM, RDBD, INIT}
+{SQ:BOA:optics:, 1, "trans1","asynMotor", SMC1,   1,   "translation 1",mm,    Pos,    200,   10,  20,    0,    1,   .2,    0.0001,    4,   150,  -150, 0.1,   ""}
+{SQ:BOA:optics:, 2, "lift1", "asynMotor", SMC1,   2,   "lift 1",       mm,    Pos,    200,   10,  80,    0,    1,   .2,    0.0001,    4,   100, 0,    0.1,   ""}
+{SQ:BOA:optics:, 3, "trans2","asynMotor", SMC1,   3,   "translation 2",mm,    Pos,    200,   10,  80,    0,    1,   .2,    0.0001,    4,   150, -150, 0.1,   ""}
+{SQ:BOA:optics:, 4, "lift2", "asynMotor", SMC1,   4,   "lift 2"       ,mm,    Pos,    200,   10,  80,    0,    1,   .2,    0.0001,    4,   100, 0,    0.1, ""}
+}
+
+file "$(TOP)/db/motorMessage.db"
+{
+pattern
+{P,N, M,PORT}
+{SQ:BOA:optics:, 1, "trans1", SMC1} 
+{SQ:BOA:optics:, 2, "lift1", SMC1} 
+{SQ:BOA:optics:, 3, "trans2", SMC1} 
+{SQ:BOA:optics:, 4, "lift2", SMC1} 
+}
diff --git a/testmmac/testmmacs.cmd b/testmmac/testmmacs.cmd
index 08b5c4d..1038bf4 100755
--- a/testmmac/testmmacs.cmd
+++ b/testmmac/testmmacs.cmd
@@ -1,4 +1,4 @@
-#!/usr/local/bin/iocsh 
+#!/usr/local/bin/iocsh  
 
 require sinq,koennecke
 require autosave,koennecke
@@ -21,13 +21,15 @@ set_pass1_restoreFile("$(TOP)/autosave/test.sav","")
 #asynSetTraceMask("macs1", 0, 255)
 #drvAsynIPPortConfigure("macs1", "localhost:8080",0,0,0)
 drvAsynIPPortConfigure("macs1", "Marcel1--MACS:1917")
-MasterMACSCreateController("mota","macs1",7)
+MasterMACSCreateController("mota","macs1",7, 500, 10000)
 MasterMACSCreateAxis("mota",5)
 MasterMACSCreateAxis("mota",6)
 
 dbLoadTemplate "mmacs2.sub"
 
 iocInit()
+setMovingPollPeriod("mota", .5)
+setIdlePollPeriod("mota", 10. )
 
 create_monitor_set("test.req", 10, "")