format with clang-format

sinqEPICSApp/src/NanotecDriver.cpp

@@ -1,15 +1,15 @@
 /*
 FILENAME... NanotecDriver.cpp
-USAGE...    Motor driver support for the Nanotec SMCI controllers 
+USAGE...    Motor driver support for the Nanotec SMCI controllers
             connected to a RS-485 bus.
 
-These motors sit on a bus. The address on the bus is a random number, not 
+These motors sit on a bus. The address on the bus is a random number, not
-necessarily the motor number. These asyn motor drivers do not have access to 
+necessarily the motor number. These asyn motor drivers do not have access to
-the motor record. Thus it was impossible to pass the bus address through. 
+the motor record. Thus it was impossible to pass the bus address through.
-The option was to create an axiliary record but this sucked too because the 
+The option was to create an axiliary record but this sucked too because the
-bus address a motor is an initialisation parameter and should not be visible 
+bus address a motor is an initialisation parameter and should not be visible
-in the EPICS database. Thus I choose to pass the bus address as a comma separated 
+in the EPICS database. Thus I choose to pass the bus address as a comma
-list to the motor controller constructor.  
+separated list to the motor controller constructor.
 
 Mark Koennecke
 July 2015
@@ -19,22 +19,21 @@ Modified to use the MsgTxt field for SINQ
 Mark Koennecke, January 2019
 */
 
-
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <math.h>
 #include <ctype.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 #include <time.h>
 
-#include <iocsh.h>
 #include <epicsThread.h>
 #include <errlog.h>
+#include <iocsh.h>
 
 #include <asynOctetSyncIO.h>
 
-#include "stptok.h"
 #include "NanotecDriver.h"
+#include "stptok.h"
 #include <epicsExport.h>
 
 #define IDLEPOLL 60
@@ -42,464 +41,463 @@ Mark Koennecke, January 2019
 #define ABS(x) (x < 0 ? -(x) : (x))
 
 /** Creates a new NanotecController object.
-  * \param[in] portName          The name of the asyn port that will be created for this driver
+ * \param[in] portName          The name of the asyn port that will be created
-  * \param[in] NanotecPortName     The name of the drvAsynSerialPort that was created previously to connect to the Nanotec controller 
+ * for this driver
-  */
+ * \param[in] NanotecPortName     The name of the drvAsynSerialPort that was
-NanotecController::NanotecController(const char *portName, const char *NanotecPortName, int motCount, const char *bus)
+ * created previously to connect to the Nanotec controller
-  :  SINQController(portName, NanotecPortName, motCount+1)  
+ */
-{
+NanotecController::NanotecController(const char *portName,
-  int axis, busAddress;
+                                     const char *NanotecPortName, int motCount,
-  asynStatus status;
+                                     const char *bus)
-  NanotecAxis *pAxis;
+    : SINQController(portName, NanotecPortName, motCount + 1) {
-  static const char *functionName = "NanotecController::NanotecController";
+    int axis, busAddress;
-  char *pPtr, busNoString[20];
+    asynStatus status;
+    NanotecAxis *pAxis;
+    static const char *functionName = "NanotecController::NanotecController";
+    char *pPtr, busNoString[20];
 
+    /* Connect to Nanotec controller */
+    status = pasynOctetSyncIO->connect(NanotecPortName, 0,
+                                       &pasynUserController_, NULL);
+    if (status) {
+        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
+                  "%s: cannot connect to Nanotec controller\n", functionName);
+    }
+    pasynOctetSyncIO->setOutputEos(pasynUserController_, "\r", strlen("\r"));
+    pasynOctetSyncIO->setInputEos(pasynUserController_, "\r", strlen("\r"));
 
-  /* Connect to Nanotec controller */
+    axis = 1;
-  status = pasynOctetSyncIO->connect(NanotecPortName, 0, &pasynUserController_, NULL);
+    pPtr = (char *)bus;
-  if (status) {
+    while ((pPtr = stptok(pPtr, busNoString, sizeof(busNoString), ",")) !=
-    asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
+           NULL) {
-      "%s: cannot connect to Nanotec controller\n",
+        busAddress = atoi(busNoString);
-      functionName);
+        pAxis = new NanotecAxis(this, axis, busAddress);
-  }
+        errlogPrintf("Axis %d, busAddress = %d\n", axis, busAddress);
-  pasynOctetSyncIO->setOutputEos(pasynUserController_,"\r",strlen("\r"));
+        axis++;
-  pasynOctetSyncIO->setInputEos(pasynUserController_,"\r",strlen("\r"));
+    }
 
