Added AMOR detector tower special pmac motors: working now

sinqEPICSApp/src/NanotecDriver.cpp

@@ -394,7 +394,12 @@ asynStatus NanotecAxis::poll(bool *moving)
 
   pPtr = strchr(reply,'C');
   pPtr++;
-  posVal = atoi(pPtr);
+  if(pPtr){
+    posVal = atoi(pPtr);
+  } else {
+    errlogPrintf("Invalid response %s for #C received for axis %d\n", reply, axisNo_);
+    return asynError;
+  }
 
   //errlogPrintf("Axis %d, reply %s, position %d\n", axisNo_, reply, posVal);
   setDoubleParam(pC_->motorPosition_, (double)posVal);
@@ -408,7 +413,12 @@ asynStatus NanotecAxis::poll(bool *moving)
 
   pPtr = strchr(reply,'$');
   pPtr++;
-  statVal = atoi(pPtr);
+  if(pPtr) {
+    statVal = atoi(pPtr);
+  } else {
+     errlogPrintf("Invalid response %s for #$ received for axis %d\n", reply, axisNo_);
+     return asynError;
+  }        
   //errlogPrintf("Axis %d, reply %s, statVal = %d\n", 
   // 	       axisNo_, reply, statVal);
 

sinqEPICSApp/src/pmacAxis.cpp

@@ -1,4 +1,4 @@
-/********************************************
+/*******************************************
  *  pmacAxis.cpp
  *
  *  PMAC Asyn motor based on the
@@ -28,6 +28,10 @@
  *
  * Michele Brambilla, February 2022
  *
+ * Added driver for special AMOR detector axes
+ *
+ * Mark Koennecke, June 2023
+ *
  ********************************************/
 
 #include <epicsExit.h>
@@ -42,6 +46,7 @@
 #include <stdlib.h>
 #include <string.h>
 #include <time.h>
+#include <unistd.h>
 
 #include<algorithm>
 #include<cmath>
@@ -1181,3 +1186,309 @@ asynStatus pmacV3Axis::move(double position, int relative, double min_velocity,
   updateMsgTxtFromDriver("");
   return pmacAxis::move(position, relative, min_velocity, max_velocity, acceleration);
 }
