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

Add max velo feature

Browse Source
This commit is contained in:
sandst_a
2024-03-12 16:22:13 +01:00
parent 3272fd62ed
commit de10cade0f
9 changed files with 306 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@@ -2,6 +2,7 @@
"files.associations": {
"string_view": "cpp",
"stdexcept": "cpp",
"iosfwd": "cpp"
"iosfwd": "cpp",
"typeinfo": "cpp"
}
}

11
Db/ss1.template
View File

@@ -51,4 +51,15 @@ record(bi,"${P}SS1-${NAME}-AxsStndStllAct"){
field(ONAM, "Standstill")
field(ZSV, "NO_ALARM")
field(OSV, "NO_ALARM")
field(FLNK, "${P}SS1-${NAME}-RedVeloAct.PROC")
}
# // bit 3 reduce velo active
record(bi,"${P}SS1-${NAME}-RedVeloAct"){
field(DESC, "SS1-${NAME}: Reduce velo active")
field(INP, "${P}SS1-${NAME}-Stat_.B3")
field(ZNAM, "Not Active")
field(ONAM, "Active")
field(ZSV, "NO_ALARM")
field(OSV, "NO_ALARM")
}

3
GNUmakefile
View File

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

56
README.md
View File

@@ -29,11 +29,12 @@ The configuration is made by two commands:
The ecmcAddSS1SafetyGroup() adds a SS1 safety group. The command takes the following parameters:
```
ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
<name> : Name of group.
<ec_rampdown> : Ethercat entry input for rampdown cmd.
<ec_axes_at_rest> : Ethercat entry output for group standstill status.
<time_delay_ms> : Time delay of STO [ms].
ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<time_delay_ms>)
<name> : Name of group
<ec_rampdown_cmd> : Ethercat entry input for rampdown cmd
<ec_standstill_status> : Ethercat entry output for group standstill status
<ec_max_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable)
<time_delay_ms> : Time delay of STO [ms]
```
Each group is interfacing the safety system through the same I/O (ethercat).
Axes that needs the SS1 fucntionality can be added to this group with the ecmcAddAxisToSafetyGroup() command.
@@ -41,19 +42,21 @@ Axes that needs the SS1 fucntionality can be added to this group with the ecmcAd
This command is also wrapped in a snippet addSS1Group.cmd with the follwoing parameters:
* NAME : Name of group
* EC_RAMP_DOWN : Ethercat input of ramp down signal from safety PLC
* EC_AXES_STANDSTILL : Ethercat output for signaling safetysystem that all axes are at rest.
* 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)
* DELAY_MS : Safety system delay time of STO or removal of power
Example:
```
# Create SS1 group
epicsEnvSet(EC_RAMP_DOWN,"ec${ECMC_EC_MASTER_ID}.s${BI_SLAVE}.binaryInput08.0")
epicsEnvSet(EC_AXES_STANDSTILL,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
epicsEnvSet(EC_AXES_REST,"ec${ECMC_EC_MASTER_ID}.s${BO_SLAVE}.binaryOutput07.0")
epicsEnvSet(SAFETY_TIMEOUT,500)
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN=${EC_RAMP_DOWN},EC_AXES_STANDSTILL=${EC_AXES_STANDSTILL},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},DELAY_MS=${SAFETY_TIMEOUT}"
```
## ecmcAddAxisToSafetyGroup()
With the "ecmcAddAxisToSafetyGroup()" command an ecmc axis can be added to a safety group.
All axes linked to a certain group will act on command from the safety system (initiation of velocity rampdown).
@@ -62,7 +65,8 @@ The ecmcAddAxisToSafetyGroup() command takes the following parameters:
ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>, <velo_limit>, <filter_time>)
<name> : Name of safety group to add axis to.
<Axis id> : Axis index to add (ecmc axis index).
<velo limit> : Axis standstill velo limit [unit of axis].
<velo_rest_limit> : Axis at rest velo limit [unit of axis].
<velo_max_limit> : Axis max velo limit [unit of axis].
<filter_time> : NOT USED (for future implemenation). Time for axis to be below velo limit [ms].
```
Note: The "filter_time" parameter is not used right now. As soon as the axis is below teh velo_limit it will be considered to stand still and will then be disabled.
@@ -70,13 +74,14 @@ Note: The "filter_time" parameter is not used right now. As soon as the axis is
Note: The plugin checks the trajectory generated velocity setpoint and not the actual velocity.
This command is also wrapped in a snippet addAxisToSafetyGroup.cmd with the following parameters:
* NAME : Name of group to add axis to (group must be created first with addSS1Group.cmd)
* AX_ID : ecmc axis index of axis to add
* VELO_LIM : Velocity limit [EGU of axis], default 0. If setpoint and actual velocity is lower or equal than VELO_LIM the axis is considered to be at rest
* NAME : Name of group to add axis to (group must be created first with addSS1Group.cmd)
* AX_ID : ecmc axis index of axis to add
* VELO_REST_LIM : Velocity at rest limit [unit same as EGU of axis]
* VELO_MAX_LIM : Velocity maximum limit, -1 to disable [unit same as EGU of axis]
Example:
```
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_REST_LIM=1,VELO_MAX_LIM=100"
```
# Example of startup script:
@@ -84,17 +89,26 @@ ${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_I
##############################################################################
## Load safety plugin
#
require ecmc_plugin_safety sandst_a
# Create SS1 group
epicsEnvSet(RAMP_DOWN_CMD,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.0")
epicsEnvSet(STANDSTILL_STAT,"ec${ECMC_EC_MASTER_ID}.s${DRV_SLAVE}.ZERO.1")
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addSS1Group.cmd "NAME=first,EC_RAMP_DOWN_CMD=${RAMP_DOWN_CMD},EC_STANDSTILL_STAT=${STANDSTILL_STAT},DELAY_MS=500"
# Add axes to group "first"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=1,VELO_LIM=1"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=2,VELO_LIM=1"
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd.cmd "NAME=first,AX_ID=3,VELO_LIM=1"
# 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)
#- 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}"
#- Add axis
#- AX_ID : Axis ID
#- VELO_REST_LIM : Velocity at rest limit [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit, -1 to disable [unit same as EGU of axis]
${SCRIPTEXEC} ${ecmc_plugin_safety_DIR}addAxisToSafetyGroup.cmd "NAME=first,AX_ID=1,VELO_REST_LIM=1,VELO_MAX_LIM=100"
....
```