-  axis = 1;
+    startPoller(1000. / 1000., IDLEPOLL, 2);
-  pPtr = (char *)bus;
-  while((pPtr = stptok(pPtr,busNoString,sizeof(busNoString),",")) != NULL){
-    busAddress = atoi(busNoString);
-    pAxis = new NanotecAxis(this, axis,busAddress);
-    errlogPrintf("Axis %d, busAddress = %d\n", axis, busAddress);
-    axis++;
-  }
-
-  startPoller(1000./1000., IDLEPOLL, 2);
 }
 
-
 /** Creates a new NanotecController object.
-  * Configuration command, called directly or from iocsh
+ * Configuration command, called directly or from iocsh
-  * \param[in] portName          The name of the asyn port that will be created for this driver
+ * \param[in] portName          The name of the asyn port that will be created
-  * \param[in] NanotecPortName       The name of the drvAsynIPPPort that was created previously to connect to the Nanotec controller 
+ * for this driver
-  */
+ * \param[in] NanotecPortName       The name of the drvAsynIPPPort that was
-extern "C" int NanotecCreateController(const char *portName, const char *NanotecPortName, int numMot, const char *busAddresses)
+ * created previously to connect to the Nanotec controller
-{
+ */
-  NanotecController *pNanotecController
+extern "C" int NanotecCreateController(const char *portName,
-    = new NanotecController(portName, NanotecPortName,numMot,busAddresses);
+                                       const char *NanotecPortName, int numMot,
-  pNanotecController = NULL;
+                                       const char *busAddresses) {
-  return(asynSuccess);
+    NanotecController *pNanotecController =
+        new NanotecController(portName, NanotecPortName, numMot, busAddresses);
+    pNanotecController = NULL;
+    return (asynSuccess);
 }
 
 /** Reports on status of the driver
-  * \param[in] fp The file pointer on which report information will be written
+ * \param[in] fp The file pointer on which report information will be written
-  * \param[in] level The level of report detail desired
+ * \param[in] level The level of report detail desired
-  *
+ *
-  * If details > 0 then information is printed about each axis.
+ * If details > 0 then information is printed about each axis.
-  * After printing controller-specific information it calls asynMotorController::report()
+ * After printing controller-specific information it calls
-  */
+ * asynMotorController::report()
-void NanotecController::report(FILE *fp, int level)
+ */
-{
+void NanotecController::report(FILE *fp, int level) {
-  fprintf(fp, "Nanotec motor driver %s, numAxes=%d\n", 
+    fprintf(fp, "Nanotec motor driver %s, numAxes=%d\n", this->portName,
-    this->portName, numAxes_);
+            numAxes_);
 
-  // Call the base class method
+    // Call the base class method
-  asynMotorController::report(fp, level);
+    asynMotorController::report(fp, level);
 }
 
 /** Returns a pointer to an NanotecAxis object.
-  * Returns NULL if the axis number encoded in pasynUser is invalid.
+ * Returns NULL if the axis number encoded in pasynUser is invalid.
-  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
+ * \param[in] pasynUser asynUser structure that encodes the axis index number.
-NanotecAxis* NanotecController::getAxis(asynUser *pasynUser)
+ */
-{
+NanotecAxis *NanotecController::getAxis(asynUser *pasynUser) {
-  return static_cast<NanotecAxis*>(asynMotorController::getAxis(pasynUser));
+    return static_cast<NanotecAxis *>(asynMotorController::getAxis(pasynUser));
 }
 
 /** Returns a pointer to an NanotecAxis object.
-  * Returns NULL if the axis number encoded in pasynUser is invalid.
+ * Returns NULL if the axis number encoded in pasynUser is invalid.
-  * \param[in] axisNo Axis index number. */
+ * \param[in] axisNo Axis index number. */
-NanotecAxis* NanotecController::getAxis(int axisNo)
+NanotecAxis *NanotecController::getAxis(int axisNo) {
-{
+    return static_cast<NanotecAxis *>(asynMotorController::getAxis(axisNo));
-  return static_cast<NanotecAxis*>(asynMotorController::getAxis(axisNo));
 }
 
-
-
 // These are the NanotecAxis methods
 
 /** Creates a new NanotecAxis object.
-  * \param[in] pC Pointer to the NanotecController to which this axis belongs. 
+ * \param[in] pC Pointer to the NanotecController to which this axis belongs.
-  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
+ * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
-  * 
+ *
-  * Initializes register numbers, etc.
+ * Initializes register numbers, etc.
-  */
+ */
 NanotecAxis::NanotecAxis(NanotecController *pC, int axisNo, int busAddress)
-  : SINQAxis(pC, axisNo),
+    : SINQAxis(pC, axisNo), pC_(pC) {
-    pC_(pC)
+    this->busAddress = busAddress;
-{  
+    next_poll = -1;
-  this->busAddress = busAddress;
-  next_poll = -1;
 }
 
-
-
 /** Reports on status of the axis
-  * \param[in] fp The file pointer on which report information will be written
+ * \param[in] fp The file pointer on which report information will be written
-  * \param[in] level The level of report detail desired
+ * \param[in] level The level of report detail desired
-  *
+ *
-  * After printing device-specific information calls asynMotorAxis::report()
+ * After printing device-specific information calls asynMotorAxis::report()
-  */
+ */
-void NanotecAxis::report(FILE *fp, int level)
+void NanotecAxis::report(FILE *fp, int level) {
-{
+    if (level > 0) {
-  if (level > 0) {
+        fprintf(fp, "  axis %d\n", axisNo_);
-    fprintf(fp, "  axis %d\n",
-            axisNo_);
-  }
-
-  // Call the base class method
-  //asynMotorAxis::report(fp, level);
-}
-
-asynStatus NanotecController::transactController(int axisNo, char command[COMLEN], char reply[COMLEN])
-{
-  asynStatus status;
-  size_t in, out;
-  int reason;
-  SINQAxis *axis = getAxis(axisNo);
-
-  pasynOctetSyncIO->flush(pasynUserController_);
-  
-  status = pasynOctetSyncIO->writeRead(pasynUserController_, command, strlen(command),
-				       reply,COMLEN, 1.,&out,&in,&reason); 
-  if(status != asynSuccess){
-    if(axis != NULL){
-      axis->updateMsgTxtFromDriver("Lost connection to motor controller");
     }
-    return status;
-  }
 
-  /*
+    // Call the base class method
-    check for Nanotec errors
+    // asynMotorAxis::report(fp, level);
-  */
-  if(strstr(reply,"?") != NULL){
-    errlogSevPrintf(errlogMajor, "Bad command %s", command);
-    return asynError;
-  }
-
-  if(strlen(reply) < 1) {
-    errlogSevPrintf(errlogMajor, "No reply received for  %s", command);
-    return asynError;
-  }
-
-  return status;
 }
 
-asynStatus NanotecAxis::move(double position, int relative, double minVelocity, double maxVelocity, double acceleration)
+asynStatus NanotecController::transactController(int axisNo,
-{
+                                                 char command[COMLEN],
-  asynStatus status;
+                                                 char reply[COMLEN]) {
-  //static const char *functionName = "NanotecAxis::move";
+    asynStatus status;
-  char command[COMLEN], reply[COMLEN];
+    size_t in, out;
-  size_t in, out;
+    int reason;
-  int reason;
+    SINQAxis *axis = getAxis(axisNo);
 
-  updateMsgTxtFromDriver("");
+    pasynOctetSyncIO->flush(pasynUserController_);
 
-  // status = sendAccelAndVelocity(acceleration, maxVelocity);
+    status = pasynOctetSyncIO->writeRead(pasynUserController_, command,
-  
+                                         strlen(command), reply, COMLEN, 1.,
-  if (relative) {
+                                         &out, &in, &reason);
-    position += this->position;
+    if (status != asynSuccess) {
-  }
+        if (axis != NULL) {
+            axis->updateMsgTxtFromDriver("Lost connection to motor controller");
+        }
+        return status;
+    }
 
-  homing = 0;
+    /*
-  setIntegerParam(pC_->motorStatusAtHome_, false);
+      check for Nanotec errors
-  setIntegerParam(pC_->motorStatusHighLimit_, false);
+    */
-  setIntegerParam(pC_->motorStatusLowLimit_, false);
+    if (strstr(reply, "?") != NULL) {
+        errlogSevPrintf(errlogMajor, "Bad command %s", command);
+        return asynError;
+    }
+
+    if (strlen(reply) < 1) {
+        errlogSevPrintf(errlogMajor, "No reply received for  %s", command);
+        return asynError;
+    }
 
-  /*
-    set mode
-  */
-  snprintf(command,sizeof(command),"#%dp2",busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
     return status;
-  }
-  
-  /*
-    set target
-  */
-  snprintf(command,sizeof(command),"#%ds%d",busAddress, (int)position);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
-    return status;
-  }
-  
-  /*
-    and start..
-  */
-  snprintf(command,sizeof(command),"#%dA",busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
-    return status;
-  }
-
-  next_poll = -1;
-  return status;
 }
 
-asynStatus NanotecAxis::home(double minVelocity, double maxVelocity, double acceleration, int forwards)
+asynStatus NanotecAxis::move(double position, int relative, double minVelocity,
-{
+                             double maxVelocity, double acceleration) {
-  asynStatus status;
+    asynStatus status;
-  //static const char *functionName = "NanotecAxis::home";
+    // static const char *functionName = "NanotecAxis::move";
-  char command[COMLEN], reply[COMLEN];
+    char command[COMLEN], reply[COMLEN];
+    size_t in, out;
+    int reason;
 
-  setIntegerParam(pC_->motorStatusAtHome_, false);
+    updateMsgTxtFromDriver("");
 
-  updateMsgTxtFromDriver("");
+    // status = sendAccelAndVelocity(acceleration, maxVelocity);
 
-  /*
+    if (relative) {
-    reset positioning errors
+        position += this->position;
-  */
+    }
-  snprintf(command,sizeof(command),"#%dD",busAddress);
+
-  status = pC_->transactController(axisNo_,command,reply);
+    homing = 0;
-  if(status != asynSuccess){
+    setIntegerParam(pC_->motorStatusAtHome_, false);
+    setIntegerParam(pC_->motorStatusHighLimit_, false);
+    setIntegerParam(pC_->motorStatusLowLimit_, false);
+
+    /*
+      set mode
+    */
+    snprintf(command, sizeof(command), "#%dp2", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    /*
+      set target
+    */
+    snprintf(command, sizeof(command), "#%ds%d", busAddress, (int)position);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    /*
+      and start..
+    */
+    snprintf(command, sizeof(command), "#%dA", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    next_poll = -1;
     return status;
-  }
- 
- /*
-    set mode
-  */
-  snprintf(command,sizeof(command),"#%dp4",busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
-    return status;
-  }
-  
-
-  /*
-    set direction
-  */
-  snprintf(command,sizeof(command),"#%dd0",busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
-    return status;
-  }
-  
-
-  /*
-    and start..
-  */
-  snprintf(command,sizeof(command),"#%dA",busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
-  if(status != asynSuccess){
-    return status;
-  }
-
-
-  homing = 1;
-  next_poll= -1;
-  return status;
 }
 
-asynStatus NanotecAxis::moveVelocity(double minVelocity, double maxVelocity, double acceleration)
+asynStatus NanotecAxis::home(double minVelocity, double maxVelocity,
-{
+                             double acceleration, int forwards) {
-  asynStatus status;
+    asynStatus status;
-  //static const char *functionName = "NanotecAxis::moveVelocity";
+    // static const char *functionName = "NanotecAxis::home";
+    char command[COMLEN], reply[COMLEN];
+
+    setIntegerParam(pC_->motorStatusAtHome_, false);
+
+    updateMsgTxtFromDriver("");
+
+    /*
+      reset positioning errors
+    */
+    snprintf(command, sizeof(command), "#%dD", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    /*
+       set mode
+     */
+    snprintf(command, sizeof(command), "#%dp4", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    /*
+      set direction
+    */
+    snprintf(command, sizeof(command), "#%dd0", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    /*
+      and start..
+    */
+    snprintf(command, sizeof(command), "#%dA", busAddress);
+    status = pC_->transactController(axisNo_, command, reply);
+    if (status != asynSuccess) {
+        return status;
+    }
+
+    homing = 1;
+    next_poll = -1;
+    return status;
+}
+
+asynStatus NanotecAxis::moveVelocity(double minVelocity, double maxVelocity,
+                                     double acceleration) {
+    asynStatus status;
+    // static const char *functionName = "NanotecAxis::moveVelocity";
     double target;
 
-  // asynPrint(pasynUser_, ASYN_TRACE_FLOW,
+    // asynPrint(pasynUser_, ASYN_TRACE_FLOW,
-  //   "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
+    //   "%s: minVelocity=%f, maxVelocity=%f, acceleration=%f\n",
-  //   functionName, minVelocity, maxVelocity, acceleration);
+    //   functionName, minVelocity, maxVelocity, acceleration);
-    
-  updateMsgTxtFromDriver("");
 
+    updateMsgTxtFromDriver("");
 
-  if (maxVelocity > 0.) {
+    if (maxVelocity > 0.) {
-    /* This is a positive move */
+        /* This is a positive move */
-    pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&target);
+        pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &target);
-  } else {
+    } else {
-      /* This is a negative move  */
+        /* This is a negative move  */
-    pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&target);
+        pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &target);
-  }
+    }
 
-  status = move(target,0,0,0,0);
+    status = move(target, 0, 0, 0, 0);
 
-  return status;
+    return status;
 }
 
-asynStatus NanotecAxis::stop(double acceleration )
+asynStatus NanotecAxis::stop(double acceleration) {
-{
+    asynStatus status;
-  asynStatus status;
+    // static const char *functionName = "NanotecAxis::stop";
-  //static const char *functionName = "NanotecAxis::stop";
+    char command[COMLEN], reply[COMLEN];
-  char command[COMLEN], reply[COMLEN];
 
-  sprintf(command, "#%dS1", busAddress);
+    sprintf(command, "#%dS1", busAddress);
-  status = pC_->transactController(axisNo_,command,reply);
+    status = pC_->transactController(axisNo_, command, reply);
-  errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
+    errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
 
-  return status;
+    return status;
 }
 
-asynStatus NanotecAxis::setPosition(double position)
+asynStatus NanotecAxis::setPosition(double position) {
-{
+    asynStatus status;
-  asynStatus status;
+    // static const char *functionName = "NanotecAxis::setPosition";
-  //static const char *functionName = "NanotecAxis::setPosition";
+    char command[COMLEN], reply[COMLEN];
-  char command[COMLEN], reply[COMLEN];
 
-  updateMsgTxtFromDriver("");
+    updateMsgTxtFromDriver("");
 
-  sprintf(command, "#%dD%d", busAddress, (int)position);
+    sprintf(command, "#%dD%d", busAddress, (int)position);
-  status = pC_->transactController(axisNo_,command,reply);
+    status = pC_->transactController(axisNo_, command, reply);
-  next_poll = -1;
+    next_poll = -1;
 
-  return status;
+    return status;
 }
 
-asynStatus NanotecAxis::setClosedLoop(bool closedLoop)
+asynStatus NanotecAxis::setClosedLoop(bool closedLoop) {
-{
+    // static const char *functionName = "NanotecAxis::setClosedLoop";
-  //static const char *functionName = "NanotecAxis::setClosedLoop";
- 
-  /*
-    This belongs into the Kingdom of Electronics.
-    We do not do this.
-   */
 
-  return asynError;
+    /*
+      This belongs into the Kingdom of Electronics.
+      We do not do this.
+     */
+
+    return asynError;
 }
 
 /** Polls the axis.
-  * This function reads the motor position, the limit status, the home status, the moving status, 
+ * This function reads the motor position, the limit status, the home status,
-  * and the drive power-on status. 
+ * the moving status, and the drive power-on status. It calls setIntegerParam()
-  * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
+ * and setDoubleParam() for each item that it polls, and then calls
-  * and then calls callParamCallbacks() at the end.
+ * callParamCallbacks() at the end.
-  * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
+ * \param[out] moving A flag that is set indicating that the axis is moving
-asynStatus NanotecAxis::poll(bool *moving)
+ * (true) or done (false). */
-{ 
+asynStatus NanotecAxis::poll(bool *moving) {
-  asynStatus comStatus;
+    asynStatus comStatus;
-  char command[COMLEN], reply[COMLEN];
+    char command[COMLEN], reply[COMLEN];
-  char *pPtr; 
+    char *pPtr;
-  int posVal, statVal, count = 0;
+    int posVal, statVal, count = 0;
-  double lowLim, highLim;
+    double lowLim, highLim;
 
-
+    // protect against excessive polling
-  // protect against excessive polling
+    if (time(NULL) < next_poll) {
-  if(time(NULL) < next_poll){
+        *moving = false;
-    *moving = false;
+        return asynSuccess;
-    return asynSuccess;
-  }
- 
-  // Read the current motor position
-  sprintf(command,"#%dC", busAddress);
-  comStatus = pC_->transactController(axisNo_,command,reply);
-  if(comStatus) goto skip;
-
-  pPtr = strchr(reply,'C');
-  if(pPtr){
-    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;
-  }
-
-  //errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal);
-  setDoubleParam(pC_->motorPosition_, (double)posVal);
-  //setDoubleParam(pC_->motorEncoderPosition_, position);
-  
-
-  // Read the moving status of this motor
-  sprintf(command,"#%d$",busAddress);
-  comStatus = pC_->transactController(axisNo_,command,reply);
-  if(comStatus) goto skip;
-
-  pPtr = strchr(reply,'$');
-  if(pPtr) {
-    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", 
-  // 	       axisNo_, reply, statVal);
-
-  setIntegerParam(pC_->motorStatusDone_, false);
-  *moving = true;
-  pC_->getDoubleParam(axisNo_,pC_->motorLowLimit_,&lowLim);
-  pC_->getDoubleParam(axisNo_,pC_->motorHighLimit_,&highLim);
-
-  if(homing){
-    /*
-      code for homing 
-    */
-    switch(statVal) {
-    case 163:
-      setPosition(lowLim);
-      *moving = false;
-      setIntegerParam(pC_->motorStatusAtHome_, true);
-      setIntegerParam(pC_->motorStatusDone_, true);
-      break;
-    default :
-      if(statVal & 1) {
-	*moving = false;
-	setIntegerParam(pC_->motorStatusAtHome_, true);
-	setIntegerParam(pC_->motorStatusDone_, true);
-      }
-      break;
     }
-  } else {
-    /*
-      code for normal movement
-    */ 
-      if(statVal & 1) {
-	*moving = false;
-	setIntegerParam(pC_->motorStatusDone_, true);
-      } else if (statVal & 4) {
-	setIntegerParam(pC_->motorStatusDone_, true);
-	setIntegerParam(pC_->motorStatusProblem_, true);
-	errlogSevPrintf(errlogMajor, "Limit or other positioning problem at %d", axisNo_);
-	updateMsgTxtFromDriver("Positioning problem");
-	if(ABS(posVal - lowLim) < ABS(posVal - highLim)){
-	  setIntegerParam(pC_->motorStatusLowLimit_, true);
-	  updateMsgTxtFromDriver("Low Limit Hit");
-	} else {
-	  setIntegerParam(pC_->motorStatusHighLimit_, true);
-	  updateMsgTxtFromDriver("High Limit Hit");
-	}
-	*moving = false;
-      } 
-  }
 
-  if(*moving == true){
+    // Read the current motor position
-    next_poll = -1;
+    sprintf(command, "#%dC", busAddress);
-  }
+    comStatus = pC_->transactController(axisNo_, command, reply);
+    if (comStatus)
+        goto skip;
 
-  skip:
+    pPtr = strchr(reply, 'C');
-  setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1:0);
+    if (pPtr) {
-  callParamCallbacks();
+        pPtr++;
-  return comStatus ? asynError : asynSuccess;
+        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;
+    }
+
+    // errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal);
+    setDoubleParam(pC_->motorPosition_, (double)posVal);
+    // setDoubleParam(pC_->motorEncoderPosition_, position);
+
+    // Read the moving status of this motor
+    sprintf(command, "#%d$", busAddress);
+    comStatus = pC_->transactController(axisNo_, command, reply);
+    if (comStatus)
+        goto skip;
+
+    pPtr = strchr(reply, '$');
+    if (pPtr) {
+        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",
+    //  	       axisNo_, reply, statVal);
+
+    setIntegerParam(pC_->motorStatusDone_, false);
+    *moving = true;
+    pC_->getDoubleParam(axisNo_, pC_->motorLowLimit_, &lowLim);
+    pC_->getDoubleParam(axisNo_, pC_->motorHighLimit_, &highLim);
+
+    if (homing) {
+        /*
+          code for homing
+        */
+        switch (statVal) {
+        case 163:
+            setPosition(lowLim);
+            *moving = false;
+            setIntegerParam(pC_->motorStatusAtHome_, true);
+            setIntegerParam(pC_->motorStatusDone_, true);
+            break;
+        default:
+            if (statVal & 1) {
+                *moving = false;
+                setIntegerParam(pC_->motorStatusAtHome_, true);
+                setIntegerParam(pC_->motorStatusDone_, true);
+            }
+            break;
+        }
+    } else {
+        /*
+          code for normal movement
+        */
+        if (statVal & 1) {
+            *moving = false;
+            setIntegerParam(pC_->motorStatusDone_, true);
+        } else if (statVal & 4) {
+            setIntegerParam(pC_->motorStatusDone_, true);
+            setIntegerParam(pC_->motorStatusProblem_, true);
+            errlogSevPrintf(errlogMajor,
+                            "Limit or other positioning problem at %d",
+                            axisNo_);
+            updateMsgTxtFromDriver("Positioning problem");
+            if (ABS(posVal - lowLim) < ABS(posVal - highLim)) {
+                setIntegerParam(pC_->motorStatusLowLimit_, true);
+                updateMsgTxtFromDriver("Low Limit Hit");
+            } else {
+                setIntegerParam(pC_->motorStatusHighLimit_, true);
+                updateMsgTxtFromDriver("High Limit Hit");
+            }
+            *moving = false;
+        }
+    }
+
+    if (*moving == true) {
+        next_poll = -1;
+    }
+
+skip:
+    setIntegerParam(pC_->motorStatusProblem_, comStatus ? 1 : 0);
+    callParamCallbacks();
+    return comStatus ? asynError : asynSuccess;
 }
 
 /** Code for iocsh registration */
-static const iocshArg NanotecCreateControllerArg0 = {"Port name", iocshArgString};
+static const iocshArg NanotecCreateControllerArg0 = {"Port name",
-static const iocshArg NanotecCreateControllerArg1 = {"Nanotec port name", iocshArgString};
+                                                     iocshArgString};
-static const iocshArg NanotecCreateControllerArg2 = {"Number of axes", iocshArgInt};
+static const iocshArg NanotecCreateControllerArg1 = {"Nanotec port name",
-static const iocshArg NanotecCreateControllerArg3 = {"Komma separated list of bus addresses", iocshArgString};
+                                                     iocshArgString};
-static const iocshArg * const NanotecCreateControllerArgs[] = {&NanotecCreateControllerArg0,
+static const iocshArg NanotecCreateControllerArg2 = {"Number of axes",
-							       &NanotecCreateControllerArg1,
+                                                     iocshArgInt};
-							       &NanotecCreateControllerArg2,
+static const iocshArg NanotecCreateControllerArg3 = {
-							       &NanotecCreateControllerArg3
+    "Komma separated list of bus addresses", iocshArgString};
-                                                              };
+static const iocshArg *const NanotecCreateControllerArgs[] = {
-static const iocshFuncDef NanotecCreateControllerDef = {"NanotecCreateController", 4, NanotecCreateControllerArgs};
+    &NanotecCreateControllerArg0, &NanotecCreateControllerArg1,
-static void NanotecCreateContollerCallFunc(const iocshArgBuf *args)
+    &NanotecCreateControllerArg2, &NanotecCreateControllerArg3};
-{
+static const iocshFuncDef NanotecCreateControllerDef = {
-  NanotecCreateController(args[0].sval, args[1].sval, args[2].ival,args[3].sval);
+    "NanotecCreateController", 4, NanotecCreateControllerArgs};
+static void NanotecCreateContollerCallFunc(const iocshArgBuf *args) {
+    NanotecCreateController(args[0].sval, args[1].sval, args[2].ival,
+                            args[3].sval);
 }
 
-static void NanotecRegister(void)
+static void NanotecRegister(void) {
-{
+    iocshRegister(&NanotecCreateControllerDef, NanotecCreateContollerCallFunc);
-  iocshRegister(&NanotecCreateControllerDef, NanotecCreateContollerCallFunc);
 }
 
 extern "C" {

sinqEPICSApp/src/NanotecDriver.h

@@ -10,51 +10,54 @@ Modified to use the MsgTxt field for SINQ
 Mark Koennecke, January 2019
 */
 
-#include "SINQController.h"
 #include "SINQAxis.h"
+#include "SINQController.h"
 
 #define COMLEN 80
 #define MAXMOT 99
 
-class NanotecAxis : public SINQAxis
+class NanotecAxis : public SINQAxis {
-{
+  public:
-public:
+    /* These are the methods we override from the base class */
-  /* These are the methods we override from the base class */
+    NanotecAxis(class NanotecController *pC, int axis, int busAddress);
-  NanotecAxis(class NanotecController *pC, int axis, int busAddress);
+    void report(FILE *fp, int level);
-  void report(FILE *fp, int level);
+    asynStatus move(double position, int relative, double min_velocity,
-  asynStatus move(double position, int relative, double min_velocity, double max_velocity, double acceleration);
+                    double max_velocity, double acceleration);
-  asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
+    asynStatus moveVelocity(double min_velocity, double max_velocity,
-  asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
+                            double acceleration);
-  asynStatus stop(double acceleration);
+    asynStatus home(double min_velocity, double max_velocity,
-  asynStatus poll(bool *moving);
+                    double acceleration, int forwards);
-  asynStatus setPosition(double position);
+    asynStatus stop(double acceleration);
-  asynStatus setClosedLoop(bool closedLoop);
+    asynStatus poll(bool *moving);
+    asynStatus setPosition(double position);
+    asynStatus setClosedLoop(bool closedLoop);
 
-private:
+  private:
-  NanotecController *pC_;          /**< Pointer to the asynMotorController to which this axis belongs.
+    NanotecController
-                                   *   Abbreviated because it is used very frequently */
+        *pC_; /**< Pointer to the asynMotorController to which this axis
-  double position;
+               * belongs. Abbreviated because it is used very frequently */
-  int homing;
+    double position;
-  time_t next_poll;
+    int homing;
-  int busAddress;
+    time_t next_poll;
-  
+    int busAddress;
-friend class NanotecController;
+
+    friend class NanotecController;
 };
 
 class NanotecController : public SINQController {
-public:
+  public:
-  NanotecController(const char *portName, const char *NanotecPortName, int numMot, const char *busAddresses);
+    NanotecController(const char *portName, const char *NanotecPortName,
+                      int numMot, const char *busAddresses);
 
-  void report(FILE *fp, int level);
+    void report(FILE *fp, int level);
-  NanotecAxis* getAxis(asynUser *pasynUser);
+    NanotecAxis *getAxis(asynUser *pasynUser);
-  NanotecAxis* getAxis(int axisNo);
+    NanotecAxis *getAxis(int axisNo);
-
-friend class NanotecAxis;
- private:
- asynUser *pasynUserController_;
-
- asynStatus transactController(int axisNo, char command[COMLEN], char reply[COMLEN]);
 
+    friend class NanotecAxis;
 
+  private:
+    asynUser *pasynUserController_;
 
+    asynStatus transactController(int axisNo, char command[COMLEN],
+                                  char reply[COMLEN]);
 };