+
+/*========================= Amor Detector Motor code ==============*/
+AmorDetectorAxis::AmorDetectorAxis(pmacController *pC, int axisNo, int function)
+    : pmacAxis(pC, axisNo, false) {
+  static const char *functionName = "pmacAxis::pmacAxis";
+
+  pC_->debugFlow(functionName);
+
+  _function = function;
+
+  callParamCallbacks();
+
+  /* Wake up the poller task which will make it do a poll,
+   * updating values for this axis to use the new resolution (stepSize_) */
+  pC_->wakeupPoller();
+}
+/*-----------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::moveVelocity(double min_velocity, 
+                                          double max_velocity,
+                                          double acceleration)
+{
+    updateMsgTxtFromDriver("Function moveVelocity not allowed");
+    return asynError;
+}        
+
+/*------------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::home(double min_velocity, 
+                                          double max_velocity,
+                                          double acceleration,
+                                          int forwards)
+{
+    updateMsgTxtFromDriver("Cannot home AMOR detector motors");
+    return asynError;
+}        
+/*-------------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::setPosition(double position) 
+{
+    updateMsgTxtFromDriver("Cannot setPosition() AMOR detector motors");
+    return asynError;
+}        
+/*-------------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::move(double position, int relative, 
+                                  double min_velocity, double max_velocity,
+                                  double acceleration)
+{
+  asynStatus status = asynError;
+  static const char *functionName = "AmorDetectorAxis::move";
+  double realPosition;
+  int parkStatus;
+  time_t errorWait;
+
+  updateMsgTxtFromDriver("");
+
+  pC_->debugFlow(functionName);
+
+  char command[128] = {0};
+  char response[32] = {0};
+
+  pC_->debugFlow(functionName);
+
+  /* test if we are not in the change position */
+  sprintf(command,"P358");
+  pC_->lowLevelWriteRead(axisNo_, command, response);
+  sscanf(response, "%d", &parkStatus);
+  if(_function != ADPARK && parkStatus == 2) {
+    updateMsgTxtFromDriver("Cannot run motor when in park position");
+    errlogPrintf("Cannot run motor when in park position");
+    return asynError;
+  }        
+
+  if (relative) {
+    realPosition = previous_position_ + position / MULT;
+  } else {
+    realPosition = position / MULT;
+  }
+
+  /* set target */
+  errlogPrintf("function %d, sending position %f\n", _function, realPosition);
+
+ switch(_function){
+  case ADCOM:
+       sprintf(command, "Q451=%f", realPosition);
+       break;
+  case ADCOZ:
+       sprintf(command, "Q454=%f", realPosition);
+       break;
+  case ADPARK:
+       // test if a reset is required first
+       sprintf(command, "P359");
+       pC_->lowLevelWriteRead(axisNo_, command, response);
+       sscanf(response, "%d", &parkStatus);
+       errlogPrintf("park status %d\n", parkStatus);
+       if(parkStatus != 0){
+         sprintf(command,"P352=2");
+         pC_->lowLevelWriteRead(axisNo_, command, response);
+         errlogPrintf("Park status %d, error reset: %s\n", parkStatus, command);
+       }
+       errorWait = time(NULL);
+       while(true){
+          usleep(5000);
+          sprintf(command, "P359");
+          pC_->lowLevelWriteRead(axisNo_, command, response);
+          sscanf(response, "%d", &parkStatus);
+          errlogPrintf("P359 = %d\n", parkStatus);
+          if(parkStatus == 0){
+                  break;
+          }
+          if(time(NULL) > errorWait + 3){
+             errlogPrintf("Failed to clear error status after 3 seconds\n");
+             updateMsgTxtFromDriver("Failed to clear errorState in 3 seconds");
+            return asynError;
+          }        
+       }
+      // now drive to the real place
+       if(realPosition == 0) {
+          sprintf(command, "P350=3");
+       } else if(realPosition < -90){
+           sprintf(command, "P350=2");
+       } else {
+         updateMsgTxtFromDriver("Can only reach -100, 0, nothing else");
+         return asynError;
+       }
+       errlogPrintf("Park position %f, park command: %s\n", realPosition, command);
+ 
+  }
+  status = pC_->lowLevelWriteRead(axisNo_, command, response);
+  if(status != asynSuccess) {
+     asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "AmorDetectorAxis::move failed to send command %s", command);
+    updateMsgTxtFromDriver("Cannot send Amor Detector Command");
+    return status;
+  }
+
+  /* send move command */
+  if(_function != ADPARK) {
+    sprintf(command, "P350=1");
+    status = pC_->lowLevelWriteRead(axisNo_, command, response);
+    if(status != asynSuccess) {
+        asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "AmorDetectorAxis::move failed to send command %s", command);
+      updateMsgTxtFromDriver("Cannot send Amor Detector Command");
+     }
+   }
+
+  det_starting = true;
+  det_startTime = time(NULL);
+
+  next_poll = BUSYPOLL;
+
+  return status;
+}
+/*-----------------------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::stop(double acceleration) {
+  asynStatus status = asynError;
+  static const char *functionName = "AmorDetectorAxis::stopAxis";
+  bool moving = false;
+
+  pC_->debugFlow(functionName);
+
+  char command[128] = {0};
+  char response[32] = {0};
+
+  this->poll(&moving);
+  det_starting = false;
+
+  if(moving) {
+    // only send a stop when actually moving
+    sprintf(command, "P350=8");
+    status = pC_->lowLevelWriteRead(axisNo_, command, response);
+  }
+  return status;
+}
+/*-------------------------------------------------------------------------------*/
+asynStatus AmorDetectorAxis::poll(bool *moving)
+{
+  asynStatus status = asynError;
+  static const char *functionName = "AmorDetectorAxis::poll";
+  int targetReached, errorCode, parkCode;
+  double position;
+
+  pC_->debugFlow(functionName);
+
+  char command[128] = {0};
+  char response[32] = {0};
+  char errorMessage[128] = {0};
+
+  /* read in position flag*/
+  sprintf(command, "P354");
+  status = pC_->lowLevelWriteRead(axisNo_, command, response);
+  if(status != asynSuccess){
+      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "AmorDetectorAxis::poll failed to read in position");
+      updateMsgTxtFromDriver("Failed to read Amor Detector in position");
+      return status;
+  }
+  sscanf(response, "%d", &targetReached);
+          
+  //if(_function == ADCOM){
+  //  errlogPrintf("startTime: %ld, current time: %ld, starting flag %d, targetReached %d\n", det_startTime, time(NULL), 
+  //             det_starting, targetReached);
+  //}
+
+  // The thing is sometimes slow to start, allow 7 seconds
+  // Especially coz is really slow to indicate
+  if(det_starting && time(NULL) > (det_startTime + 7)) {
+     det_starting = false;
+     //if(_function == ADCOM){
+     //   errlogPrintf("Resetting starting flag after timeout\n");
+     //}
+  }
+
+  setIntegerParam(pC_->motorStatusProblem_, false);
+  if(targetReached == 0 && det_starting == false){
+    *moving = false;
+    setIntegerParam(pC_->motorStatusDone_, true);
+    next_poll = IDLEPOLL;
+  } else {
+    *moving = true;
+    if(targetReached == 1){
+        det_starting = false;
+        //if(_function == ADCOM){
+        //    errlogPrintf("Resetting starting flag following targetReached flag\n");
+        //}
+    }
+    setIntegerParam(pC_->motorStatusDone_, false);
+  }
+
+  // read position
+  if(_function == ADCOM)  {
+    strcpy(command,"Q0510");
+  } else if(_function == ADCOZ) {
+    strcpy(command, "Q454");
+  } else if(_function == ADPARK){
+    strcpy(command,"P358");
+  }        
+  status = pC_->lowLevelWriteRead(axisNo_, command, response);
+  if(status != asynSuccess){
+      asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "AmorDetectorAxis::poll failed to read value");
+      updateMsgTxtFromDriver("Failed to read Amor Detector value");
+      return status;
+  }
+  if(_function != ADPARK){
+      sscanf(response, "%lf", &position);
+  } else {
+      // analyse the park code to check where the thing is. This does
+      // not show good values while driving. But that is a limitation
+      // of the hardware. 
+      sscanf(response, "%d", &parkCode);
+      if(parkCode == 1) {
+         position = 0;
+      } else {
+        position = -100;
+      }
+      // errlogPrintf("parkCode %d , position %f,targetREached = %d\n", parkCode, position, targetReached);
+  }
+  setDoubleParam(pC_->motorPosition_, position * 1000.);
+  setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);  
+ 
+  //if(_function == ADCOM){
+  //  errlogPrintf("polling for function %d, position %f, pos*10^3 %f, targetREached = %d\n", _function, position, 
+  //               position*MULT, targetReached);
+  // }
+
+  // read error status
+  sprintf(command, "P359");
+  status = pC_->lowLevelWriteRead(axisNo_, command, response);
+  if(status == asynSuccess) {
+    sscanf(response, "%d", &errorCode);
+    switch(errorCode) {
+    case 0:
+        break;
+    case 1:
+        strcpy(errorMessage,"COZ not released");
+        break;
+    case 2:
+        strcpy(errorMessage,"Motor is in wrong state");
+        break;
+    case 3: 
+        strcpy(errorMessage,"FTZ error while moving");
+        break;
+    case 4:
+        strcpy(errorMessage, "Break  while moving");
+        break;
+    case 5:
+        strcpy(errorMessage, "No freigabe during move");
+        break;
+    default:
+        strcpy(errorMessage, "Unknown error code from: ");
+        strcat(errorMessage, response);
+        break;
+    }
+    if(strlen(errorMessage) > 1) {
+       asynPrint(pC_->pasynUserSelf, ASYN_TRACE_ERROR,
+              "AmorDetectorAxis::poll: %s", errorMessage);
+      updateMsgTxtFromDriver(errorMessage);
+      setIntegerParam(pC_->motorStatusProblem_, true);
+      status = asynError;
+    }
+  }
+
+  callParamCallbacks();
+  return status;
+}
+
+

