Clip too big reference speeds instead of aborting setter

roboticslab-uc3m · Jan 17, 2024 · 32aec36 · 32aec36
1 parent 5adb74f
commit 32aec36
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IPositionControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/embedded-pid/IPositionControlRawImpl.cpp
@@ -60,13 +60,13 @@ bool TechnosoftIposEmbedded::setRefSpeedRaw(int j, double sp)
 
     if (sp <= 0.0)
     {
-        yCIWarning(IPOS, id()) << "Illegal negative speed provided:" << sp;
+        yCIError(IPOS, id()) << "Illegal reference speed provided:" << sp;
         return false;
     }
     else if (sp > maxVel)
     {
         yCIWarning(IPOS, id()) << "Reference speed exceeds maximum velocity, i.e." << maxVel.load();
-        return false;
+        sp = maxVel;
     }
 
     double value = std::abs(degreesToInternalUnits(sp, 1));
@@ -88,7 +88,7 @@ bool TechnosoftIposEmbedded::setRefAccelerationRaw(int j, double acc)
 
     if (acc <= 0.0)
     {
-        yCIWarning(IPOS, id()) << "Illegal negative acceleration provided:" << acc;
+        yCIError(IPOS, id()) << "Illegal reference acceleration provided:" << acc;
         return false;
     }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IPositionControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IPositionControlRawImpl.cpp
@@ -79,13 +79,13 @@ bool TechnosoftIposExternal::setRefSpeedRaw(int j, double sp)
 
     if (sp <= 0.0)
     {
-        yCIWarning(IPOS, id()) << "Illegal reference speed provided:" << sp;
+        yCIError(IPOS, id()) << "Illegal reference speed provided:" << sp;
         return false;
     }
     else if (sp > maxVel)
     {
         yCIWarning(IPOS, id()) << "Reference speed exceeds maximum velocity:" << sp << ">" << maxVel.load();
-        return false;
+        sp = maxVel;
     }
 
     if (trajectory.isActive())
@@ -108,7 +108,7 @@ bool TechnosoftIposExternal::setRefAccelerationRaw(int j, double acc)
 
     if (acc <= 0.0)
     {
-        yCIWarning(IPOS, id()) << "Illegal reference acceleration provided:" << acc;
+        yCIError(IPOS, id()) << "Illegal reference acceleration provided:" << acc;
         return false;
     }