Allow enable/disable (tentative)

sinqEPICSApp/Db/motorSet.db

@@ -4,3 +4,10 @@ record(ao, "$(P)$(M)-SetPosition") {
     field(OUT, "@asyn($(PORT),$(N),1) SET_MOTOR_POSITION")
     field(PINI, "YES")
 }
+
+# enable axis
+record(longout, "$(P)$(M):Enable") {
+    field(DTYP, "asynInt32")
+    field(OUT, "@asyn($(PORT),$(N),1) ENABLE_AXIS")
+    field(PINI, "YES")
+}

sinqEPICSApp/src/pmacAxis.cpp

@@ -368,34 +368,6 @@ static char *translateAxisError(int axErr)
   }
 }
 
-asynStatus pmacV3Axis::getAxisStatus(bool *moving)
-{
-  char command[pC_->PMAC_MAXBUF_];
-  char response[pC_->PMAC_MAXBUF_];
-  int cmdStatus = 0;
-  int done = 0, posChanging = 0;
-  double position = 0;
-  int nvals = 0;
-  int axisProblemFlag = 0;
-  epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
-  char message[132], *axMessage;
-
-  /* read our status items one by one: our PMAC does not seem to give multiple
-   * responses ..*/
-  sprintf(command, "P%2.2d01", axisNo_);
-  cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
-  nvals = sscanf(response, "%d", &axErr);
-  if (cmdStatus || nvals != 1) {
-    asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
-              "drvPmacAxisGetStatus: Failed to read axis Error  Status: "
-              "%d\nCommand :%s\nResponse:%s\n",
-              cmdStatus, command, response);
-    updateMsgTxtFromDriver("Cannot read Axis Error Status");
-  }
-}
-
-
-
 asynStatus pmacAxis::getAxisStatus(bool *moving)
 {
     char command[pC_->PMAC_MAXBUF_];
@@ -625,6 +597,16 @@ asynStatus pmacHRPTAxis::getAxisStatus(bool *moving)
   return result;
 }
 
+asynStatus pmacAxis::enable(int on) {
+  static const char *functionName = "pmacAxis::enable";
+
+  pC_->debugFlow(functionName);
+
+  // Cannot do this.
+
+  return asynSuccess;
+}
+
 /*================================= SeleneAxis code ======================================================*/
 SeleneAxis::SeleneAxis(SeleneController *pC, int axisNo, double limitTarget)
   :   pmacAxis(pC, axisNo, false)
@@ -890,13 +872,13 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
   double position = 0;
   int nvals = 0;
   int axisProblemFlag = 0;
-  epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0;
+  epicsUInt32 axErr = 0, axStat = 0, highLim = 0, lowLim = 0, axDone = 0;
   char message[132], *axMessage;
 
-  sprintf(command, "Q%2.2d10 P%2.2d00", axisNo_, axisNo_);
+  sprintf(command, "Q%2.2d10 P%2.2d00 P%2.2d23", axisNo_, axisNo_, axisNo_);
   cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