sinqEPICSApp/src/pmacAxis.h

@@ -157,4 +157,35 @@ public:
   friend class pmacV3Controller;
  };
 
+/*
+ * Special motors for the AMOR detector movement. The whole
+ * command set is different but on a pmac controller. This implements
+ * a coordinated movement of cox, coz and ftz in order not to break
+ * the flight tube which may have been mounted. This is mapped to three
+ * motors selected via the function code: com, the detector omega, coz, 
+ * the detector z offset and park, for parking the flightpath.
+ */
+
+#define ADCOM 1
+#define ADCOZ 2
+#define ADPARK 3
+
+class AmorDetectorAxis: public pmacAxis {
+public:
+  AmorDetectorAxis(pmacController *pController, int axisNo, int function);
+
+  asynStatus move(double position, int relative, double min_velocity,
+                  double max_velocity, double acceleration);
+  asynStatus poll(bool *moving);
+  asynStatus moveVelocity(double min_velocity, double max_velocity, double acceleration);
+  asynStatus home(double min_velocity, double max_velocity, double acceleration, int forwards);
+  asynStatus stop(double acceleration);
+  asynStatus setPosition(double position);
+
+protected:
+  int _function;
+  int det_starting;
+  time_t det_startTime;
+};
+
 #endif /* pmacAxis_H */