8
scripts/addAxisToSafetyGroup.cmd
View File

@@ -17,14 +17,16 @@
#- Arguments:
#- NAME : Name of safety group
#- AX_ID : Axis ID
#- VELO_LIM : Velocity standstill limit [unit same as EGU of axis]
#- VELO_REST_LIM : Velocity at rest limit [unit same as EGU of axis]
#- VELO_MAX_LIM : Velocity maximum limit, -1 to disable [unit same as EGU of axis]
#-
#################################################################################
#- ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>, <velo_limit>, <filter_time>)
#- <name> : Name of safety group to add axis to.
#- <Axis id> : Axis index to add (ecmc axis index).
#- <velo limit> : Axis standstill velo limit [unit of axis].
#- <velo_rest_limit> : Axis at rest velo limit [unit of axis].
#- <velo_max_limit> : Axis max velo limit [unit of axis].
#- <filter_time> : NOT USED (for future implemenation). Time for axis to be below velo limit [ms].
ecmcAddAxisToSafetyGroup("${NAME}",${AX_ID},${VELO_LIM=0},0)
ecmcAddAxisToSafetyGroup("${NAME}",${AX_ID},${VELO_REST_LIM=0},0,${VELO_MAX_LIM=0.0})

19
scripts/addSS1Group.cmd
View File

