Merge pull request epics-modules#44 from icshwi/anders_EnabelMotionFuncs

Enable / disable motion funcs

RELEASE.md

@@ -1,6 +1,13 @@
 Release Notes
 ===
 # ECMC master
+* Add command to enable/disable motion functions (all are by default enabled):
+```
+ecmcConfigOrDie "Cfg.SetAxisEnableMotionFunctions(<axisid>,
+                                                  <enable pos>,
+                                                  <enable const velo>,
+                                                  <enable home>)"
+```
 * ecmc-updated param changed to uint64 if asyn version >=4.37
 * Fix printout of plugin plc functions info for funcs with more than 6 args.
 

devEcmcSup/com/ecmcCmdParser.c

@@ -1634,6 +1634,18 @@ static int handleCfgCommand(const char *myarg_1) {
     return setAxisEnableSoftLimitFwd(iValue, iValue2);
   }
 
+  /*int Cfg.SetAxisEnableMotionFunctions(int axis_no, 
+                                         int enablePos,
+                                         int enableConstVelo,
+                                         int enableHome);*/
+  nvals =
+    sscanf(myarg_1, "SetAxisEnableMotionFunctions(%d,%d,%d,%d)",
+           &iValue, &iValue2, &iValue3, &iValue4);
+
+  if (nvals == 4) {
+    return setAxisEnableMotionFunctions(iValue, iValue2, iValue3, iValue4);
+  }
+
   /*int Cfg.SetAxisMonAtTargetTol(int axis_no, double value);*/
   nvals = sscanf(myarg_1, "SetAxisMonAtTargetTol(%d,%lf)", &iValue, &dValue);
 

devEcmcSup/main/ecmcError.cpp

