motion/ecmc_plugin_safety
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added and tested velo limitation

Browse Source
This commit is contained in:
sandst_a
2024-03-13 08:55:22 +01:00
parent d6f04bfa4d
commit ad8dd2cc36
4 changed files with 52 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
GNUmakefile
View File

@@ -17,7 +17,7 @@ OPT_CXXFLAGS_YES = -O3
# dependencies
ECmasterECMC_VERSION = v1.1.0
# ecmc_VERSION = 9.2.0
ecmc_VERSION = test
ecmc_VERSION = safety3
################################################################################
# THIS RELATES TO THE EtherCAT MASTER LIBRARY

22
README.md
View File

@@ -10,14 +10,22 @@ This plugin is designed for interfacing safety systems, see example in below pic
# SS1-t
The plugin supports stopping axes according to a concept described as SS1-t, see below picture. In SS1-t the STO (or removal of power) from the axes are delayed for a certain defined time allowing a controlled rampdown of velocity. The rampdown of the axis is handled by the non-safe motion controller while the removal of power (or triggering of STO) after the time delay is handled by a safety system, . This makes it possible to stop moving axes in a controlled way and disable the drives before the power is interrupted (or STO triggered). This will result in a safer system and less harware failures and error messages.
After the time delay expires, hopfully all axes are at standstill and power is already removed from the drives.
After the time delay expires, hopfully all axes are at standstill and power is already removed from the drives.
![SS1-t](docs/SS1-t.png)
## Velocity limit
An additional optional feature of this plugin is limiting of axis velocity based on an the state of an ethercat I/O (this is not a safety certified fucntionality).
Axes that are configured with a max velocity will be monitored by this plugin and if exceeding the configured max velocity, the axis will be disabled.
For initiation of new movements, the target velocity will be limited to 95% of the configured maximum velocity. The reason for this is toi allow small fluctuations in actual velocity without disablaing the axes.
## Interface
Basically the safey system is interfaced with two binary signals (ethercat I/O):
* Ramp down command (from safety system to ecmc)
* Axis stand still status (to safety system from ecmc)
Basically the safey system is interfaced with three binary signals (ethercat I/O):
* Ramp down command (from safety system to ecmc). 0 means ramp down command is active.
* Axis stand still status (to safety system from ecmc). 1 means that all axes are at rest
* Limitation of velocity (optional). 0 means limitation of velocity is acrive
If, for instance, an safety event is triggerd by the safety system, it will immediately command this plugin to rampdown velocity of all axes (that a configured to stop). When all axes, that are configured to rampdown, have stopped then this plugin will disable the axes and set an ethercat output informing the safety system that the axes are standstill. After a certain timout the safety system will make sure power is removed from the motion axes by triggering an STO or removing power. The removal of power or triggering of STO will made regardless if the axes are at rest or not. A reset of the safety system, allowing power to the drives, will only be possible once the safety system gets a confirmation that all axes are at rest.
@@ -43,7 +51,7 @@ This command is also wrapped in a snippet addSS1Group.cmd with the follwoing par
* NAME : Name of group
* EC_RAMP_DOWN : Ethercat input of ramp down signal from safety PLC
* EC_AXES_REST : Ethercat entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system)
* EC_AXES_MAX_VELO : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system)
* EC_AXES_MAX_VELO : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system), put 0 to disable
* DELAY_MS : Safety system delay time of STO or removal of power
Example:
@@ -96,13 +104,13 @@ require ecmc_plugin_safety sandst_a
# Create SS1 group
#- EC_RAMP_DOWN : Ethercat entry for ramp down command, input to ecmc (command from safety PLC/system)
#- EC_AXES_REST : Ethercat entry for signaling that all axes in group are at rest, output from ecmc (feedback to safety PLC/system)
#- EC_AXES_MAX_VELO : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system)
#- EC_AXES_MAX_VELO : Ethercat entry for reducing velocity, input to ecmc (command from safety PLC/system), set to "empty" to disable
#- DELAY_MS : Time between rampdown command and STO
epicsEnvSet(EC_RAMP_DOWN,"ec${ECMC_EC_MASTER_ID}.s${BI_SLAVE}.binaryInput08.0")
epicsEnvSet(EC_AXES_REST,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
epicsEnvSet(EC_AXES_MAX_VELO,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.ONE.0")
epicsEnvSet(SAFETY_TIMEOUT,500)
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_REST=${EC_AXES_REST},${EC_AXES_MAX_VELO},DELAY_MS=${SAFETY_TIMEOUT}"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_REST=${EC_AXES_REST},EC_AXES_MAX_VELO=${EC_AXES_MAX_VELO},DELAY_MS=${SAFETY_TIMEOUT}"
#- Add axis
#- AX_ID : Axis ID

88
src/ecmcSS1SafetyGroup.cpp
View File

@@ -35,12 +35,12 @@ extern DBBASE *pdbbase;
* - runtime_error
*/
ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
const char *ec_limit_velo,
int time_delay_ms,
const char *cfg_string,
char* portName)
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
const char *ec_limit_velo,
int time_delay_ms,
const char *cfg_string,
char* portName)
: asynPortDriver(portName,
1, /* maxAddr */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
@@ -103,11 +103,12 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
" Name: %s\n"
" I/O for rampdown command from saftey PLC: %s\n"
" I/O for axes standstill status: %s\n"
" I/O for velo limit command from safety PLC: %s\n"
" I/O for velo limit command from safety PLC: %s (%s)\n"
" STO delay [ms]: %d\n"
" Configuration string: %s\n",
sName_,sName_,sEcRampDownCmdNameOrg_,
sEcAxesStandStillStatOrg_,sEcLimitVeloOrg_,
limitMaxVeloEnable_ ? "enabled" : "disabled",
delayMs_,sConfig_);
}
}
@@ -177,14 +178,17 @@ void ecmcSS1SafetyGroup::validateCfgs() {
&bitStandStill_)) {
throw std::runtime_error( "Safety: Parse error: Data source for standstill status.");
}
int masterIdLimitVelo=-1;
if(parseEcPath(sEcLimitVeloOrg_,
&masterIdLimitVelo,
&slaveIdLimitVelo_,
aliasLimitVelo_,
&bitLimitVelo_)) {
throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
// init to masterIdRampDown if case !limitMaxVeloEnable_
int masterIdLimitVelo = masterIdStandStill;
if(limitMaxVeloEnable_) {
if(parseEcPath(sEcLimitVeloOrg_,
&masterIdLimitVelo,
&slaveIdLimitVelo_,
aliasLimitVelo_,
&bitLimitVelo_)) {
throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
}
}
if(masterIdStandStill != masterIdRampDown || masterIdLimitVelo != masterIdRampDown) {
@@ -298,17 +302,18 @@ void ecmcSS1SafetyGroup::connectToDataSources() {
};
// standstill
slave = ecMaster_->findSlave(slaveIdLimitVelo_);
if(!slave) {
throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL.");
// Limit velo
if( limitMaxVeloEnable_ ) {
slave = ecMaster_->findSlave(slaveIdLimitVelo_);
if(!slave) {
throw std::runtime_error( "Safety: EtherCAT slave limit velo I/O NULL.");
}
ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_);
if(!ecEntryLimitVelo_) {
throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL.");
};
}
ecEntryLimitVelo_ = slave->findEntry(aliasLimitVelo_);
if(!ecEntryLimitVelo_) {
throw std::runtime_error( "Safety: EtherCAT entry for limit velo I/O NULL.");
};
dataSourcesLinked_ = 1;
return;
@@ -593,28 +598,6 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
}
}
void ecmcSS1SafetyGroup::addAxis(int axisId,
double veloStandstillLimit,
int standStillTimeMs) {
if(!getAxisValid(axisId)) {
throw std::out_of_range("Safety: Invalid axis id");
}
axes_.push_back(new safetyAxis(axisId, veloStandstillLimit,
standStillTimeMs));
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety %s: Added axis %d to safety group.\n"
" velo stand still limit = %lf,\n"
" standstill filter time = %d\n"
,sName_,axisId,veloStandstillLimit,standStillTimeMs);
}
return;
}
void ecmcSS1SafetyGroup::addAxis(int axisId,
double veloStandstillLimit,
int standStillTimeMs,
@@ -627,13 +610,14 @@ void ecmcSS1SafetyGroup::addAxis(int axisId,
axes_.push_back(new safetyAxis(axisId, veloStandstillLimit,
standStillTimeMs, veloMaxLimit));
axesCounter_++;
if(cfgDbgMode_) {
printf("Safety %s: Added axis %d to safety group.\n"
" velo stand still limit = %lf,\n"
" velo max limit = %lf,\n"
" standstill filter time = %d\n"
,sName_,axisId,veloStandstillLimit,veloMaxLimit,standStillTimeMs);
" Velo stand still limit: %lf\n"
" velo max limit: %lf (%s)\n"
" standstill filter time: %d\n"
,sName_,axisId,veloStandstillLimit,veloMaxLimit,
veloMaxLimit>0 ? "enabled" : "disabled", standStillTimeMs);
}
return;

3
src/ecmcSS1SafetyGroup.h
View File

@@ -84,9 +84,6 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
// Call just before realtime because then all data sources should be available
void validate();
void addAxis(int axisId,
double veloLimit,
int standStillTimeMs);
void addAxis(int axisId,
double veloLimit,
int standStillTimeMs,