@@ -16,18 +16,21 @@
#-
#- Arguments:
#- NAME : Name of safety group
#- EC_RAMP_DOWN : Ethercat entry for rampd down command, input to ecmc (command from safety PLC/system)
#- EC_AXES_STANDSTILL : Ethercat entry for all axes in group at standstill, output from ecmc (feedback to safety PLC/system)
#- 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)
#- DELAY_MS : Time between rampdown command and STO
#-
#################################################################################
#- ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
#- <name> : Name of group.
#- <ec_rampdown> : Ethercat entry input for rampdown cmd.
#- <ec_standstill> : Ethercat entry output for group standstill status.
#- <time_delay_ms> : Time delay of STO [ms].
ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN}","${EC_AXES_STANDSTILL}",${DELAY_MS=0})
#-Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)
#- <name> : Name of group
#- <ec_rampdown_cmd> : Ethercat entry input for rampdown cmd
#- <ec_standstill_status> : Ethercat entry output for group standstill status
#- <ec_max_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to "empty" to disable)
#- <time_delay_ms> : Time delay of STO [ms]
ecmcAddSS1SafetyGroup("${NAME}","${EC_RAMP_DOWN}","${EC_AXES_REST}","${EC_AXES_MAX_VELO=empty}",${DELAY_MS=0})
#- Load SS1 group records
dbLoadRecords("ss1.template","P=${ECMC_PREFIX},NAME=${NAME}")

205
src/ecmcSS1SafetyGroup.cpp
View File

