updated pid for clamped output

Author: Dwatkins

src/pid/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(catkin REQUIRED COMPONENTS)
 
 catkin_package(
 	INCLUDE_DIRS include
+        LIBRARIES pid
 )
 
 ## Check C++11 / C++0x
@@ -17,4 +18,9 @@ elseif(COMPILER_SUPPORTS_CXX0X)
 set(CMAKE_CXX_FLAGS "-std=c++0x")
 else()
 message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
+endif()
+
+
+include_directories(include)
+add_library(pid
+            src/pid.h)

src/pid/include/pid/pid.h

@@ -1,91 +1,119 @@
-class libpid
+#ifndef PID_H
+#define PID_H
+
+class PID
 {
  public:
-  libpid()
+  PID();
+  ~PID() {}
+  PID(double p, 
+      double i, 
+      double d, 
+      double derivator, 
+      double integrator,
+      double integrator_max,
+      double integrator_min);
+  
+  PID& operator=(const PID& rhs);
+  
+  double update(double current_value)
   {
-    kp_ = ki_ = kd_ = derivator_ = integrator_ = integrator_max_ = integrator_min_ = set_point_ = 0;
-  }
-
-  libpid(double p,
-	 double i,
-	 double d,
-	 double derivator,
-	 double integrator,
-	 double integrator_max,
-   double integrator_min)
-   {
-    kp_ = p;
-    ki_ = i;
-    kd_ = d;
-    derivator_ = derivator;
-    integrator_ = integrator;
-    integrator_max_ = integrator_max;
-    integrator_min_ = integrator_min;
-  }
-
-  double update(double current_value){
     double error = set_point_ - current_value;
-    double p_value = kp_ * error;
-    double d_value = kd_ * (error - derivator_);
+    integrator_ += error;
+    //clamp integrator_ value between integrator_max_ and integrator_min_
+    double i_value = ki_ * (integrator_ > integrator_max_ ? integrator_max_ : (integrator_ < integrator_min_ ? integrator_min_ : integrator_));
+    double pid = kp_*error + kd_*(error-derivator_) + i_value;
+    if(isLimited_)
+      {
+        //clamp pid value between output_max_ and output_min_
+        pid = pid > output_max_ ? output_max_ : (pid < output_min_ ? output_min_ : pid);
+      }
     derivator_ = error;
-    integrator_ = integrator_ + error;
-    if (integrator_ > integrator_max_)
-      integrator_ = integrator_max_;
-    else if (integrator_ < integrator_min_)
-      integrator_ = integrator_min_;
-    double i_value = integrator_ * ki_;
-    double pid = p_value + i_value + d_value;
-    return pid;
-  }
-
-  void setPoint(double set_point){
+    return pid;  
+  };
+  
+  void setPoint(double set_point)
+  {
     set_point_ = set_point;
-  }
-
-  void setIntegrator(double integrator){
+  };
+  
+  void setIntegrator(double integrator)
+  {
     integrator_ = integrator;
-  }
-
-  void setIntegratorMax(double max){
+  };
+  
+  void setIntegratorMax(double max)
+  {
     integrator_max_ = max;
-  }
+  };
 
-  void setIntegratorMin(double min) {
+  void setIntegratorMin(double min)
+  {
     integrator_min_ = min;
-  }
-
-  void setKp(double kp){
-    kp_ = (kp >= 0) ? kp : 0;
-  }
-
-  void setKi(double ki){
-    ki_ = (ki >= 0) ? ki : 0;
-  }
-
-  void setKd(double kd){
-    kd_ = (kd >= 0) ? kd : 0;
-  }
-  double getKp(){
-    return kp_;
-  }
-  double getKd(){
-    return kd_;
-  }
-  double getKi(){
-    return ki_;
-  }
+  };
+  
+  void setDerivator(double derivator)
+  {
+    derivator_ = derivator;
+  };
+  
+  void setKp(double kp)
+  {
+    kp_ = kp;
+  };
+  
+  void setKi(double ki)
+  {
+    ki_ = ki;
+  };
+  
+  void setKd(double kd)
+  {
+    kd_ = kd;
+  };
+  
+  void limitOutput(double min, double max)
+  {
+    isLimited_ = true;
+    output_min_ = min;
+    output_max_ = max;
+  };
 
-  double getPoint(){
+  void delimitOutput()
+  {
+    isLimited_ = false;
+  };
+  
+  double getPoint()
+  {
     return set_point_;
-  }
-
-  double getIntegrator(){
+  };
+  
+  double getIntegrator()
+  {
     return integrator_;
-  }
-  double getDerivator(){
+  };
+  
+  double getDerivator()
+  {
     return derivator_;
-  }
+  };
+  
+  double getOutputMin()
+  {
+    return output_min_;
+  };
+  
+  double getOutputMax()
+  {
+    return output_max_;
+  };
 
+  bool outputIsLimited()
+  {
+    return isLimited_;
+  };
+  
  private:
   double kp_;
   double ki_;
@@ -95,4 +123,9 @@ class libpid
   double integrator_min_;
   double integrator_max_;
   double set_point_;
+  double output_max_;
+  double output_min_;
+  bool isLimited_;
 };
+
+#endif

src/pid/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>pid</name>
-  <version>0.0.0</version>
+  <version>1.0.0</version>
   <description>The pid package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

src/pid/src/pid/pid.cpp

@@ -0,0 +1,47 @@
+#include "pid.h"
+
+PID::PID()
+{
+  kp_ = ki_ = kd_ = derivator_ = integrator_ = integrator_max_ = integrator_min_ = set_point_ = output_max_ = output_min_ = 0;
+  isLimited_ = false;
+}
+
+PID::PID(double p, 
+	 double i, 
+	 double d, 
+	 double derivator, 
+	 double integrator,
+	 double integrator_max,
+	 double integrator_min) {
+  kp_ = p;
+  ki_ = i;
+  kd_ = d;
+  derivator_ = derivator;
+  integrator_ = integrator;
+  integrator_max_ = integrator_max;
+  integrator_min_ = integrator_min;
+  output_max_ = 0;
+  output_min_ = 0;
+  isLimited_ = false;
+}
+
+PID&
+PID::operator=(const PID& rhs) {
+  if (this == &rhs)
+    return *this;
+  kp_ = rhs.kp_;
+  ki_ = rhs.ki_;
+  kd_ = rhs.kd_;
+  set_point_ = rhs.set_point_;
+  derivator_ = rhs.derivator_;
+  integrator_ = rhs.integrator_;
+  integrator_max_ = rhs.integrator_max_;
+  integrator_min_ = rhs.integrator_min_;
+  output_min_ = rhs.output_min_;
+  output_max_ = rhs.output_max_;
+  isLimited_ = rhs.isLimited_;
+
+  return *this;
+}
+
+