@@ -719,6 +719,10 @@ const char * ecmcError::convertErrorIdToString(int errorId) {
   case 0x14D15:
     return "ERROR_SEQ_TARGET_POS_OUT_OF_RANGE";
 
+    break;
+  case 0x14D16:
+    return "ERROR_SEQ_MOTION_CMD_NOT_ENABLED";
+
     break;
 
   case 0x14E00:    // TRAJECTORY

devEcmcSup/motion/ecmcAxisBase.cpp

@@ -1629,3 +1629,35 @@ int ecmcAxisBase::setDisableAxisAtErrorReset(bool disable){
   disableAxisAtErrorReset_ = disable;
   return 0;
 }
+
+int ecmcAxisBase::setAllowMotionFunctions(bool enablePos, 
+                                          bool enableConstVel,
+                                          bool enableHome) {
+
+
+  if (getSeq() != NULL) {      
+    return getSeq()->setAllowMotionFunctions(enablePos,enableConstVel,enableHome);
+  }
+  return ERROR_AXIS_SEQ_OBJECT_NULL;
+}
+
+int ecmcAxisBase::getAllowPos() {
+  if (getSeq() != NULL) {      
+    return getSeq()->getAllowPos();
+  }
+  return -ERROR_AXIS_SEQ_OBJECT_NULL;
+}
+
+int ecmcAxisBase::getAllowConstVelo(){
+    if (getSeq() != NULL) {      
+    return getSeq()->getAllowConstVelo();
+  }
+  return -ERROR_AXIS_SEQ_OBJECT_NULL;
+}
+
+int ecmcAxisBase::getAllowHome(){
+  if (getSeq() != NULL) {      
+    return getSeq()->getAllowHome();
+  }
+  return -ERROR_AXIS_SEQ_OBJECT_NULL;
+}

devEcmcSup/motion/ecmcAxisBase.h

@@ -253,6 +253,10 @@ class ecmcAxisBase : public ecmcError {
   int                   setPosition(double homePositionSet);  // Autosave
   int                   stopMotion(int killAmplifier);
   asynStatus            axisAsynWriteCmd(void* data, size_t bytes, asynParamType asynParType);
+  int                   setAllowMotionFunctions(bool enablePos, bool enableConstVel, bool enableHome);
+  int                   getAllowPos();
+  int                   getAllowConstVelo();
+  int                   getAllowHome();
 
  protected:
   void         initVars();

devEcmcSup/motion/ecmcAxisSequencer.cpp

@@ -51,6 +51,9 @@ void ecmcAxisSequencer::initVars() {
   homeLatchCountOffset_ = 0;
   homeLatchCountAct_    = 0;
   overUnderFlowLatch_   = ECMC_ENC_NORMAL;
+  enablePos_            = true;
+  enableConstVel_       = true;
+  enableHome_           = true;
 }
 
 // Cyclic execution
@@ -270,6 +273,10 @@ int ecmcAxisSequencer::setExecute(bool execute) {
   case ECMC_CMD_MOVEVEL:
 
     if (data_->command_.execute  && !executeOld_) {
+      if(!enableConstVel_) {
+        return setErrorID(__FILE__, __FUNCTION__, __LINE__, ERROR_SEQ_MOTION_CMD_NOT_ENABLED);
+      }
+
       data_->status_.busy = true;
       traj_->setMotionMode(ECMC_MOVE_MODE_VEL);
       traj_->setTargetVel(data_->command_.velocityTarget);                  
@@ -285,6 +292,10 @@ int ecmcAxisSequencer::setExecute(bool execute) {
   case ECMC_CMD_MOVEREL:
 
     if (data_->command_.execute && !executeOld_) {
+      if(!enablePos_) {
+        return setErrorID(__FILE__, __FUNCTION__, __LINE__, ERROR_SEQ_MOTION_CMD_NOT_ENABLED);
+      }
+
       data_->status_.busy = true;
       traj_->setMotionMode(ECMC_MOVE_MODE_POS);
       traj_->setTargetVel(data_->command_.velocityTarget);
@@ -300,6 +311,10 @@ int ecmcAxisSequencer::setExecute(bool execute) {
   case ECMC_CMD_MOVEABS:
 
     if (data_->command_.execute && !executeOld_) {
+      if(!enablePos_) {
+        return setErrorID(__FILE__, __FUNCTION__, __LINE__, ERROR_SEQ_MOTION_CMD_NOT_ENABLED);
+      }
+
       if(data_->command_.moduloRange>0) {
         if(data_->command_.positionTarget<0 || data_->command_.positionTarget>= data_->command_.moduloRange) {
           LOGERR(
@@ -354,7 +369,11 @@ int ecmcAxisSequencer::setExecute(bool execute) {
   case ECMC_CMD_HOMING:
 
     if (data_->command_.execute && !executeOld_) {
+
       stopSeq();
+      if(!enableHome_) {
+        return setErrorID(__FILE__, __FUNCTION__, __LINE__, ERROR_SEQ_MOTION_CMD_NOT_ENABLED);
+      }
 
       if ((traj_ != NULL) && (enc_ != NULL) && (mon_ != NULL) &&
           (cntrl_ != NULL)) {
@@ -2182,4 +2201,25 @@ void ecmcAxisSequencer::finalizeHomingSeq(double newPosition) {
 
 void ecmcAxisSequencer::setHomeLatchCountOffset(int count) {
   homeLatchCountOffset_ = count;
-}
+}
+
+int ecmcAxisSequencer::setAllowMotionFunctions(bool enablePos,
+                                               bool enableConstVel,
+                                               bool enableHome) {
+  enablePos_ = enablePos;
+  enableConstVel_ = enableConstVel;
+  enableHome_ = enableHome;
+  return 0;
+}
+
+int ecmcAxisSequencer::getAllowPos() {
+  return enablePos_;
+}
+
+int ecmcAxisSequencer::getAllowConstVelo(){
+  return enableConstVel_;
+}
+
+int ecmcAxisSequencer::getAllowHome(){
+  return enableHome_;
+}

devEcmcSup/motion/ecmcAxisSequencer.h

@@ -44,6 +44,7 @@
 #define ERROR_SEQ_ABS_OVER_UNDER_FLOW_ERROR 0x14D13
 #define ERROR_SEQ_LATCH_COUNT_OUT_OF_RANGE 0x14D14
 #define ERROR_SEQ_TARGET_POS_OUT_OF_RANGE 0x14D15
+#define ERROR_SEQ_MOTION_CMD_NOT_ENABLED 0x14D16
 
 // Homing
 enum ecmcHomingType {
@@ -106,8 +107,13 @@ class ecmcAxisSequencer : public ecmcError {
   int    validate();
   int    setSequenceTimeout(int timeout);
   int    setExternalExecute(bool execute);
-  //int    setExtTrajIF(ecmcMasterSlaveIF *extIf);
   int    setAxisDataRef(ecmcAxisData *data);
+  int    setAllowMotionFunctions(bool enablePos,
+                                 bool enableConstVel,
+                                 bool enableHome);
+  int    getAllowPos();
+  int    getAllowConstVelo();
+  int    getAllowHome();
 
  private:
   void   initVars();
@@ -165,6 +171,9 @@ class ecmcAxisSequencer : public ecmcError {
   ecmcOverUnderFlowType overUnderFlowLatch_;
   int homeLatchCountOffset_;
   int homeLatchCountAct_;
+  bool enablePos_;
+  bool enableConstVel_;
+  bool enableHome_;
 };
 
 #endif  /* ecmcAxisSequencer_H_ */

devEcmcSup/motion/ecmcMotion.cpp

@@ -406,6 +406,18 @@ int getAxisEnable(int axisIndex, int *value) {
   return 0;
 }
 
+int setAxisEnableMotionFunctions(int axisIndex,
+                                 int enablePos,
+                                 int enableConstVel,
+                                 int enableHome) {
+
+  CHECK_AXIS_RETURN_IF_ERROR_AND_BLOCK_COM(axisIndex)
+
+  return axes[axisIndex]->setAllowMotionFunctions(enablePos,
+                                                  enableConstVel,
+                                                  enableHome);
+}
+
 int setAxisEnableAlarmAtHardLimits(int axisIndex, int enable) {
   LOGINFO4("%s/%s:%d axisIndex=%d value=%d\n",
            __FILE__,

devEcmcSup/motion/ecmcMotion.h

@@ -1189,6 +1189,23 @@ int setAxisCmdData(int axisIndex,
 int setAxisEnable(int axisIndex,
                   int value);
 
+/** \brief Set enable of motion functions.\n
+ *
+ * \param[in] axisIndex       Axis index.\n
+ * \param[in] enablePos       Allow positioning (default = true).\n
+ * \param[in] enableConstVel  Allow constant velocity (default = true).\n
+ * \param[in] enableHome      Allow homing (default = true).\n
+ *
+ * \return 0 if success or otherwise an error code.\n
+ *
+ * \note Example: Only allow positioning for axis 3.\n
+ * "Cfg.setAxisEnableMotionFunctions(3,1,0,0)" //Command string to ecmcCmdParser.c.\n
+ */
+int setAxisEnableMotionFunctions(int axisIndex,
+                                 int enablePos,
+                                 int enableConstVel,
+                                 int enableHome);
+
 /** \brief Set enable alarms at limits bit.\n
  *
  * \param[in] axisIndex  Axis index.\n

devEcmcSup/motor/ecmcMotorRecordAxis.cpp

@@ -518,21 +518,41 @@ asynStatus ecmcMotorRecordAxis::move(double position, int relative, double minVe
   int errorCode = 0;
 
   if(ecmcRTMutex) epicsMutexLock(ecmcRTMutex);
-  if(relative) {
-    errorCode = drvlocal.ecmcAxis->moveRelativePosition(position,
-                                                        maxVelocity,
-                                                        acceleration,
-                                                        acceleration);
-  }
-  else
-  {
-    errorCode = drvlocal.ecmcAxis->moveAbsolutePosition(position,
-                                                        maxVelocity,
-                                                        acceleration,
-                                                        acceleration);
-
-  }
+
+    if(drvlocal.ecmcAxis->getBlockExtCom()) {
+      LOGERR(
+        "%s/%s:%d: ERROR: Communication to ECMC blocked, motion commands not allowed..\n",
+        __FILE__,
+        __FUNCTION__,
+        __LINE__);
+      return asynError;
+    }
+
+    //if(drvlocal.ecmcAxis->getAllowPos()) {
+
+    if(relative) {
+      errorCode = drvlocal.ecmcAxis->moveRelativePosition(position,
+                                                          maxVelocity,
+                                                          acceleration,
+                                                          acceleration);
+    }
+    else
+    {
+      errorCode = drvlocal.ecmcAxis->moveAbsolutePosition(position,
+                                                          maxVelocity,
+                                                          acceleration,
+                                                          acceleration);        
+    }    
+    //} else {
+    //  LOGERR(
+    //    "%s/%s:%d: ERROR: Constant velo disabled and therefore not allowed.\n",
+    //    __FILE__,
+    //    __FUNCTION__,
+    //    __LINE__);
+    //}
+
   if(ecmcRTMutex) epicsMutexUnlock(ecmcRTMutex);
+
 #ifndef motorWaitPollsBeforeReadyString
   drvlocal.waitNumPollsBeforeReady += WAITNUMPOLLSBEFOREREADY;
 #endif
@@ -601,8 +621,29 @@ asynStatus ecmcMotorRecordAxis::home(double minVelocity, double maxVelocity, dou
     return asynError;
   }
 
+  int errorCode = 0;
   if(ecmcRTMutex) epicsMutexLock(ecmcRTMutex);
-  int errorCode =  drvlocal.ecmcAxis->moveHome(cmdData,homPos,velToCam,velOffCam,accHom,accHom);
+
+    if(drvlocal.ecmcAxis->getBlockExtCom()) {
+      LOGERR(
+        "%s/%s:%d: ERROR: Communication to ECMC blocked, motion commands not allowed..\n",
+        __FILE__,
+        __FUNCTION__,
+        __LINE__);
+      return asynError;
+    }
+
+    //if(drvlocal.ecmcAxis->getAllowHome()) {
+    errorCode =  drvlocal.ecmcAxis->moveHome(cmdData,homPos,velToCam,velOffCam,accHom,accHom);
+    //} else
+    //{
+    //  LOGERR(
+    //    "%s/%s:%d: ERROR: Homing disabled and therefore not allowed.\n",
+    //    __FILE__,
+    //    __FUNCTION__,
+    //    __LINE__);    
+    //}
+
   if(ecmcRTMutex) epicsMutexUnlock(ecmcRTMutex);
 
 #ifndef motorWaitPollsBeforeReadyString
@@ -655,10 +696,31 @@ asynStatus ecmcMotorRecordAxis::moveVelocity(double minVelocity, double maxVeloc
     acc = -acc;
   }
 
+  int errorCode = 0;
   if(ecmcRTMutex) epicsMutexLock(ecmcRTMutex);
-  int errorCode = drvlocal.ecmcAxis->moveVelocity(velo,
-                                                  acc,
-                                                  acc);
+
+    if(drvlocal.ecmcAxis->getBlockExtCom()) {
+      LOGERR(
+        "%s/%s:%d: ERROR: Communication to ECMC blocked, motion commands not allowed..\n",
+        __FILE__,
+        __FUNCTION__,
+        __LINE__);
+      return asynError;
+    }
+
+    //if(drvlocal.ecmcAxis->getAllowConstVelo()) {
+    errorCode = drvlocal.ecmcAxis->moveVelocity(velo,
+                                                acc,
+                                                acc);
+    //} else
+    //{
+    //  LOGERR(
+    //    "%s/%s:%d: ERROR: Constant velo disabled and therefore not allowed.\n",
+    //    __FILE__,
+    //    __FUNCTION__,
+    //    __LINE__);
+    //}
+
   if(ecmcRTMutex) epicsMutexUnlock(ecmcRTMutex);
 
 #ifndef motorWaitPollsBeforeReadyString

devEcmcSup/plc/ecmcPLCTask.cpp

@@ -771,6 +771,7 @@ int ecmcPLCTask::loadMcLib() {
   ecmcPLCTaskAddFunction("mc_get_busy",     mc_get_busy);
   ecmcPLCTaskAddFunction("mc_get_homed",    mc_get_homed);
   ecmcPLCTaskAddFunction("mc_get_axis_err", mc_get_axis_err);
+  ecmcPLCTaskAddFunction("mc_set_enable_motion_funcs", mc_set_enable_motion_funcs);
 
   if (mc_cmd_count != cmdCounter) {
     LOGERR("%s/%s:%d: PLC Lib MC command count missmatch (0x%x).\n",