diff --git a/qt/ecmc_plugin_safety_main.ui b/qt/ecmc_plugin_safety_main.ui
index 18a815b..d4ee110 100644
--- a/qt/ecmc_plugin_safety_main.ui
+++ b/qt/ecmc_plugin_safety_main.ui
@@ -7,7 +7,7 @@
0
0
315
- 150
+ 143
@@ -17,7 +17,7 @@
20
- 45
+ 98
30
30
@@ -44,7 +44,7 @@
55
- 47
+ 100
251
26
@@ -60,7 +60,7 @@
55
- 75
+ 40
246
28
@@ -76,7 +76,7 @@
20
- 75
+ 40
30
30
@@ -103,7 +103,7 @@
55
- 105
+ 70
206
28
@@ -119,7 +119,7 @@
20
- 105
+ 70
30
30
diff --git a/qt/sim_motion_safety_box.ui b/qt/sim_motion_safety_box.ui
index fe2a81c..eb30c88 100644
--- a/qt/sim_motion_safety_box.ui
+++ b/qt/sim_motion_safety_box.ui
@@ -6,7 +6,7 @@
0
0
- 364
+ 341
312
@@ -120,12 +120,15 @@
- 170
+ 54
280
- 100
+ 41
22
+
+ Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter
+
$(IOC):m$(M_ID=0)s$(BI_S_ID)-One
@@ -133,7 +136,7 @@
- 30
+ 20
10
301
28
@@ -179,6 +182,38 @@
EPushButton::None
+
+
+
+ 135
+ 85
+ 131
+ 28
+
+
+
+ (Ramp down command)
+
+
+ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter
+
+
+
+
+
+ 120
+ 275
+ 171
+ 28
+
+
+
+ Command PV (ONE)
+
+
+ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter
+
+
diff --git a/src/ecmcSS1SafetyGroup.cpp b/src/ecmcSS1SafetyGroup.cpp
index 1065e7e..3635cdd 100644
--- a/src/ecmcSS1SafetyGroup.cpp
+++ b/src/ecmcSS1SafetyGroup.cpp
@@ -94,6 +94,7 @@ ecmcSS1SafetyGroup::ecmcSS1SafetyGroup(const char *name,
limitVeloCmd_ = 0;
axesDisabled_ = 0;
cfgDbgMode_ = 0;
+ oldStop_ = 0;
memset(&status_,0,sizeof(status_));
parseConfigStr(cfg_string);
@@ -621,8 +622,24 @@ void ecmcSS1SafetyGroup::setAxesSafetyInterlocks(int stop) {
if(!*saxis) {
throw std::runtime_error( "Safety: Axis object NULL.");
}
- setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);
+
+ setAxisEmergencyStopInterlock((*saxis)->axisId_,stop);
+
+ if(!oldStop_ && stop) {
+ //Calcualte new deceleration if delay is defined and abs(velo) > 0
+ if(delayMs_ > 0) {
+ double velo = 0;
+ getAxisTrajVelo((*saxis)->axisId_,&velo);
+ velo = std::abs(velo);
+ if(velo > 0) {
+ double newEmergDec = velo / (ECMC_PLUGIN_RAMP_DOWN_TIME_RATIO * delayMs_ / 1000.0);
+ setAxisEmergDeceleration((*saxis)->axisId_,newEmergDec);
+ printf("Safety %s: Set new emergency deceleration %lf \n",sName_,newEmergDec);
+ }
+ }
+ }
}
+ oldStop_ = stop;
}
void ecmcSS1SafetyGroup::addAxis(int axisId,
diff --git a/src/ecmcSS1SafetyGroup.h b/src/ecmcSS1SafetyGroup.h
index 520f64f..4ce1b80 100644
--- a/src/ecmcSS1SafetyGroup.h
+++ b/src/ecmcSS1SafetyGroup.h
@@ -162,8 +162,9 @@ class ecmcSS1SafetyGroup : public asynPortDriver {
ecmcEcEntry *ecEntryRampDown_;
ecmcEcEntry *ecEntryStandstill_;
ecmcEcEntry *ecEntryLimitVelo_;
-
+ bool oldStop_;
static std::string to_string(int value);
+
};
#endif /* ECMC_SAFETY_GROUP_PLG_H_ */
diff --git a/src/ecmcSafetyPlgDefs.h b/src/ecmcSafetyPlgDefs.h
index 88dfdba..0ba59d3 100644
--- a/src/ecmcSafetyPlgDefs.h
+++ b/src/ecmcSafetyPlgDefs.h
@@ -25,4 +25,7 @@
#define ECMC_PLUGIN_SAFETY_ERROR_CODE 1
#define ECMC_PLUGIN_ALREADY_LOADED_ERROR_CODE 2
+// How big part of delay is used for rampdown
+#define ECMC_PLUGIN_RAMP_DOWN_TIME_RATIO 0.9
+
#endif /* ECMC_MOTION_PLG_DEFS_H_ */