From 62d66d2c19f9e92ff3abcdb851afc511f715f022 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Fri, 12 Mar 2021 11:42:26 +0100 Subject: [PATCH 1/5] Add mc_move_gear_in() plc function. --- devEcmcSup/plc/ecmcPLCTask.cpp | 3 +- devEcmcSup/plc/ecmcPLCTask_libMc.inc | 88 ++++++++++++++++++++++++++-- 2 files changed, 86 insertions(+), 5 deletions(-) diff --git a/devEcmcSup/plc/ecmcPLCTask.cpp b/devEcmcSup/plc/ecmcPLCTask.cpp index 09aa91cd..a61b2336 100644 --- a/devEcmcSup/plc/ecmcPLCTask.cpp +++ b/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_gear_in", mc_move_gear_in); + if (mc_cmd_count != cmdCounter) { LOGERR("%s/%s:%d: PLC Lib MC command count missmatch (0x%x).\n", __FILE__, diff --git a/devEcmcSup/plc/ecmcPLCTask_libMc.inc b/devEcmcSup/plc/ecmcPLCTask_libMc.inc index e56eadfa..4ca0dc21 100644 --- a/devEcmcSup/plc/ecmcPLCTask_libMc.inc +++ b/devEcmcSup/plc/ecmcPLCTask_libMc.inc @@ -13,6 +13,8 @@ #ifndef ecmcPLC_libMc_inc_ #define ecmcPLC_libMc_inc_ +#include "../main/ecmcErrorsList.h" + #define CHECK_PLC_AXIS_RETURN_IF_ERROR(axIndex) { \ if(axIndex>=ECMC_MAX_AXES || axIndex<=0){ \ mc_errorCode=0; \ @@ -36,6 +38,7 @@ const char* mcLibCmdList[] = { "mc_move_abs(", "mc_get_homed(", "mc_get_axis_err(", "mc_set_enable_motion_funcs(", + "mc_move_gear_in(", }; ecmcAxisBase *ecmcPLCTask::statAxes_[ECMC_MAX_AXES]={}; @@ -46,13 +49,20 @@ static int statLastAxesExecuteVel_[ECMC_MAX_AXES]={}; static int statLastAxesExecuteHalt_[ECMC_MAX_AXES]={}; static int statLastAxesExecuteHome_[ECMC_MAX_AXES]={}; static int mc_errorCode=0; -static int mc_cmd_count = 12; +static int mc_cmd_count = 13; +// Note cannot use ecmcAxisBase::move* since the execution in plc with execute needs to be correct inline double mc_move_abs(double axIndex,double execute,double pos, double vel, double acc,double dec) { int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); - + + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + int trigg=!statLastAxesExecuteAbs_[index] && (bool)execute; statLastAxesExecuteAbs_[index]=execute; @@ -88,11 +98,18 @@ inline double mc_move_abs(double axIndex,double execute,double pos, double vel, return 0.0; } +// Note cannot use ecmcAxisBase::move* since the execution in plc with execute needs to be correct inline double mc_move_rel(double axIndex,double execute,double pos, double vel, double acc,double dec) { int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + int trigg=!statLastAxesExecuteRel_[index] && (bool)execute; statLastAxesExecuteRel_[index]=execute; @@ -128,11 +145,18 @@ inline double mc_move_rel(double axIndex,double execute,double pos, double vel, return 0.0; } +// Note cannot use ecmcAxisBase::move* since the execution in plc with execute needs to be correct inline double mc_move_vel(double axIndex,double execute, double vel, double acc,double dec) { int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + int trigg=!statLastAxesExecuteVel_[index] && (bool)execute; statLastAxesExecuteVel_[index]=execute; @@ -167,11 +191,18 @@ inline double mc_move_vel(double axIndex,double execute, double vel, double acc, return 0.0; } +// Note cannot use ecmcAxisBase::move* since the execution in plc with execute needs to be correct inline double mc_home(double axIndex,double execute,double seqId, double velTwoardsCam,double velOffCam) { int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + int trigg=!statLastAxesExecuteHome_[index] && (bool)execute; statLastAxesExecuteHome_[index]=execute; @@ -215,6 +246,12 @@ inline double mc_halt(double axIndex,double execute) int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + if (!(bool)execute){ mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setExecute((bool)execute); return mc_errorCode; @@ -286,11 +323,54 @@ inline double mc_set_enable_motion_funcs(double axIndex, (int)enablePos,(int)enableVel,(int)enableHome); } - inline double mc_get_err() { return (double)mc_errorCode; } -#endif /* ecmcPLC_libMc_inc_ */ +inline double mc_move_gear_in(double axIndex,double execute, double vel, double acc, double dec) +{ + int index=(int)axIndex; + CHECK_PLC_AXIS_RETURN_IF_ERROR(index); + + // check traj source, must be internal + if(ecmcPLCTask::statAxes_[index]->getTrajDataSourceType() != ECMC_DATA_SOURCE_INTERNAL) { + mc_errorCode=ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + return ERROR_MAIN_TRAJ_SOURCE_NOT_INTERNAL; + } + + int trigg=!statLastAxesExecuteAbs_[index] && (bool)execute; + statLastAxesExecuteAbs_[index]=execute; + + if (!(bool)execute){ + mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setExecute((bool)execute); + return mc_errorCode; + } + + if(trigg){ + mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setExecute(0); + if(mc_errorCode){ + return mc_errorCode; + } + + ecmcPLCTask::statAxes_[index]->getSeq()->setTargetVel(vel); + ecmcPLCTask::statAxes_[index]->getTraj()->setDec(dec); + ecmcPLCTask::statAxes_[index]->getTraj()->setAcc(acc); + mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setCommand(ECMC_CMD_MOVEABS); + if(mc_errorCode){ + return mc_errorCode; + } + mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setCmdData(1); + if(mc_errorCode){ + return mc_errorCode; + } + mc_errorCode=(double)ecmcPLCTask::statAxes_[index]->setExecute((bool)execute); + if(mc_errorCode){ + return mc_errorCode; + } + } + return 0.0; +} + +#endif /* ecmcPLC_libMc_inc_ */ From 15e41fc6172102e28becb10ed87dc61755721177 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Fri, 12 Mar 2021 11:43:15 +0100 Subject: [PATCH 2/5] Only allow local motion commands if traj source is internal (also mc_halt..). --- devEcmcSup/main/ecmcError.cpp | 5 +++ devEcmcSup/main/ecmcErrorsList.h | 2 ++ devEcmcSup/motion/ecmcAxisBase.cpp | 51 +++++++++++++++++++++++++++++- 3 files changed, 57 insertions(+), 1 deletion(-) diff --git a/devEcmcSup/main/ecmcError.cpp b/devEcmcSup/main/ecmcError.cpp index ba1759fd..db06fff2 100644 --- a/devEcmcSup/main/ecmcError.cpp +++ b/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"; diff --git a/devEcmcSup/main/ecmcErrorsList.h b/devEcmcSup/main/ecmcErrorsList.h index b0a5a2fa..0b3ddb01 100644 --- a/devEcmcSup/main/ecmcErrorsList.h +++ b/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_ */ diff --git a/devEcmcSup/motion/ecmcAxisBase.cpp b/devEcmcSup/motion/ecmcAxisBase.cpp index 08009075..851bb735 100644 --- a/devEcmcSup/motion/ecmcAxisBase.cpp +++ b/devEcmcSup/motion/ecmcAxisBase.cpp @@ -1338,7 +1338,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 +1388,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 +1437,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 +1495,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; From 03c5d010e6ac0345c4eb80e9e04457f2cf7fab37 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Fri, 12 Mar 2021 16:31:14 +0100 Subject: [PATCH 3/5] Add plc function mc_move_gear_in() and some plc vars and more. Keep auto sync untill first enable. mc_move_gear_in() will run the axis to the external plc setpoint of the defined axis. Added variables: 1. ax.traj.extsetpos : external plc setpoint position of the axis (rw) 2. ax.enc.extactpos : external plc actual position of the axis (rw) Keep auto sync of setpoint to encoder value untill first enable. --- devEcmcSup/main/ecmcDefinitions.h | 4 ++++ devEcmcSup/motion/ecmcAxisBase.cpp | 10 ++++++++++ devEcmcSup/motion/ecmcAxisBase.h | 3 +++ devEcmcSup/motion/ecmcAxisReal.cpp | 7 +++---- devEcmcSup/motion/ecmcAxisReal.h | 1 - devEcmcSup/motion/ecmcAxisVirt.cpp | 10 ++++++---- devEcmcSup/motion/ecmcAxisVirt.h | 1 - devEcmcSup/plc/ecmcPLCDataIF.cpp | 28 ++++++++++++++++++++++++++++ devEcmcSup/plc/ecmcPLCTask_libMc.inc | 3 ++- 9 files changed, 56 insertions(+), 11 deletions(-) diff --git a/devEcmcSup/main/ecmcDefinitions.h b/devEcmcSup/main/ecmcDefinitions.h index b3a9bdd5..dc3050d3 100644 --- a/devEcmcSup/main/ecmcDefinitions.h +++ b/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 { diff --git a/devEcmcSup/motion/ecmcAxisBase.cpp b/devEcmcSup/motion/ecmcAxisBase.cpp index 851bb735..afe8bccb 100644 --- a/devEcmcSup/motion/ecmcAxisBase.cpp +++ b/devEcmcSup/motion/ecmcAxisBase.cpp @@ -87,6 +87,7 @@ ecmcAxisBase::~ecmcAxisBase() { void ecmcAxisBase::initVars() { // errorReset(); //THIS IS NONO.. + initDone_ = 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; + initDone_ = 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; diff --git a/devEcmcSup/motion/ecmcAxisBase.h b/devEcmcSup/motion/ecmcAxisBase.h index 0eceed0d..d9965947 100644 --- a/devEcmcSup/motion/ecmcAxisBase.h +++ b/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 initDone_; }; #endif /* ECMCAXISBASE_H_ */ diff --git a/devEcmcSup/motion/ecmcAxisReal.cpp b/devEcmcSup/motion/ecmcAxisReal.cpp index ed60b53a..74ae76b4 100644 --- a/devEcmcSup/motion/ecmcAxisReal.cpp +++ b/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() && !initDone_ && 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) { diff --git a/devEcmcSup/motion/ecmcAxisReal.h b/devEcmcSup/motion/ecmcAxisReal.h index f1159957..b35e0228 100644 --- a/devEcmcSup/motion/ecmcAxisReal.h +++ b/devEcmcSup/motion/ecmcAxisReal.h @@ -34,7 +34,6 @@ class ecmcAxisReal : public ecmcAxisBase { private: void initVars(); - bool initDone_; bool temporaryLocalTrajSource_; ecmcDriveBase *drv_; ecmcPIDController *cntrl_; diff --git a/devEcmcSup/motion/ecmcAxisVirt.cpp b/devEcmcSup/motion/ecmcAxisVirt.cpp index 2c872679..6975b3e7 100644 --- a/devEcmcSup/motion/ecmcAxisVirt.cpp +++ b/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(!initDone_ && masterOK){ + data_.status_.currentPositionSetpoint = + data_.status_.currentPositionActual; + traj_->setStartPos(data_.status_.currentPositionSetpoint); + } } if (!masterOK) { diff --git a/devEcmcSup/motion/ecmcAxisVirt.h b/devEcmcSup/motion/ecmcAxisVirt.h index 99739017..bb5507f2 100644 --- a/devEcmcSup/motion/ecmcAxisVirt.h +++ b/devEcmcSup/motion/ecmcAxisVirt.h @@ -38,7 +38,6 @@ class ecmcAxisVirt: public ecmcAxisBase private: void initVars(); - bool initDone_; bool temporaryLocalTrajSource_; }; diff --git a/devEcmcSup/plc/ecmcPLCDataIF.cpp b/devEcmcSup/plc/ecmcPLCDataIF.cpp index 8bc02150..d5add51e 100644 --- a/devEcmcSup/plc/ecmcPLCDataIF.cpp +++ b/devEcmcSup/plc/ecmcPLCDataIF.cpp @@ -546,6 +546,14 @@ int ecmcPLCDataIF::readAxis() { data_ = static_cast(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 axis_->setExtSetPos(data_); + break; + + case ECMC_AXIS_DATA_POS_ACT_EXTERNAL: + return 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; } diff --git a/devEcmcSup/plc/ecmcPLCTask_libMc.inc b/devEcmcSup/plc/ecmcPLCTask_libMc.inc index 4ca0dc21..eef1bb14 100644 --- a/devEcmcSup/plc/ecmcPLCTask_libMc.inc +++ b/devEcmcSup/plc/ecmcPLCTask_libMc.inc @@ -352,7 +352,8 @@ inline double mc_move_gear_in(double axIndex,double execute, double vel, double if(mc_errorCode){ return mc_errorCode; } - + double pos = ecmcPLCTask::statAxes_[index]->getExtSetPos(); + ecmcPLCTask::statAxes_[index]->getSeq()->setTargetPos(pos); ecmcPLCTask::statAxes_[index]->getSeq()->setTargetVel(vel); ecmcPLCTask::statAxes_[index]->getTraj()->setDec(dec); ecmcPLCTask::statAxes_[index]->getTraj()->setAcc(acc); From e7e7126a1928d5f7f868332ddfbb03a291e5a2d1 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Mon, 15 Mar 2021 10:09:16 +0100 Subject: [PATCH 4/5] Rename mc_gear_in() to mc_move_ext_pos() and make these vars ro to avoid issues with two vars writing to the same axis property. - ax.enc.extactpos : now read only, ax.enc.actpos can be used for writing like before. - ax.traj.extsetpos : now read only, ax.enc.setpos can be used for writing like before. --- devEcmcSup/motion/ecmcAxisBase.cpp | 4 ++-- devEcmcSup/motion/ecmcAxisBase.h | 2 +- devEcmcSup/motion/ecmcAxisReal.cpp | 2 +- devEcmcSup/motion/ecmcAxisVirt.cpp | 2 +- devEcmcSup/plc/ecmcPLCDataIF.cpp | 4 ++-- devEcmcSup/plc/ecmcPLCTask.cpp | 2 +- devEcmcSup/plc/ecmcPLCTask_libMc.inc | 4 ++-- 7 files changed, 10 insertions(+), 10 deletions(-) diff --git a/devEcmcSup/motion/ecmcAxisBase.cpp b/devEcmcSup/motion/ecmcAxisBase.cpp index afe8bccb..f4eb64c7 100644 --- a/devEcmcSup/motion/ecmcAxisBase.cpp +++ b/devEcmcSup/motion/ecmcAxisBase.cpp @@ -87,7 +87,7 @@ ecmcAxisBase::~ecmcAxisBase() { void ecmcAxisBase::initVars() { // errorReset(); //THIS IS NONO.. - initDone_ = 0; + beforeFirstEnable_ = 0; data_.axisType_ = ECMC_AXIS_TYPE_BASE; data_.command_.reset = false; allowCmdFromOtherPLC_ = false; @@ -491,7 +491,7 @@ int ecmcAxisBase::setEnableLocal(bool enable) { data_.status_.currentPositionSetpoint = data_.status_.currentPositionActual; data_.status_.currentPositionSetpointOld = data_.status_.currentPositionSetpoint; data_.status_.currentVelocitySetpoint = 0; - initDone_ = true; + beforeFirstEnable_ = true; } traj_->setEnable(enable); diff --git a/devEcmcSup/motion/ecmcAxisBase.h b/devEcmcSup/motion/ecmcAxisBase.h index d9965947..b615bd18 100644 --- a/devEcmcSup/motion/ecmcAxisBase.h +++ b/devEcmcSup/motion/ecmcAxisBase.h @@ -299,7 +299,7 @@ class ecmcAxisBase : public ecmcError { bool enableExtTrajVeloFilter_; bool enableExtEncVeloFilter_; bool disableAxisAtErrorReset_; - bool initDone_; + bool beforeFirstEnable_; }; #endif /* ECMCAXISBASE_H_ */ diff --git a/devEcmcSup/motion/ecmcAxisReal.cpp b/devEcmcSup/motion/ecmcAxisReal.cpp index 74ae76b4..b2e13721 100644 --- a/devEcmcSup/motion/ecmcAxisReal.cpp +++ b/devEcmcSup/motion/ecmcAxisReal.cpp @@ -166,7 +166,7 @@ void ecmcAxisReal::execute(bool masterOK) { } // Only update if enable cmd is low to avoid change of setpoint // during between enable and enabled - if (!getEnable() && !initDone_ && masterOK) { + if (!getEnable() && !beforeFirstEnable_ && masterOK) { data_.status_.currentPositionSetpoint = data_.status_.currentPositionActual; traj_->setStartPos(data_.status_.currentPositionSetpoint); diff --git a/devEcmcSup/motion/ecmcAxisVirt.cpp b/devEcmcSup/motion/ecmcAxisVirt.cpp index 6975b3e7..769ae261 100644 --- a/devEcmcSup/motion/ecmcAxisVirt.cpp +++ b/devEcmcSup/motion/ecmcAxisVirt.cpp @@ -101,7 +101,7 @@ void ecmcAxisVirt::execute(bool masterOK) { setExecute(false); } - if(!initDone_ && masterOK){ + if(!beforeFirstEnable_ && masterOK){ data_.status_.currentPositionSetpoint = data_.status_.currentPositionActual; traj_->setStartPos(data_.status_.currentPositionSetpoint); diff --git a/devEcmcSup/plc/ecmcPLCDataIF.cpp b/devEcmcSup/plc/ecmcPLCDataIF.cpp index d5add51e..aee1187b 100644 --- a/devEcmcSup/plc/ecmcPLCDataIF.cpp +++ b/devEcmcSup/plc/ecmcPLCDataIF.cpp @@ -813,11 +813,11 @@ int ecmcPLCDataIF::writeAxis() { break; case ECMC_AXIS_DATA_POS_SET_EXTERNAL: - return axis_->setExtSetPos(data_); + return 0; //axis_->setExtSetPos(data_); break; case ECMC_AXIS_DATA_POS_ACT_EXTERNAL: - return axis_->setExtActPos(data_); + return 0; //axis_->setExtActPos(data_); break; default: diff --git a/devEcmcSup/plc/ecmcPLCTask.cpp b/devEcmcSup/plc/ecmcPLCTask.cpp index a61b2336..05f916f8 100644 --- a/devEcmcSup/plc/ecmcPLCTask.cpp +++ b/devEcmcSup/plc/ecmcPLCTask.cpp @@ -772,7 +772,7 @@ 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_gear_in", mc_move_gear_in); + 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", diff --git a/devEcmcSup/plc/ecmcPLCTask_libMc.inc b/devEcmcSup/plc/ecmcPLCTask_libMc.inc index eef1bb14..fd2bcbbf 100644 --- a/devEcmcSup/plc/ecmcPLCTask_libMc.inc +++ b/devEcmcSup/plc/ecmcPLCTask_libMc.inc @@ -38,7 +38,7 @@ const char* mcLibCmdList[] = { "mc_move_abs(", "mc_get_homed(", "mc_get_axis_err(", "mc_set_enable_motion_funcs(", - "mc_move_gear_in(", + "mc_move_ext_pos(", }; ecmcAxisBase *ecmcPLCTask::statAxes_[ECMC_MAX_AXES]={}; @@ -328,7 +328,7 @@ inline double mc_get_err() return (double)mc_errorCode; } -inline double mc_move_gear_in(double axIndex,double execute, double vel, double acc, double dec) +inline double mc_move_ext_pos(double axIndex,double execute, double vel, double acc, double dec) { int index=(int)axIndex; CHECK_PLC_AXIS_RETURN_IF_ERROR(index); From 9708da3e122a5b4c4e52ecefe201c541a68f15d9 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Mon, 15 Mar 2021 10:29:34 +0100 Subject: [PATCH 5/5] Update RELEASE.md --- RELEASE.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/RELEASE.md b/RELEASE.md index 6d961d3f..9d1dcccb 100644 --- a/RELEASE.md +++ b/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.enc.extactpos (ro): Current external plc calculated actual position (ax.enc.actpos can be used for writing). + + - ax.traj.extsetpos (ro): Current external plc calculated setpoint position(ax.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):