Merge pull request epics-modules#46 from icshwi/anders_gear_in

mc_move_ext_pos()

RELEASE.md

@@ -1,7 +1,16 @@
 Release Notes
 ===
 # ECMC master
-* Position setpoint is not overwritten with actual position when axis is disabled. Setpoint is updated with actual position during enabling sequence instead of always in disable.
+* Add plc function mc_move_ext_pos():
+  Plc function will move axis to the current external plc setpoint. Inteded to be used to move a slave axis to it's start position before allowing external setpoints.
+
+* Add variables:
+
+  - ax<id>.enc.extactpos  (ro): Current external plc calculated actual position (ax<id>.enc.actpos can be used for writing).
+
+  - ax<id>.traj.extsetpos (ro):  Current external plc calculated setpoint position(ax<id>.enc.setpos can be used for writing).
+
+* Position setpoint is syncronized with actual position when axis is disabled. Setpoint is updated once at positive edge of enable command. Note: The position setpoint is synced with actual position before first enable.
 
 # ECMC 6.3.2
 * Add command to enable/disable motion functions (all are by default enabled):

devEcmcSup/main/ecmcDefinitions.h

@@ -387,7 +387,9 @@ enum axisSubObjectType {
 
 #define ECMC_AXIS_DATA_STR_AXIS_ID "id"
 #define ECMC_AXIS_DATA_STR_POS_SET "traj.setpos"
+#define ECMC_AXIS_DATA_STR_POS_SET_EXTERNAL "traj.extsetpos"
 #define ECMC_AXIS_DATA_STR_POS_ACT "enc.actpos"
+#define ECMC_AXIS_DATA_STR_POS_ACT_EXTERNAL "enc.extactpos"
 #define ECMC_AXIS_DATA_STR_CNTRL_ERROR "cntrl.error"
 #define ECMC_AXIS_DATA_STR_POS_TARGET "traj.targetpos"
 #define ECMC_AXIS_DATA_STR_POS_ERROR "cntrl.poserror"
@@ -474,6 +476,8 @@ enum ecmcAxisDataType {
   ECMC_AXIS_DATA_INTERLOCK_FWD_TYPE    = 41,
   ECMC_AXIS_DATA_INTERLOCK_BWD_TYPE    = 42,
   ECMC_AXIS_DATA_ALLOW_PLC_WRITE       = 43,
+  ECMC_AXIS_DATA_POS_SET_EXTERNAL      = 44,
+  ECMC_AXIS_DATA_POS_ACT_EXTERNAL      = 45,
 };
 
 enum ecmcDataStorageType {

devEcmcSup/main/ecmcError.cpp

@@ -1787,6 +1787,11 @@ const char * ecmcError::convertErrorIdToString(int errorId) {
 
     break;
 
+  case 0x20052:
+    return "ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL";
+
+    break;
+
   case 0x20100:   // Data Recorder
     return "ERROR_DATA_RECORDER_BUFFER_NULL";
 

devEcmcSup/main/ecmcErrorsList.h

@@ -95,4 +95,6 @@
 #define ERROR_MAIN_PLUGIN_LIST_FULL 0x2004F
 #define ERROR_MAIN_PLUGIN_OBJECT_NULL 0x20050
 #define ERROR_MAIN_PLUGIN_INDEX_OUT_OF_RANGE 0x20051
+#define ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL 0x20052
+
 #endif  /* ECMCERRORSLIST_H_ */

devEcmcSup/motion/ecmcAxisBase.cpp

@@ -87,6 +87,7 @@ ecmcAxisBase::~ecmcAxisBase() {
 
 void ecmcAxisBase::initVars() {
   // errorReset();  //THIS IS NONO..
+  beforeFirstEnable_                    = 0;
   data_.axisType_              = ECMC_AXIS_TYPE_BASE;
   data_.command_.reset         = false;
   allowCmdFromOtherPLC_        = false;
@@ -490,6 +491,7 @@ int ecmcAxisBase::setEnableLocal(bool enable) {
     data_.status_.currentPositionSetpoint = data_.status_.currentPositionActual;
     data_.status_.currentPositionSetpointOld = data_.status_.currentPositionSetpoint;
     data_.status_.currentVelocitySetpoint = 0;
+    beforeFirstEnable_ = true;
   }
   traj_->setEnable(enable);
 
@@ -1188,12 +1190,20 @@ int ecmcAxisBase::setEnable(bool enable) {
   return 0;
 }
 
+double ecmcAxisBase::getExtSetPos() {
+  return data_.status_.externalTrajectoryPosition;
+}
+
 
 int ecmcAxisBase::setExtSetPos(double pos) {
   data_.status_.externalTrajectoryPosition = pos;
   return 0;
 }
 
+double ecmcAxisBase::getExtActPos() {
+  return data_.status_.externalEncoderPosition;
+}
+
 int ecmcAxisBase::setExtActPos(double pos) {
   data_.status_.externalEncoderPosition = pos;
   return 0;
@@ -1338,7 +1348,19 @@ int ecmcAxisBase::moveAbsolutePosition(
                             double velocitySet,
                             double accelerationSet,
                             double decelerationSet) {
-
+
+  if(getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) {
+    LOGERR(
+      "%s/%s:%d: ERROR (axis %d): Move to abs position failed since traj source is set to PLC (0x%x).\n",
+      __FILE__,
+      __FUNCTION__,
+      __LINE__,
+      data_.axisId_,
+      ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL);
+
+    return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL;
+  }
+
   int errorCode = getErrorID();
   if (errorCode) {
     return errorCode;
@@ -1376,6 +1398,18 @@ int ecmcAxisBase::moveRelativePosition(
                             double accelerationSet,
                             double decelerationSet) {
 
+  if(getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) {
+    LOGERR(
+      "%s/%s:%d: ERROR (axis %d): Move to abs position failed since traj source is set to PLC (0x%x).\n",
+      __FILE__,
+      __FUNCTION__,
+      __LINE__,
+      data_.axisId_,
+      ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL);
+
+    return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL;
+  }
+
   int errorCode = getErrorID();
   if (errorCode) {
     return errorCode;
@@ -1413,6 +1447,18 @@ int ecmcAxisBase::moveVelocity(
                             double accelerationSet,
                             double decelerationSet) {
 
+  if(getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) {
+    LOGERR(
+      "%s/%s:%d: ERROR (axis %d): Move to abs position failed since traj source is set to PLC (0x%x).\n",
+      __FILE__,
+      __FUNCTION__,
+      __LINE__,
+      data_.axisId_,
+      ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL);
+
+    return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL;
+  }
+
   int errorCode = getErrorID();
   if (errorCode) {
     return errorCode;
@@ -1459,6 +1505,19 @@ int ecmcAxisBase::moveHome(int    nCmdData,
                            double accelerationSet,
                            double decelerationSet
                            ) {
+
+  if(getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) {
+    LOGERR(
+      "%s/%s:%d: ERROR (axis %d): Move to abs position failed since traj source is set to PLC (0x%x).\n",
+      __FILE__,
+      __FUNCTION__,
+      __LINE__,
+      data_.axisId_,
+      ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL);
+
+    return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL;
+  }
+
   int errorCode = getErrorID();
   if (errorCode) {
     return errorCode;

devEcmcSup/motion/ecmcAxisBase.h

@@ -257,6 +257,8 @@ class ecmcAxisBase : public ecmcError {
   int                   getAllowPos();
   int                   getAllowConstVelo();
   int                   getAllowHome();
+  double                getExtSetPos();
+  double                getExtActPos();
 
  protected:
   void         initVars();
@@ -297,6 +299,7 @@ class ecmcAxisBase : public ecmcError {
   bool enableExtTrajVeloFilter_;
   bool enableExtEncVeloFilter_;
   bool disableAxisAtErrorReset_;
+  bool beforeFirstEnable_;
 };
 
 #endif  /* ECMCAXISBASE_H_ */

devEcmcSup/motion/ecmcAxisReal.cpp

@@ -60,7 +60,6 @@ ecmcAxisReal::~ecmcAxisReal() {
 }
 
 void ecmcAxisReal::initVars() {
-  initDone_                       = false;
   data_.command_.operationModeCmd = ECMC_MODE_OP_AUTO;
   currentDriveType_               = ECMC_NO_DRIVE;
   temporaryLocalTrajSource_       = false;
@@ -167,11 +166,11 @@ void ecmcAxisReal::execute(bool masterOK) {
       }
       // Only update if enable cmd is low to avoid change of setpoint 
       // during between enable and enabled
-      /*if (!getEnable()) {
+      if (!getEnable() && !beforeFirstEnable_ && masterOK) {
         data_.status_.currentPositionSetpoint =
           data_.status_.currentPositionActual;
-        traj_->setStartPos(data_.status_.currentPositionSetpoint);
-      }*/
+        traj_->setStartPos(data_.status_.currentPositionSetpoint);        
+      }
 
       if (data_.status_.enabledOld && !data_.status_.enabled &&
           data_.status_.enableOld && data_.command_.enable) {

devEcmcSup/motion/ecmcAxisReal.h

@@ -34,7 +34,6 @@ class ecmcAxisReal : public ecmcAxisBase {
 
  private:
   void               initVars(); 
-  bool initDone_;
   bool temporaryLocalTrajSource_;
   ecmcDriveBase *drv_;
   ecmcPIDController *cntrl_;

devEcmcSup/motion/ecmcAxisVirt.cpp

@@ -29,7 +29,6 @@ ecmcAxisVirt::~ecmcAxisVirt()
 {}
 
 void ecmcAxisVirt::initVars() {
-  initDone_                 = false;
   temporaryLocalTrajSource_ = false;
 }
 
@@ -101,9 +100,12 @@ void ecmcAxisVirt::execute(bool masterOK) {
       if (getExecute()) {
         setExecute(false);
       }
-      /*data_.status_.currentPositionSetpoint =
-        data_.status_.currentPositionActual;
-      traj_->setStartPos(data_.status_.currentPositionSetpoint);*/
+
+      if(!beforeFirstEnable_ && masterOK){
+        data_.status_.currentPositionSetpoint =
+          data_.status_.currentPositionActual;
+        traj_->setStartPos(data_.status_.currentPositionSetpoint);        
+      }
     }
 
     if (!masterOK) {

devEcmcSup/motion/ecmcAxisVirt.h

@@ -38,7 +38,6 @@ class ecmcAxisVirt: public ecmcAxisBase
 
 private:
   void initVars();
-  bool initDone_;
   bool temporaryLocalTrajSource_;  
 };
 

devEcmcSup/plc/ecmcPLCDataIF.cpp

@@ -546,6 +546,14 @@ int ecmcPLCDataIF::readAxis() {
     data_ = static_cast<double>(axis_->getAllowCmdFromPLC());
     break;
 
+  case ECMC_AXIS_DATA_POS_SET_EXTERNAL:
+    data_ = axis_->getExtSetPos();
+    break;
+
+  case ECMC_AXIS_DATA_POS_ACT_EXTERNAL:
+    data_ = axis_->getExtActPos();
+    break;
+
   default:
     return setErrorID(__FILE__,
                       __FUNCTION__,
@@ -804,6 +812,14 @@ int ecmcPLCDataIF::writeAxis() {
     return axis_->setAllowCmdFromPLC(data_>=1);
     break;
 
+  case ECMC_AXIS_DATA_POS_SET_EXTERNAL:
+    return 0; //axis_->setExtSetPos(data_);
+    break;
+
+  case ECMC_AXIS_DATA_POS_ACT_EXTERNAL:
+    return 0; //axis_->setExtActPos(data_);
+    break;
+
   default:
     return setErrorID(__FILE__,
                       __FUNCTION__,
@@ -1163,6 +1179,18 @@ ecmcAxisDataType ecmcPLCDataIF::parseAxisDataSource(char *axisDataSource) {
     return ECMC_AXIS_DATA_ALLOW_PLC_WRITE;
   }
 
+  npos = strcmp(varName, ECMC_AXIS_DATA_STR_POS_SET_EXTERNAL);
+
+  if (npos == 0) {    
+    return ECMC_AXIS_DATA_POS_SET_EXTERNAL;
+  }
+
+  npos = strcmp(varName, ECMC_AXIS_DATA_STR_POS_ACT_EXTERNAL);
+
+  if (npos == 0) {    
+    return ECMC_AXIS_DATA_POS_ACT_EXTERNAL;
+  }
+
   return ECMC_AXIS_DATA_NONE;
 }
 

devEcmcSup/plc/ecmcPLCTask.cpp

@@ -772,7 +772,8 @@ int ecmcPLCTask::loadMcLib() {
   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);
-
+  ecmcPLCTaskAddFunction("mc_move_ext_pos", mc_move_ext_pos);
+
   if (mc_cmd_count != cmdCounter) {
     LOGERR("%s/%s:%d: PLC Lib MC command count missmatch (0x%x).\n",
            __FILE__,