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

Calc emerg decelartion depending on actual traj velo

Browse Source
This commit is contained in:
sandst_a
2025-02-25 09:18:12 +01:00
parent 07acbe990e
commit 8f68bc021d
5 changed files with 69 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/ecmcSS1SafetyGroup.cpp
View File

@@ -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,