Merge pull request #44 from clearpathrobotics/merge-pid-calls

Chain calls of computeCommand and updatePid for code reuse

Author: Adolfo Rodriguez Tsouroukdissian

src/pid.cpp

@@ -266,78 +266,37 @@ void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uin
 
 double Pid::computeCommand(double error, ros::Duration dt)
 {
-  // Get the gain parameters from the realtime buffer
-  Gains gains = *gains_buffer_.readFromRT();
-
-  double p_term, d_term, i_term;
-  p_error_ = error; // this is error = target - state
 
   if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
     return 0.0;
 
-  // Calculate proportional contribution to command
-  p_term = gains.p_gain_ * p_error_;
-
-  // Calculate the integral of the position error
-  i_error_ += dt.toSec() * p_error_;
-  
-  // Calculate integral contribution to command
-  i_term = gains.i_gain_ * i_error_;
-
-  // 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_) );
+  double error_dot = d_error_;
 
   // Calculate the derivative error
   if (dt.toSec() > 0.0)
   {
-    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
-    p_error_last_ = p_error_;
+    error_dot = (error - p_error_last_) / dt.toSec();
+    p_error_last_ = error;
   }
-  // Calculate derivative contribution to command
-  d_term = gains.d_gain_ * d_error_;
 
-  // Compute the command
-  cmd_ = p_term + i_term + d_term;
-
-  return cmd_;
+  return computeCommand(error, error_dot, dt);
 }
 
 double Pid::updatePid(double error, ros::Duration dt)
 {
-  // Get the gain parameters from the realtime buffer
-  Gains gains = *gains_buffer_.readFromRT();
-
-  double p_term, d_term, i_term;
-  p_error_ = error; //this is pError = pState-pTarget
-
   if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
     return 0.0;
 
-  // Calculate proportional contribution to command
-  p_term = gains.p_gain_ * p_error_;
-
-  // Calculate the integral of the position error
-  i_error_ += dt.toSec() * p_error_;
-  
-  // Calculate integral contribution to command
-  i_term = gains.i_gain_ * i_error_;
-
-  // 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_) );
+  double error_dot = d_error_;
 
   // Calculate the derivative error
   if (dt.toSec() > 0.0)
   {
-    d_error_ = (p_error_ - p_error_last_) / dt.toSec();
-    p_error_last_ = p_error_;
+    error_dot = (error - p_error_last_) / dt.toSec();
+    p_error_last_ = error;
   }
-  // Calculate derivative contribution to command
-  d_term = gains.d_gain_ * d_error_;
 
-  // Compute the command
-  cmd_ = - p_term - i_term - d_term;
-
-  return cmd_;
+  return updatePid(error, error_dot, dt);
 }
 
 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)