From 731bd7f9686eaf933f6794a254c2d42b20d8e854 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Fri, 2 Feb 2024 10:57:37 +0100 Subject: [PATCH 1/5] WIP --- GNUmakefile | 3 +-- iocsh/el7031.script | 4 ++-- scripts/addSafetyAxisToGroup.cmd | 19 ------------------- src/ecmcSafetyGroup.h | 2 +- 4 files changed, 4 insertions(+), 24 deletions(-) delete mode 100644 scripts/addSafetyAxisToGroup.cmd diff --git a/GNUmakefile b/GNUmakefile index 3787072..aee150f 100644 --- a/GNUmakefile +++ b/GNUmakefile @@ -17,7 +17,7 @@ OPT_CXXFLAGS_YES = -O3 # dependencies ECmasterECMC_VERSION = v1.1.0 # motorECMC_VERSION = 7.0.7-ESS -ecmc_VERSION = v9.0.1_RC4 +ecmc_VERSION = sandst_a ################################################################################ # THIS RELATES TO THE EtherCAT MASTER LIBRARY @@ -41,5 +41,4 @@ SOURCES += $(SRC_DIR)/ecmcSafetyGroup.cpp HEADERS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.h)) DBDS += $(foreach d,${SRC_DIR}, $(wildcard $d/*.dbd)) SCRIPTS += $(BASE_DIR)/scripts/startup.cmd -SCRIPTS += $(BASE_DIR)/scripts/addSafetyAxisToGroup.cmd TEMPLATES += $(wildcard $(DB_DIR)/*.template) diff --git a/iocsh/el7031.script b/iocsh/el7031.script index 3c63d15..f4aa5d2 100644 --- a/iocsh/el7031.script +++ b/iocsh/el7031.script @@ -1,11 +1,11 @@ ############################################################################## ## Example config for el7031 -require ecmccfg v9.0.1_RC4,"ECMC_VER=v9.0.1_RC4,ENG_MODE=1" +require ecmccfg v9.0.1_RC4,"ECMC_VER=safety,ENG_MODE=1" epicsEnvSet(IOC,c6025a) # Load components lib -require ecmccomp sandst_a +require ecmccomp ############################################################################## ## Configure hardware diff --git a/scripts/addSafetyAxisToGroup.cmd b/scripts/addSafetyAxisToGroup.cmd deleted file mode 100644 index 2b2f190..0000000 --- a/scripts/addSafetyAxisToGroup.cmd +++ /dev/null @@ -1,19 +0,0 @@ - -#============================================================================== -# addSafetyAxisToGroup.cmd -#-------------- Information: -#- Description: ecmc_plugin_safety addSafetyAxisToGroup -#- -#- by Anders Sandström, Paul Scherrer Institute, 2024 -#- email: anders.sandstroem@psi.ch -#- -#-############################################################################### -#- -#- Arguments -#- [mandatory] -#- GROUP = Safety group index -#- AXIS = Axis index -#- -################################################################################# - -xxxx \ No newline at end of file diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index e106dc0..877daa9 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -56,7 +56,7 @@ class ecmcSafetyGroup : public asynPortDriver { // int status_; int axesCounter_; - static std::vector axes_; + std::vector axes_; // Some generic utility functions static uint8_t getUint8(uint8_t* data); From 0d2c06a84642df7c7f948cdedeb76225d63552c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Fri, 2 Feb 2024 15:46:12 +0100 Subject: [PATCH 2/5] WIP --- src/ecmcPluginSafety.c | 2 +- src/ecmcSafetyGroup.cpp | 129 +++++++++++++++++++++++++++++++--------- src/ecmcSafetyGroup.h | 25 ++++---- 3 files changed, 114 insertions(+), 42 deletions(-) diff --git a/src/ecmcPluginSafety.c b/src/ecmcPluginSafety.c index a33f432..c24349d 100644 --- a/src/ecmcPluginSafety.c +++ b/src/ecmcPluginSafety.c @@ -64,7 +64,7 @@ void safetyDestruct(void) **/ int safetyRealtime(int ecmcError) { - if(destructs_) return; + if(destructs_) return 0; executeSafetyGroups(); lastEcmcError = ecmcError; diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index 3ed9290..ce6c855 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -67,8 +67,11 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, axesCounter_ = 0; ecmcSampleRateHz_ = getEcmcSampleRate(); dataSourcesLinked_ = 0; - dataItemRampDownCmd_ = NULL; - dataItemStandStillStat_ = NULL; + //dataItemRampDownCmd_ = NULL; + //dataItemStandStillStat_ = NULL; + ecMaster_ = NULL; + ecEntryRampDown_ = NULL; + ecEntryStandstill_ = NULL; masterId_ = 0; slaveIdRampDown_ = 0; bitRampDown_ = 0; @@ -76,6 +79,7 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, bitStandStill_ = 0; aliasRampDown_[0] = 0; aliasStandStill_[0] = 0; + rampDownCmd_ = 0; // Config defaults cfgDbgMode_ = 0; @@ -247,27 +251,56 @@ void ecmcSafetyGroup::validateAxes() { } void ecmcSafetyGroup::connectToDataSources() { - if( dataSourcesLinked_ ) { - return; - } - // Get dataItem for rampdown command - dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_); - if(!dataItemRampDownCmd_) { - throw std::runtime_error( "Safety: Data item for ramp down command NULL."); - } - - // Get dataItem for axes standstill status - dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_); - if(!dataItemStandStillStat_) { - throw std::runtime_error( "Safety: Data item for axes standstill status NULL."); - } - - if(cfgDbgMode_) { - printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_); + ecMaster_ = (ecmcEc*)getEcMaster(); + + if(!ecMaster_) { + throw std::runtime_error( "Safety: EtherCAT master object NULL."); } + // rampdown + ecmcEcSlave *slave = ecMaster_->findSlave(slaveIdRampDown_); + if(!slave) { + throw std::runtime_error( "Safety: EtherCAT slave for rampdown I/O NULL."); + } + + ecEntryRampDown_ = slave->findEntry(aliasRampDown_); + if(!ecEntryRampDown_) { + throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL."); + } + + // standstill + slave = ecMaster_->findSlave(slaveIdStandStill_); + if(!slave) { + throw std::runtime_error( "Safety: EtherCAT slave for standstill I/O NULL."); + } + + ecEntryStandstill_ = slave->findEntry(aliasStandStill_); + if(!ecEntryStandstill_) { + throw std::runtime_error( "Safety: EtherCAT entry for rampdown I/O NULL."); + }; + dataSourcesLinked_ = 1; + + return; + + //// Get dataItem for rampdown command + //dataItemRampDownCmd_ = (ecmcDataItem*) getEcmcDataItem(sEcRampDownCmdNameStrip_); + //if(!dataItemRampDownCmd_) { + // throw std::runtime_error( "Safety: Data item for ramp down command NULL."); + //} + // + //// Get dataItem for axes standstill status + //dataItemStandStillStat_ = (ecmcDataItem*) getEcmcDataItem(sEcAxesStandStillStatStrip_); + //if(!dataItemStandStillStat_) { + // throw std::runtime_error( "Safety: Data item for axes standstill status NULL."); + //} + // + //if(cfgDbgMode_) { + // printf("Safety: Safety group \"%s\"\": Data sources linked.\n",sName_); + //} + // + //dataSourcesLinked_ = 1; } void ecmcSafetyGroup::initAsyn() { @@ -294,7 +327,50 @@ std::string ecmcSafetyGroup::to_string(int value) { // Executed by ecmc rt thread. void ecmcSafetyGroup::execute() { - // xxx + + uint64_t data = 1; + + if(!ecEntryRampDown_) { + return; + } + + if(ecEntryRampDown_->readBit(bitRampDown_, + &data)) { + throw std::out_of_range("Safety: Read rampdown cmd failed"); + } + rampDownCmd_ = data > 0; + + if(rampDownCmd_) { + // set safety interlock in ecmc + setSafetyInterlocks(); + } + +/* + //Needed functions in motion.h + int getAxisBusy(int axisIndex, + int *value); + int getAxisEncVelo(int axisIndex, + double *velo); + int getAxisTrajVelo(int axisIndex, + double *velo); + int getAxisEnabled(int axisIndex, + int *value); + int setAxisEnable(int axisIndex, + int value); + int setAxisClearEmergencyInterlock(int axisIndex); + int setAxisEmergencyStopInterlock(int axisIndex, + double deceleration); + */ +} + +// Set safety interlock +void ecmcSafetyGroup::setSafetyInterlocks() { + for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { + if(!*saxis) { + throw std::runtime_error( "Safety: Axis object NULL."); + } + setAxisEmergencyStopInterlock((*saxis)->axisIndex_,1000); + } } asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { @@ -309,16 +385,13 @@ asynStatus ecmcSafetyGroup::readInt32(asynUser *pasynUser, epicsInt32 *value) { void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) { - ecmcAxisBase *axis= (ecmcAxisBase*) getAxisPointer(axisId); - - if(axis) { - safetyAxis* saxis = new safetyAxis(axis, axisId, veloLimit, standStillTimeMs); - axes_.push_back(saxis); - axesCounter_++; - } else { - throw std::out_of_range("Safety: Invalid axis id"); + if(!getAxisValid(axisId)) { + throw std::out_of_range("Safety: Invalid axis id"); } + axes_.push_back(new safetyAxis(axisId, veloLimit, standStillTimeMs)); + axesCounter_++; + if(cfgDbgMode_) { printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_); } diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index f0ed774..bf9f3d7 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -18,23 +18,21 @@ #include "ecmcSafetyPlgDefs.h" #include #include "ecmcAxisBase.h" +#include "ecmcEc.h" #include class safetyAxis { public: - safetyAxis(ecmcAxisBase* axis, - int axisIndex, + safetyAxis(int axisIndex, double veloLimit, int standStillTimeMs) { veloLimit_ = veloLimit; - axisIndex_ = axisIndex; - axis_ = axis; + axisIndex_ = axisIndex; standStillTimeMs_ = standStillTimeMs; } double veloLimit_; int axisIndex_; - int standStillTimeMs_; - ecmcAxisBase* axis_; + int standStillTimeMs_; }; class ecmcSafetyGroup : public asynPortDriver { @@ -66,13 +64,14 @@ class ecmcSafetyGroup : public asynPortDriver { void validateAxes(); void stripBits(); void connectToDataSources(); + void setSafetyInterlocks(); void parseConfigStr(const char *configStr); void initAsyn(); double ecmcSampleRateHz_; int dataSourcesLinked_; // To avoid link several times int objectId_; // Unique object id int cycleCounter_; - + int rampDownCmd_; // Config options int cfgDbgMode_; // Config: allow dbg printouts @@ -80,7 +79,7 @@ class ecmcSafetyGroup : public asynPortDriver { int asynStatusId_; int status_; int axesCounter_; - std::vector axes_; + std::vector axes_; char* sName_; char* sEcRampDownCmdNameOrg_; @@ -88,8 +87,8 @@ class ecmcSafetyGroup : public asynPortDriver { char* sEcRampDownCmdNameStrip_; char* sEcAxesStandStillStatStrip_; char* sConfig_; - ecmcDataItem *dataItemRampDownCmd_; - ecmcDataItem *dataItemStandStillStat_; + //ecmcDataItem *dataItemRampDownCmd_; + //ecmcDataItem *dataItemStandStillStat_; int delayMs_; int masterId_; int slaveIdRampDown_; @@ -98,9 +97,9 @@ class ecmcSafetyGroup : public asynPortDriver { int bitStandStill_; char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; - - std::vector axes_; - + ecmcEc *ecMaster_; + ecmcEcEntry *ecEntryRampDown_; + ecmcEcEntry *ecEntryStandstill_; // Some generic utility functions static uint8_t getUint8(uint8_t* data); static int8_t getInt8(uint8_t* data); From e1731b713eeed92059e419670fc522afdaecb9ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Thu, 15 Feb 2024 13:45:04 +0100 Subject: [PATCH 3/5] WIP --- iocsh/el7042-0052.script | 2 +- src/ecmcSafetyGroup.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/iocsh/el7042-0052.script b/iocsh/el7042-0052.script index 9759faf..7d6cfc6 100644 --- a/iocsh/el7042-0052.script +++ b/iocsh/el7042-0052.script @@ -1,7 +1,7 @@ ############################################################################## ## Example config for el7031 -require ecmccfg v9.0.1_RC4,"ECMC_VER=safety,ENG_MODE=1" +require ecmccfg v9.0.1_RC4,"ECMC_VER=sandst_a,ENG_MODE=1" epicsEnvSet(IOC,c6025a) # Load components lib diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index ce6c855..f8e8d83 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -348,15 +348,15 @@ void ecmcSafetyGroup::execute() { /* //Needed functions in motion.h int getAxisBusy(int axisIndex, - int *value); + int *value); int getAxisEncVelo(int axisIndex, - double *velo); + double *velo); int getAxisTrajVelo(int axisIndex, - double *velo); + double *velo); int getAxisEnabled(int axisIndex, - int *value); + int *value); int setAxisEnable(int axisIndex, - int value); + int value); int setAxisClearEmergencyInterlock(int axisIndex); int setAxisEmergencyStopInterlock(int axisIndex, double deceleration); From 282a67c1a6673b69c0da1b09ae63b34a444d3f20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Thu, 15 Feb 2024 15:52:09 +0100 Subject: [PATCH 4/5] WIP --- src/ecmcSafetyGroup.cpp | 75 +++++++++++++++++++++++++++++++---------- src/ecmcSafetyGroup.h | 15 +++++++-- 2 files changed, 69 insertions(+), 21 deletions(-) diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index f8e8d83..89acd21 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -67,8 +67,6 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, axesCounter_ = 0; ecmcSampleRateHz_ = getEcmcSampleRate(); dataSourcesLinked_ = 0; - //dataItemRampDownCmd_ = NULL; - //dataItemStandStillStat_ = NULL; ecMaster_ = NULL; ecEntryRampDown_ = NULL; ecEntryStandstill_ = NULL; @@ -80,6 +78,8 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, aliasRampDown_[0] = 0; aliasStandStill_[0] = 0; rampDownCmd_ = 0; + axesAreStandstill_ = 0; + rampDownCmdOld_ = 0; // Config defaults cfgDbgMode_ = 0; @@ -328,23 +328,28 @@ std::string ecmcSafetyGroup::to_string(int value) { // Executed by ecmc rt thread. void ecmcSafetyGroup::execute() { - uint64_t data = 1; - - if(!ecEntryRampDown_) { - return; - } - + uint64_t data = 0; + // Read ramp down command from safety plc if(ecEntryRampDown_->readBit(bitRampDown_, &data)) { + // Disable all axes + setAxesEnable(0); // disable + setAxesSafetyInterlocks(0); // stop + setAxesStandstillStatus(0); // set output throw std::out_of_range("Safety: Read rampdown cmd failed"); } - rampDownCmd_ = data > 0; - - if(rampDownCmd_) { - // set safety interlock in ecmc - setSafetyInterlocks(); + rampDownCmdOld_ = rampDownCmd_; + rampDownCmd_ = data == 0; + + if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) { + printf("Safety: Ramp down cmd changed state %d\n",rampDownCmd_); } + // set safety interlock in ecmc + setAxesSafetyInterlocks(rampDownCmd_); + // check if axes are standstill to safety PLC + axesAreStandstill_ = checkAxesStandstill(); + setAxesStandstillStatus(axesAreStandstill_); /* //Needed functions in motion.h int getAxisBusy(int axisIndex, @@ -357,19 +362,53 @@ void ecmcSafetyGroup::execute() { int *value); int setAxisEnable(int axisIndex, int value); - int setAxisClearEmergencyInterlock(int axisIndex); - int setAxisEmergencyStopInterlock(int axisIndex, - double deceleration); */ } +void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) { + + if(ecEntryStandstill_->writeBit(bitStandStill_, + standstill > 0)) { + throw std::out_of_range("Safety: Read rampdown cmd failed"); + } +} + +// Check standstill axes +bool ecmcSafetyGroup::checkAxesStandstill() { + bool standstill = 1; + for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { + standstill= standstill && checkAxisStandstill((*saxis)->axisIndex_,(*saxis)->veloLimit_); + } + return standstill; +} + +// Check standstill axis +bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) { + double velo = 1; + int err = getAxisTrajVelo(axisId, &velo); + if(err) { + return 0; + } + return std::abs(velo) <= veloLimit; +} + // Set safety interlock -void ecmcSafetyGroup::setSafetyInterlocks() { +void ecmcSafetyGroup::setAxesEnable(int enable) { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { if(!*saxis) { throw std::runtime_error( "Safety: Axis object NULL."); } - setAxisEmergencyStopInterlock((*saxis)->axisIndex_,1000); + setAxisEnable((*saxis)->axisIndex_,enable); + } +} + +// Set safety interlock +void ecmcSafetyGroup::setAxesSafetyInterlocks(int stop) { + for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { + if(!*saxis) { + throw std::runtime_error( "Safety: Axis object NULL."); + } + setAxisEmergencyStopInterlock((*saxis)->axisIndex_,stop, 1000); } } diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index bf9f3d7..43ee546 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -64,7 +64,11 @@ class ecmcSafetyGroup : public asynPortDriver { void validateAxes(); void stripBits(); void connectToDataSources(); - void setSafetyInterlocks(); + void setAxesSafetyInterlocks(int stop); + void setAxesEnable(int enable); + void setAxesStandstillStatus(int standstill); + bool checkAxesStandstill(); + bool checkAxisStandstill(int axisId, double veloLimit); void parseConfigStr(const char *configStr); void initAsyn(); double ecmcSampleRateHz_; @@ -72,6 +76,8 @@ class ecmcSafetyGroup : public asynPortDriver { int objectId_; // Unique object id int cycleCounter_; int rampDownCmd_; + int rampDownCmdOld_; + // Config options int cfgDbgMode_; // Config: allow dbg printouts @@ -87,19 +93,22 @@ class ecmcSafetyGroup : public asynPortDriver { char* sEcRampDownCmdNameStrip_; char* sEcAxesStandStillStatStrip_; char* sConfig_; - //ecmcDataItem *dataItemRampDownCmd_; - //ecmcDataItem *dataItemStandStillStat_; + int delayMs_; int masterId_; int slaveIdRampDown_; int bitRampDown_; int slaveIdStandStill_; int bitStandStill_; + int axesAreStandstill_; + char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; + ecmcEc *ecMaster_; ecmcEcEntry *ecEntryRampDown_; ecmcEcEntry *ecEntryStandstill_; + // Some generic utility functions static uint8_t getUint8(uint8_t* data); static int8_t getInt8(uint8_t* data); From b091c6e247b9fc54fdec52c345960c9d83e3b7d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Sandstr=C3=B6m?= Date: Thu, 15 Feb 2024 16:46:47 +0100 Subject: [PATCH 5/5] WIP --- src/ecmcSafetyGroup.cpp | 62 +++++++++++++++++++++++++++++++---------- src/ecmcSafetyGroup.h | 6 ++-- 2 files changed, 51 insertions(+), 17 deletions(-) diff --git a/src/ecmcSafetyGroup.cpp b/src/ecmcSafetyGroup.cpp index 89acd21..d3a12a8 100644 --- a/src/ecmcSafetyGroup.cpp +++ b/src/ecmcSafetyGroup.cpp @@ -80,26 +80,28 @@ ecmcSafetyGroup::ecmcSafetyGroup(const char *name, rampDownCmd_ = 0; axesAreStandstill_ = 0; rampDownCmdOld_ = 0; - + axesAreStandstillOld_ = 0; + printEnableStatus_ = 1; + // Config defaults cfgDbgMode_ = 0; parseConfigStr(cfg_string); initAsyn(); if(cfgDbgMode_) { - printf("Safety: Safety group created:\n" + printf("Safety %s: Safety group created:\n" " Name: %s\n" " I/O for rampdown command from saftey PLC: %s\n" " I/O for axes standstill status: %s\n" " STO delay [ms]: %d\n" " Configuration string: %s\n", - sName_,sEcRampDownCmdNameOrg_,sEcAxesStandStillStatOrg_, - delayMs_,sConfig_); + sName_,sName_,sEcRampDownCmdNameOrg_, + sEcAxesStandStillStatOrg_,delayMs_,sConfig_); } } ecmcSafetyGroup::~ecmcSafetyGroup() { if(cfgDbgMode_) { - printf("Safety: Cleanup\n"); + printf("Safety %s: Cleanup\n",sName_); } free(sName_); @@ -178,24 +180,26 @@ void ecmcSafetyGroup::validateCfgs() { } if(cfgDbgMode_) { - printf("Safety: I/O link parsed:\n" + printf("Safety %s: I/O link parsed:\n" " name: %s\n" " masterid: %d\n" " slaveid: %d\n" " alias: %s\n" " bit: %d\n", + sName_, sEcRampDownCmdNameOrg_, masterIdRampDown, slaveIdRampDown_, aliasRampDown_, bitRampDown_); - printf("Safety: I/O link parsed:\n" + printf("Safety %s: I/O link parsed:\n" " name: %s\n" " masterid: %d\n" " slaveid: %d\n" " alias: %s\n" " bit: %d\n", + sName_, sEcAxesStandStillStatOrg_, masterIdStandStill, slaveIdStandStill_, @@ -217,9 +221,10 @@ void ecmcSafetyGroup::stripBits() { } if(cfgDbgMode_) { - printf("Safety: I/O bit removed:\n" + printf("Safety %s: I/O bit removed:\n" " before: %s\n" " after: %s\n", + sName_, sEcRampDownCmdNameOrg_, sEcRampDownCmdNameStrip_); } @@ -233,9 +238,10 @@ void ecmcSafetyGroup::stripBits() { } if(cfgDbgMode_) { - printf("Safety: I/O bit removed:\n" + printf("Safety %s: I/O bit removed:\n" " before: %s\n" " after: %s\n", + sName_, sEcAxesStandStillStatOrg_, sEcAxesStandStillStatStrip_); } @@ -333,7 +339,7 @@ void ecmcSafetyGroup::execute() { if(ecEntryRampDown_->readBit(bitRampDown_, &data)) { // Disable all axes - setAxesEnable(0); // disable + setAxesDisable(); // disable setAxesSafetyInterlocks(0); // stop setAxesStandstillStatus(0); // set output throw std::out_of_range("Safety: Read rampdown cmd failed"); @@ -342,7 +348,11 @@ void ecmcSafetyGroup::execute() { rampDownCmd_ = data == 0; if(cfgDbgMode_ && rampDownCmdOld_ != rampDownCmd_) { - printf("Safety: Ramp down cmd changed state %d\n",rampDownCmd_); + if(rampDownCmd_) { + printf("Safety %s: Ramp down cmd active\n",sName_); + } else { + printf("Safety %s: Ramp down cmd not active\n",sName_); + } } // set safety interlock in ecmc @@ -350,6 +360,14 @@ void ecmcSafetyGroup::execute() { // check if axes are standstill to safety PLC axesAreStandstill_ = checkAxesStandstill(); setAxesStandstillStatus(axesAreStandstill_); + // Disable + if(axesAreStandstill_ && rampDownCmd_) { + setAxesDisable(); + } else { + printEnableStatus_ = true; + } + + /* //Needed functions in motion.h int getAxisBusy(int axisIndex, @@ -367,10 +385,19 @@ void ecmcSafetyGroup::execute() { void ecmcSafetyGroup::setAxesStandstillStatus(int standstill) { + if(axesAreStandstillOld_ != standstill) { + if(standstill) { + printf("Safety %s: Axes are not moving\n",sName_); + } else { + printf("Safety %s: Axes are moving\n",sName_); + } + } + if(ecEntryStandstill_->writeBit(bitStandStill_, standstill > 0)) { throw std::out_of_range("Safety: Read rampdown cmd failed"); } + axesAreStandstillOld_ = standstill; } // Check standstill axes @@ -393,13 +420,18 @@ bool ecmcSafetyGroup::checkAxisStandstill(int axisId, double veloLimit) { } // Set safety interlock -void ecmcSafetyGroup::setAxesEnable(int enable) { +void ecmcSafetyGroup::setAxesDisable() { for(std::vector::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) { if(!*saxis) { - throw std::runtime_error( "Safety: Axis object NULL."); + throw std::runtime_error("Safety: Axis object NULL."); } - setAxisEnable((*saxis)->axisIndex_,enable); + setAxisEnable((*saxis)->axisIndex_,0); } + + if(printEnableStatus_ && cfgDbgMode_) { + printf("Safety %s: Axes are beeing disabled\n",sName_); + } + printEnableStatus_ = 0; } // Set safety interlock @@ -432,7 +464,7 @@ void ecmcSafetyGroup::addAxis(int axisId, double veloLimit,int standStillTimeMs) axesCounter_++; if(cfgDbgMode_) { - printf("Safety: Added axis %d to safety group \"%s\"\n",axisId,sName_); + printf("Safety %s: Added axis %d to safety group\n",sName_,axisId); } return; diff --git a/src/ecmcSafetyGroup.h b/src/ecmcSafetyGroup.h index 43ee546..c51bc59 100644 --- a/src/ecmcSafetyGroup.h +++ b/src/ecmcSafetyGroup.h @@ -65,7 +65,7 @@ class ecmcSafetyGroup : public asynPortDriver { void stripBits(); void connectToDataSources(); void setAxesSafetyInterlocks(int stop); - void setAxesEnable(int enable); + void setAxesDisable(); void setAxesStandstillStatus(int standstill); bool checkAxesStandstill(); bool checkAxisStandstill(int axisId, double veloLimit); @@ -101,7 +101,9 @@ class ecmcSafetyGroup : public asynPortDriver { int slaveIdStandStill_; int bitStandStill_; int axesAreStandstill_; - + int axesAreStandstillOld_; + int printEnableStatus_; + char aliasRampDown_[EC_MAX_OBJECT_PATH_CHAR_LENGTH]; char aliasStandStill_[EC_MAX_OBJECT_PATH_CHAR_LENGTH];