PID.h: I項がオーバーしないようにI項クリップ設定を追加。

[tondenhei/et2013.git] / PID.h

diff --git a/PID.h b/PID.h
index 4ec8ee6..9605834 100644 (file)
--- a/PID.h
+++ b/PID.h
@@ -8,6 +8,8 @@ class PID
        T m_kd;
        T m_clipMin;
        T m_clipMax;
+       T m_integralClipMin;
+       T m_integralClipMax;
        T m_prevDiff;
        T m_sumDiff;
 public:
@@ -17,11 +19,15 @@ public:
        //
        T CalcControlValue(const T& diff);
        void SetClip(const T& clipMin, const T& clipMax);
+       void SetIntegralClip(const T& clipMin, const T& clipMax);
+       void ResetPrevDiff();
 };
 
 template<class T>
 PID<T>::PID(const T& kp, const T& ki, const T& kd)
-       : m_kp(kp), m_ki(ki), m_kd(kd), m_clipMin(0), m_clipMax(-1), m_prevDiff(0), m_sumDiff(0)
+       : m_kp(kp), m_ki(ki), m_kd(kd),
+       m_clipMin(0), m_clipMax(-1), m_integralClipMin(0), m_integralClipMax(-1),
+       m_prevDiff(0), m_sumDiff(0)
 {
 }
 
@@ -37,6 +43,15 @@ T PID<T>::CalcControlValue(const T& diff)
        m_sumDiff += diff;
        m_prevDiff = diff;
 
+       if(m_integralClipMin <= m_integralClipMax){
+               if(m_sumDiff < m_integralClipMin){
+                       m_sumDiff = m_integralClipMin;
+               }
+               if(m_integralClipMax < m_sumDiff){
+                       m_sumDiff = m_integralClipMax;
+               }
+       }
+
        if(m_clipMin <= m_clipMax){
                if(val < m_clipMin){
                        val = m_clipMin;
@@ -54,3 +69,17 @@ void PID<T>::SetClip(const T& clipMin, const T& clipMax)
        m_clipMin = clipMin;
        m_clipMax = clipMax;
 }
+
+template<class T>
+void PID<T>::SetIntegralClip(const T& clipMin, const T& clipMax)
+{
+       m_integralClipMin = clipMin;
+       m_integralClipMax = clipMax;
+}
+
+template<class T>
+void PID<T>::ResetPrevDiff()
+{
+       m_prevDiff = 0;
+       m_sumDiff = 0;
+}