sinqEPICSApp/src/pmacController.cpp

@@ -751,7 +751,33 @@ asynStatus pmacCreateAxes(const char *pmacName,
   return asynSuccess;
 }
 
+/**
+ * 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 */
+{     
+  pmacController *pC;
+  pmacAxis *pAxis;
 
+  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;
+}
 
 /*================================ SeleneController ===============================================*/
 
@@ -996,6 +1022,19 @@ static void configpmacAxesCallFunc(const iocshArgBuf *args)
   pmacCreateAxes(args[0].sval, args[1].ival);
 }
 
+/* pmacCraeteAmorDetectorAxis */
+static const iocshArg pmacCreateAmorDetectorAxisArg0 = {"Controller port name", iocshArgString};
+static const iocshArg pmacCreateAmorDetectorAxisArg1 = {"Axis number", iocshArgInt};
+static const iocshArg pmacCreateAmorDetectorAxisArg2 = {"Function code", iocshArgInt};
+static const iocshArg * const pmacCreateAmorDetectorAxisArgs[] = {&pmacCreateAmorDetectorAxisArg0,
+                                                     &pmacCreateAmorDetectorAxisArg1, &pmacCreateAmorDetectorAxisArg2};
+static const iocshFuncDef configpmacAmorDetectorAxis = {"pmacCreateAmorDetectorAxis", 3, pmacCreateAmorDetectorAxisArgs};
+
+static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args)
+{
+  pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival);
+}
+
 
 
 static void pmacControllerRegister(void)
@@ -1005,6 +1044,7 @@ static void pmacControllerRegister(void)
   iocshRegister(&configpmacV3CreateController, configpmacV3CreateControllerCallFunc);
   iocshRegister(&configpmacAxis,               configpmacAxisCallFunc);
   iocshRegister(&configpmacHRPTAxis,           configpmacHRPTAxisCallFunc);
+  iocshRegister(&configpmacAmorDetectorAxis, configpmacAmorDetectorAxisCallFunc);
   iocshRegister(&configSeleneCreateAxis,       configSeleneCreateAxisCallFunc);
   iocshRegister(&configLiftAxis,               configLiftAxisCallFunc);
   iocshRegister(&configpmacV3Axis,             configpmacV3AxisCallFunc);

sinqEPICSApp/src/pmacController.h

@@ -140,6 +140,7 @@ class pmacController : public SINQController {
   friend class SeleneAxis;
   friend class LiftAxis;
   friend class pmacV3Axis;
+  friend class AmorDetectorAxis;
 };
 
 #define NUM_PMAC_PARAMS (&LAST_PMAC_PARAM - &FIRST_PMAC_PARAM + 1)