@@ -56,56 +56,59 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
0, /* Default priority */
0) /* Default stack size */
{
sName_ = strdup(name);
sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd);
sEcRampDownCmdNameStrip_ = strdup(ec_rampdown_cmd);
sEcAxesStandStillStatOrg_ = strdup(ec_standstill_status);
sEcAxesStandStillStatStrip_ = strdup(ec_standstill_status);
sEcReduceVeloOrg_ = strdup(ec_limit_velo);
sEcReduceVeloStrip_ = strdup(ec_limit_velo);
sConfig_ = strdup(cfg_string);
delayMs_ = time_delay_ms;
sName_ = strdup(name);
sEcRampDownCmdNameOrg_ = strdup(ec_rampdown_cmd);
sEcAxesStandStillStatOrg_ = strdup(ec_standstill_status);
sEcLimitVeloOrg_ = strdup(ec_limit_velo);
limitMaxVeloEnable_ = 1;
if(strcmp(sEcLimitVeloOrg_, "empty") == 0) {
limitMaxVeloEnable_ = 0;
}
sConfig_ = strdup(cfg_string);
delayMs_ = time_delay_ms;
ptrStatus_ = (int*)&status_;
asynStatusId_ = -1;
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
ecMaster_ = NULL;
ecEntryRampDown_ = NULL;
ecEntryStandstill_ = NULL;
ecEntryLimitVelo_ = NULL;
masterId_ = 0;
slaveIdRampDown_ = 0;
bitRampDown_ = 0;
slaveIdStandStill_ = 0;
bitStandStill_ = 0;
aliasRampDown_[0] = 0;
aliasStandStill_[0] = 0;
aliasLimitVelo_[0] = 0;
rampDownCmd_ = 0;
axesAreStandstill_ = 0;
rampDownCmdOld_ = 0;
axesAreStandstillOld_ = 0;
printEnableStatus_ = 1;
limitVeloCmdOld_ = 0;
limitVeloCmd_ = 0;
cfgDbgMode_ = 0;
memset(&status_,0,sizeof(status_));
ptrStatus_ = (int*)&status_;
asynStatusId_ = -1;
axesCounter_ = 0;
ecmcSampleRateHz_ = getEcmcSampleRate();
dataSourcesLinked_ = 0;
ecMaster_ = NULL;
ecEntryRampDown_ = NULL;
ecEntryStandstill_ = NULL;
ecEntryLimitVelo_ = NULL;
masterId_ = 0;
slaveIdRampDown_ = 0;
bitRampDown_ = 0;
slaveIdStandStill_ = 0;
bitStandStill_ = 0;
aliasRampDown_[0] = 0;
aliasStandStill_[0] = 0;
aliasLimitVelo_[0] = 0;
rampDownCmd_ = 0;
axesAreStandstill_ = 0;
rampDownCmdOld_ = 0;
axesAreStandstillOld_ = 0;
printEnableStatus_ = 1;
limitVeloCmdOld_ = 0;
limitVeloCmd_ = 0;
// Config defaults
cfgDbgMode_ = 0;
parseConfigStr(cfg_string);
initAsyn();
if(cfgDbgMode_) {
printf("Safety %s: Safety group created:\n"
" Type: SS1\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",
printf("Safety %s: Group created:\n"
" Type: SS1-t\n"
" 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"
" STO delay [ms]: %d\n"
" Configuration string: %s\n",
sName_,sName_,sEcRampDownCmdNameOrg_,
sEcAxesStandStillStatOrg_,delayMs_,sConfig_);
sEcAxesStandStillStatOrg_,sEcLimitVeloOrg_,
delayMs_,sConfig_);
}
}
@@ -117,11 +120,8 @@ ecmcSS1SafetyGroup::~ecmcSS1SafetyGroup() {
free(sName_);
free(sEcRampDownCmdNameOrg_);
free(sEcAxesStandStillStatOrg_);
free(sEcRampDownCmdNameStrip_);
free(sEcAxesStandStillStatStrip_);
free(sConfig_);
free(sEcReduceVeloOrg_);
free(sEcReduceVeloStrip_);
free(sEcLimitVeloOrg_);
}
void ecmcSS1SafetyGroup::parseConfigStr(const char *configStr) {
@@ -179,7 +179,7 @@ void ecmcSS1SafetyGroup::validateCfgs() {
}
int masterIdLimitVelo=-1;
if(parseEcPath(sEcReduceVeloOrg_,
if(parseEcPath(sEcLimitVeloOrg_,
&masterIdLimitVelo,
&slaveIdLimitVelo_,
aliasLimitVelo_,
@@ -187,7 +187,7 @@ void ecmcSS1SafetyGroup::validateCfgs() {
throw std::runtime_error( "Safety: Parse error: Data source for limit velo.");
}
if(masterIdStandStill != masterIdRampDown || sEcReduceVeloOrg_ != masterIdRampDown) {
if(masterIdStandStill != masterIdRampDown || masterIdLimitVelo != masterIdRampDown) {
throw std::runtime_error( "Safety: Parse error: Master id for datasources different.");
}
masterId_ = masterIdStandStill;
@@ -249,6 +249,22 @@ void ecmcSS1SafetyGroup::validateAxes() {
printf("Safety %s: Error, group empty (axis count zero)\n",sName_);
throw std::runtime_error( "Safety: Error, empty group not allowed.");
}
// Check if max velo enabled in axis
int axesMaxVeloEnabled = 0;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
axesMaxVeloEnabled = (*saxis)->veloMaxLimitEnabled_ || axesMaxVeloEnabled;
}
if(limitMaxVeloEnable_ && !axesMaxVeloEnabled) {
printf("Safety %s: Error, Limit max velo enabled but no axis configured with max velo\n",sName_);
throw std::runtime_error( "Safety: Error, Limit max velo enabled but no axis configured with max velo");
}
if(!limitMaxVeloEnable_ && axesMaxVeloEnabled) {
printf("Safety %s: Error, Limit max velo disabled but axis configured with a max velo\n",sName_);
throw std::runtime_error( "Safety: Error, Limit max velo disabled but axis configured with a max velo");
}
}
void ecmcSS1SafetyGroup::connectToDataSources() {
@@ -384,6 +400,11 @@ void ecmcSS1SafetyGroup::exeRampDown() {
// Limit velo if needed
void ecmcSS1SafetyGroup::exeLimitVelo() {
if(!limitMaxVeloEnable_) {
return;
}
uint64_t data = 0;
// Read ramp down command from safety plc
if(ecEntryLimitVelo_->readBit(bitLimitVelo_,
@@ -405,15 +426,20 @@ void ecmcSS1SafetyGroup::exeLimitVelo() {
resetPrintoutStatus();
if(cfgDbgMode_) {
if(rampDownCmd_) {
if(limitVeloCmd_) {
printf("Safety %s: Limit velo cmd active\n",sName_);
} else {
printf("Safety %s: Limit velo cmd not active\n",sName_);
}
}
}
if(limitVeloCmd_) {
checkAxesMaxVeloAndDisableIfNeeded();
}
if()
// Write velo limit and activation to ecmc axis object
setAxesMaxVelo();
}
// Executed by ecmc rt thread.
@@ -453,11 +479,55 @@ void ecmcSS1SafetyGroup::setAxesStandstillStatus(int standstill) {
bool ecmcSS1SafetyGroup::checkAxesStandstill() {
bool standstill = 1;
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
standstill= standstill && checkAxisStandstill((*saxis));
standstill= standstill && checkAxisStandstill((*saxis));
}
return standstill;
}
// Check max velo violation
void ecmcSS1SafetyGroup::checkAxesMaxVeloAndDisableIfNeeded() {
if(!limitVeloCmd_) {
return;
}
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if(!(*saxis)->veloMaxLimitEnabled_) {
break;
}
if(checkAxisMaxVelo((*saxis))) {
printf("Safety %s: Axis %d, velo too high, disabling axis.\n", sName_, (*saxis)->axisId_);
setAxisEnable((*saxis)->axisId_,0);
}
}
return;
}
// Set max velo in axis object
void ecmcSS1SafetyGroup::setAxesMaxVelo() {
for(std::vector<safetyAxis*>::iterator saxis = axes_.begin(); saxis != axes_.end(); ++saxis) {
if( (*saxis)->veloMaxLimitEnabled_ ) {
setAxisExtMaxVelo((*saxis)->axisId_,0.95*(*saxis)->veloMaxLimit_,limitVeloCmd_);
}
}
}
bool ecmcSS1SafetyGroup::checkAxisMaxVelo(safetyAxis* axis) {
double traj = 1;
int err = getAxisTrajVelo(axis->axisId_, &traj);
if(err) {
return 0;
}
double enc = 1;
err = getAxisEncVelo(axis->axisId_, &enc);
if(err) {
return 0;
}
return std::abs(traj) >= axis->veloMaxLimit_ || std::abs(enc) >= axis->veloMaxLimit_;
}
// Check standstill axis
bool ecmcSS1SafetyGroup::checkAxisStandstill(safetyAxis* axis) {
double traj = 1;
@@ -504,7 +574,7 @@ void ecmcSS1SafetyGroup::setAxesDisable() {
if(!*saxis) {
throw std::runtime_error("Safety: Axis object NULL.");
}
setAxisEnable((*saxis)->axisId_,0);
setAxisEnable((*saxis)->axisId_,0);
}
if(printEnableStatus_ && cfgDbgMode_) {
@@ -523,7 +593,32 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
}
}
void ecmcSS1SafetyGroup::addAxis(int axisId, double veloStandstillLimit,int standStillTimeMs,double veloMaxLimit) {
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,
double veloMaxLimit) {
if(!getAxisValid(axisId)) {
throw std::out_of_range("Safety: Invalid axis id");

46
src/ecmcSS1SafetyGroup.h
View File

@@ -39,6 +39,18 @@ class safetyAxis {
axisId_ = axisId;
standStillTimeMs_ = standStillTimeMs;
veloMaxLimit_ = veloMaxLimit;
veloMaxLimitEnabled_ = veloMaxLimit > 0;
printEnableStat_ = 1;
}
safetyAxis(int axisId,
double velostandstillLimit,
int standStillTimeMs) {
veloStandstillLimit_ = velostandstillLimit;
axisId_ = axisId;
standStillTimeMs_ = standStillTimeMs;
veloMaxLimit_ = -1;
veloMaxLimitEnabled_ = 0;
printEnableStat_ = 1;
}
@@ -47,8 +59,11 @@ class safetyAxis {
int axisId_;
int standStillTimeMs_;
int printEnableStat_;
int veloMaxLimitEnabled_;
};
class ecmcSS1SafetyGroup : public asynPortDriver {
public:
@@ -58,18 +73,24 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
* - out_of_range
*/
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);
~ecmcSS1SafetyGroup();
// 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);
void addAxis(int axisId,
double veloLimit,
int standStillTimeMs,
double veloMaxLimit);
void execute();
virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
std::string getName();
@@ -85,11 +106,13 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
bool checkAxisStandstill(safetyAxis* axis);
bool checkAxisStandstillAndDisableIfNeeded(safetyAxis* axis);
bool checkAxesStandstillAndDisableIfNeeded();
void checkAxesMaxVeloAndDisableIfNeeded();
void setAxesMaxVelo();
bool checkAxisMaxVelo(safetyAxis* axis);
void resetPrintoutStatus();
void parseConfigStr(const char *configStr);
void initAsyn();
void refreshAsyn();
void addMaxVeloLimit(const char *ec_rampdown_cmd);
void exeRampDown();
void exeLimitVelo();
double ecmcSampleRateHz_;
@@ -115,10 +138,7 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
char* sName_;
char* sEcRampDownCmdNameOrg_;
char* sEcAxesStandStillStatOrg_;
char* sEcRampDownCmdNameStrip_;
char* sEcAxesStandStillStatStrip_;
char* sEcReduceVeloOrg_;
char* sEcReduceVeloStrip_;
char* sEcLimitVeloOrg_;
char* sConfig_;
int delayMs_;
@@ -130,6 +150,8 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
int slaveIdLimitVelo_;
int bitLimitVelo_;
int limitMaxVeloEnable_;
int axesAreStandstill_;
int axesAreStandstillOld_;
int printEnableStatus_;

77
src/ecmcSafetyPlgWrap.cpp
View File

@@ -51,6 +51,7 @@ ecmcSS1SafetyGroup* getGroupFromName(const char *name) {
int createSafetyGroup(const char *name,
const char *ec_rampdown_cmd,
const char *ec_standstill_status,
const char *ec_max_velo_cmd,
int time_delay_ms) {
// ensure group does not already exist
@@ -71,6 +72,7 @@ int createSafetyGroup(const char *name,
safetyGroup = new ecmcSS1SafetyGroup(name,
ec_rampdown_cmd,
ec_standstill_status,
ec_max_velo_cmd,
time_delay_ms,
configString,
portNameBuffer);
@@ -90,7 +92,8 @@ int createSafetyGroup(const char *name,
int addAxisToSafetyGroup(const char *groupName,
int axisId,
double veloLimit,
int standStillTimeMs) {
int standStillTimeMs,
double maxVeloLimit) {
ecmcSS1SafetyGroup* grp = getGroupFromName(groupName);
@@ -99,7 +102,7 @@ int addAxisToSafetyGroup(const char *groupName,
throw std::runtime_error( "Safety: Error, group not found.");
}
grp->addAxis(axisId,veloLimit,standStillTimeMs);
grp->addAxis(axisId,veloLimit,standStillTimeMs,maxVeloLimit);
return asynSuccess;
}
@@ -160,15 +163,20 @@ int executeSafetyGroups() {
*/
void ecmcAddSS1SafetyGroupPrintHelp() {
printf("\n");
printf(" Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<time_delay_ms>)\n");
printf(" Use ecmcAddSS1SafetyGroup(<name>, <ec_rampdown_cmd>, <ec_standstill_status>,<ec_max_velo_cmd>,<time_delay_ms>)\n");
printf(" <name> : Name of group.\n");
printf(" <ec_rampdown_cmd> : Ethercat entry input for rampdown cmd.\n");
printf(" <ec_standstill_status> : Ethercat entry output for group standstill status.\n");
printf(" <ec_max_velo_cmd> : Ethercat entry input for activation of maximum velo limitation (set to \"empty\" to disable).\n");
printf(" <time_delay_ms> : Time delay of STO [ms].\n");
printf("\n");
}
int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const char* ec_standstill_status,int time_delay_ms) {
int ecmcAddSS1SafetyGroup(const char* name,
const char* ec_rampdown_cmd,
const char* ec_standstill_status,
const char* ec_max_velo_cmd,
int time_delay_ms) {
if(!name) {
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
@@ -190,13 +198,20 @@ int ecmcAddSS1SafetyGroup(const char* name, const char* ec_rampdown_cmd,const ch
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
}
if(!ec_max_velo_cmd) {
printf("Error: ec_max_velo_cmd ethercat entry not defined.\n");
ecmcAddSS1SafetyGroupPrintHelp();
return asynError;
}
if(time_delay_ms <= 0) {
printf("Error: time_delay invalid.\n");
exit (EXIT_FAILURE);
}
try {
createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status, time_delay_ms);
createSafetyGroup(name,ec_rampdown_cmd,ec_standstill_status,ec_max_velo_cmd,time_delay_ms);
}
catch(std::exception& e) {
printf("Exception: %s. Add safety group failed.\n",e.what());
@@ -213,16 +228,19 @@ static const iocshArg initArg1_1 =
static const iocshArg initArg2_1 =
{ "ec entry output axes standstill status", iocshArgString };
static const iocshArg initArg3_1 =
{ "ec entry input activate max velo", iocshArgString };
static const iocshArg initArg4_1 =
{ "STO delay [ms]", iocshArgInt };
static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg1_1,
&initArg2_1,
&initArg3_1};
&initArg3_1,
&initArg4_1,};
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 4, initArgs_1 };
static const iocshFuncDef initFuncDef_1 = { "ecmcAddSS1SafetyGroup", 5, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].ival);
ecmcAddSS1SafetyGroup(args[0].sval, args[1].sval, args[2].sval, args[3].sval, args[4].ival);
}
/**
@@ -231,14 +249,19 @@ static void initCallFunc_1(const iocshArgBuf *args) {
void ecmcAddAxisToSafetyGroupPrintHelp() {
printf("\n");
printf(" Use ecmcAddAxisToSafetyGroup(<group_name>, <axis_index>)\n");
printf(" <name> : Name of safety group.\n");
printf(" <Axis id> : Axis index to add.\n");
printf(" <velo limit> : Axis standstill velo limit [unit of axis].\n");
printf(" <time> : Time for axis to be below velo limit [ms].\n");
printf(" <name> : Name of safety group.\n");
printf(" <Axis id> : Axis index to add.\n");
printf(" <standstill_velo_limit> : Axis standstill velo limit [unit of axis].\n");
printf(" <standstill_time> : Time for axis to be below standstill_velo_limit [ms].\n");
printf(" <max_velo_limit> : Axis max velo limit [unit of axis] (0 to disable).\n");
printf("\n");
}
int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int stand_still_time) {
int ecmcAddAxisToSafetyGroup(const char* name,
int axis_id,
double standstill_velo_limit,
int stand_still_time,
double max_velo_limit) {
if(!name) {
ecmcAddAxisToSafetyGroupPrintHelp();
return asynError;
@@ -254,17 +277,26 @@ int ecmcAddAxisToSafetyGroup(const char* name, int axis_id, double velo_lim, int
exit(EXIT_FAILURE);
}
if(velo_lim < 0) {
printf("Error: Invalid velocity limit.\n");
if(standstill_velo_limit < 0) {
printf("Error: Invalid standstill velocity limit.\n");
exit(EXIT_FAILURE);
}
if(stand_still_time < 0) {
printf("Error: Invalid stand still filter time.\n");
printf("Error: Invalid standstill filter time.\n");
exit(EXIT_FAILURE);
}
if(max_velo_limit < 0) {
printf("Error: Invalid maximum velocity limit.\n");
exit(EXIT_FAILURE);
}
try {
return addAxisToSafetyGroup(name,axis_id, velo_lim, stand_still_time);
return addAxisToSafetyGroup(name,axis_id,
standstill_velo_limit,
stand_still_time,
max_velo_limit);
}
catch(std::exception& e) {
printf("Exception: %s. Add axis to safety group failed.\n",e.what());
@@ -279,18 +311,21 @@ static const iocshArg initArg0_2 =
static const iocshArg initArg1_2 =
{ "Axis id []", iocshArgInt };
static const iocshArg initArg2_2 =
{ "Velo limit [unit same as axis cfg]", iocshArgDouble };
{ "Velo stand still limit [unit same as axis cfg]", iocshArgDouble };
static const iocshArg initArg3_2 =
{ "Velo stand still filter time [ms]", iocshArgInt };
static const iocshArg initArg4_2 =
{ "Velo max limit [unit same as axis cfg]", iocshArgDouble };
static const iocshArg *const initArgs_2[] = { &initArg0_2,
&initArg1_2,
&initArg2_2,
&initArg3_2};
&initArg3_2,
&initArg4_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 4, initArgs_2};
static const iocshFuncDef initFuncDef_2 = { "ecmcAddAxisToSafetyGroup", 5, initArgs_2};
static void initCallFunc_2(const iocshArgBuf *args) {
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival);
ecmcAddAxisToSafetyGroup(args[0].sval, args[1].ival, args[2].dval, args[3].ival, args[4].dval);
}
void ecmcSafetyPlgRegister(void) {