PID.h

   1 #pragma once
   2
   3 template <class T>
   4 class PID
   5 {
   6         T m_kp;
   7         T m_ki;
   8         T m_kd;
   9         T m_clipMin;
  10         T m_clipMax;
  11         T m_prevDiff;
  12         T m_sumDiff;
  13 public:
  14         PID();
  15         PID(const T& kp, const T& ki, const T& kd);
  16         ~PID(void);
  17         //
  18         T CalcControlValue(const T& diff);
  19         void SetClip(const T& clipMin, const T& clipMax);
  20         void ResetPrevDiff();
  21 };
  22
  23 template<class T>
  24 PID<T>::PID(const T& kp, const T& ki, const T& kd)
  25         : m_kp(kp), m_ki(ki), m_kd(kd), m_clipMin(0), m_clipMax(-1), m_prevDiff(0), m_sumDiff(0)
  26 {
  27 }
  28
  29 template<class T>
  30 PID<T>::~PID(void)
  31 {
  32 }
  33
  34 template<class T>
  35 T PID<T>::CalcControlValue(const T& diff)
  36 {
  37         T val = m_kp * diff + m_kd * (diff - m_prevDiff) + m_ki * m_sumDiff;
  38         m_sumDiff += diff;
  39         m_prevDiff = diff;
  40
  41         if(m_clipMin <= m_clipMax){
  42                 if(val < m_clipMin){
  43                         val = m_clipMin;
  44                 }
  45                 if(m_clipMax < val){
  46                         val = m_clipMax;
  47                 }
  48         }
  49         return val;
  50 }
  51
  52 template<class T>
  53 void PID<T>::SetClip(const T& clipMin, const T& clipMax)
  54 {
  55         m_clipMin = clipMin;
  56         m_clipMax = clipMax;
  57 }
  58
  59 template<class T>
  60 void PID<T>::ResetPrevDiff()
  61 {
  62         m_prevDiff = 0;
  63         m_sumDiff = 0;
  64 }