-  nvals = sscanf(response, "%lf %d", &position, &axStat);
+  nvals = sscanf(response, "%lf %d %d", &position, &axStat, &axDone);
-  if (cmdStatus || nvals != 2) {
+  if (cmdStatus || nvals != 3) {
     asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
               "drvPmacAxisGetStatus: Failed to read position and status,  "
               "Status: %d\nCommand :%s\nResponse:%s\n",
@@ -924,7 +906,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
   // errlogPrintf("Polling, axStat = %d, position = %f\n", axStat, position);
 
   /* are we done? */
-  if ((axStat == 0 || axStat == 14 || axStat < 0) && starting == 0) {
+  if ( axDone == 0 && starting == 0) {
     done = 1;
   } else {
     starting = 0;
@@ -992,7 +974,7 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
   }
 
   sprintf(message, "poll results: axis %d, status %d, done = %d",
-          axisNo_, axStat, done);
+          axisNo_, axStat, axDone);
   pC_->debugFlow(message);
 
   if (*moving == false) {
@@ -1065,3 +1047,31 @@ asynStatus pmacV3Axis::getAxisStatus(bool *moving) {
 
   return asynSuccess;
 }
+
+// pmacV3Axis::enable(int on) {
+//   char command[pC_->PMAC_MAXBUF_];
+//   char response[pC_->PMAC_MAXBUF_];
+//   int cmdStatus = 0;
+//   static const char *functionName = "pmacV3Axis::enable";
+
+//   pC_->debugFlow(functionName);
+
+//   // Enable the axis. After startup, the axis are disabled on the controller...
+//   sprintf(command, "M%2.2d14=%1.1d", axisNo_, on);
+//   cmdStatus = pC_->lowLevelWriteRead(axisNo_, command, response);
+//   if (cmdStatus) {
+//     if(on) {
+//     asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+//               "%s: Error: enabling axis %d failed.\n", functionName, axisNo_);
+//     } else {
+//           asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+//               "%s: Error: disabling axis %d failed.\n", functionName, axisNo_);
+//     }
+//     return asynError;
+//   }
+
+//   callParamCallbacks();
+
+//   return asynSuccess;
+// }
+

sinqEPICSApp/src/pmacAxis.h

@@ -24,6 +24,8 @@
 class pmacController;
 class SeleneController;
 
+#define PMAC_EnableAxis "PMAC_ENABLE_AXIS"
+
 class pmacAxis : public SINQAxis
 {
   public:
@@ -36,6 +38,7 @@ class pmacAxis : public SINQAxis
   asynStatus stop(double acceleration);
   asynStatus poll(bool *moving);
   asynStatus setPosition(double position);
+  asynStatus enable(int on);
 
 protected:
   pmacController *pC_;
@@ -139,6 +142,7 @@ class LiftAxis : public pmacAxis
 
 class pmacV3Axis : public pmacAxis {
 public:
+
   using pmacAxis::pmacAxis;
   friend class pmacController;
  };

sinqEPICSApp/src/pmacController.cpp

@@ -133,7 +133,10 @@ extern "C" {
 					 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);
   
@@ -546,6 +549,19 @@ asynStatus SeleneCreateController(const char *portName, const char *lowLevelPort
     return asynSuccess;
 }
 
+asynStatus pmacV3CreateController(const char *portName,
+                                const char *lowLevelPortName,
+                                int lowLevelPortAddress, int numAxes,
+                                int movingPollPeriod, int idlePollPeriod) {
+
+  pmacV3Controller *ppmacController = new pmacV3Controller(
+      portName, lowLevelPortName, lowLevelPortAddress, numAxes,
+      movingPollPeriod / 1000., idlePollPeriod / 1000.);
+  ppmacController = NULL;
+
+  return asynSuccess;
+}
+
   /*---------------------------------------------------------------------------------------------------------------------*/
 SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
 				     int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName,
@@ -559,13 +575,26 @@ SeleneController::SeleneController(const char *portName, const char *lowLevelPor
     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_);
+      callParamCallbacks();
+  }
 
   /**
    * C wrapper for the pmacAxis constructor.
    * See pmacAxis::pmacAxis.
    *
    */
-asynStatus pmacCreateAxis(const char *pmacName,         /* specify which controller by port name */
+  asynStatus pmacCreateAxis(
+      const char *pmacName, /* specify which controller by port name */
       int axis)             /* axis number (start from 1). */
   {
     pmacController *pC;
@@ -575,8 +604,8 @@ asynStatus pmacCreateAxis(const char *pmacName,         /* specify which control
 
     pC = (pmacController *)findAsynPortDriver(pmacName);
     if (!pC) {
-    printf("%s:%s: Error port %s not found\n",
+      printf("%s:%s: Error port %s not found\n", driverName, functionName,
-           driverName, functionName, pmacName);
+             pmacName);
       return asynError;
     }
 
@@ -752,6 +781,8 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu
     status = lowLevelWriteRead(pAxis->axisNo_,command, response);
   }
 
+  // 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);
@@ -760,6 +791,42 @@ asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 valu
 
 }
 
+asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
+  int function = pasynUser->reason;
+  asynStatus status = asynError;
+  pmacAxis *pAxis = NULL;
+  static const char *functionName = "pmacV3Controller::writeInt32";
+
+  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);
+
+  if (function == enableAxis_) {
+    sprintf(command, "M%2.2d14=%d\n", pAxis->axisNo_, value);
+    asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
+              "%s: Enable axis on controller %s, axis %d enable=%d\n",
+              functionName, portName, pAxis->axisNo_, value);
+    errlogPrintf("Enable axis %d: %d, command = %s\n", pAxis->axisNo_, value, command);
+  } 
+
+  // 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::writeInt32(pasynUser, value);
+
+  return status;
+}
 
 /* Code for iocsh registration */
 
@@ -790,7 +857,13 @@ static void configSeleneCreateControllerCallFunc(const iocshArgBuf *args)
 {
   SeleneCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
 }
-
+static const iocshFuncDef configpmacV3CreateController = {
+    "pmacV3CreateController", 6, pmacCreateControllerArgs
+};
+static void configpmacV3CreateControllerCallFunc(const iocshArgBuf *args) {
+  pmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival,
+                         args[4].ival, args[5].ival);
+}
 
 /* pmacCreateAxis */
 static const iocshArg pmacCreateAxisArg0 = {"Controller port name", iocshArgString};

sinqEPICSApp/src/pmacController.h

@@ -164,4 +164,22 @@ class SeleneController : public pmacController {
     
 };
 
+#define EnableAxisString "ENABLE_AXIS"
+
+class pmacV3Controller : public pmacController {
+public:
+  pmacV3Controller(const char *portName, const char *lowLevelPortName,
+                 int lowLevelPortAddress, int numAxes, double movingPollPeriod,
+                 double idlePollPeriod, const int &extraParams = 2);
+
+  // overloaded because we want to enable/disable the motor
+  asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
+
+  friend class pmacV3Axis;
+  friend class pmacAxis;
+
+protected:
+  int enableAxis_;
+};
+
 #endif /* pmacController_H */