Factor out updatePid as negative calls to computeCommand

Author: Paul Bovbel

src/pid.cpp

@@ -284,19 +284,7 @@ double Pid::computeCommand(double error, ros::Duration dt)
 
 double Pid::updatePid(double error, ros::Duration dt)
 {
-  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
-    return 0.0;
-
-  double error_dot = d_error_;
-
-  // Calculate the derivative error
-  if (dt.toSec() > 0.0)
-  {
-    error_dot = (error - p_error_last_) / dt.toSec();
-    p_error_last_ = error;
-  }
-
-  return updatePid(error, error_dot, dt);
+  return -computeCommand(error, dt);
 }
 
 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
@@ -335,36 +323,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
 
 double Pid::updatePid(double error, double error_dot, 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
-  d_error_ = error_dot;
-
-  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
-    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_) );
-
-  // 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);
 }
 
 void Pid::setCurrentCmd(double cmd)