Use boost clamp

huemerj · May 3, 2016 · 8859856 · 8859856
1 parent 125e75e
commit 8859856
Showing 1 changed file with 6 additions and 2 deletions.
diff --git a/src/pid.cpp b/src/pid.cpp
@@ -41,6 +41,8 @@
 #include <control_toolbox/pid.h>
 #include <tinyxml.h>
 
+#include <boost/algorithm/clamp.hpp>
+
 namespace control_toolbox {
 
 static const std::string DEFAULT_NAMESPACE = "pid"; // \todo better default prefix?
@@ -318,7 +320,9 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   if(gains.antiwindup_)
   {
     // Prevent i_error_ from climbing higher than permitted by i_max_/i_min_
-    i_error_ = std::max(gains.i_min_ / std::fabs(gains.i_gain_), std::min(i_error_, gains.i_max_ / std::fabs(gains.i_gain_)));
+    i_error_ = boost::algorithm::clamp(i_error_,
+                                       gains.i_min_ / std::abs(gains.i_gain_),
+                                       gains.i_max_ / std::abs(gains.i_gain_));
   }
 
   // Calculate integral contribution to command
@@ -327,7 +331,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
   if(!gains.antiwindup_)
   {
     // Limit i_term so that the limit is meaningful in the output
-    i_term = std::max(gains.i_min_, std::min(i_term, gains.i_max_));
+    i_term = boost::algorithm::clamp(i_term, gains.i_min_, gains.i_max_);
   }
 
   // Calculate derivative contribution to command