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

diff --git a/ETLineTracer.cpp b/ETLineTracer.cpp
index 05334da..0306012 100644 (file)
--- a/ETLineTracer.cpp
+++ b/ETLineTracer.cpp
@@ -3,9 +3,10 @@
 #include "Runner.h"\r
 \r
 ETLineTracer::ETLineTracer(LinePos& linepos, Runner& runner)\r
-       : m_linepos(linepos), m_runner(runner), m_pid(0.3, 0.0, 10.0)\r
+       : m_linepos(linepos), m_runner(runner), m_pid(0.3, 0.001, 10.0)\r
 {\r
        m_pid.SetClip(-50,50);\r
+       m_pid.SetIntegralClip(-500,500);\r
 }\r
 ETLineTracer::~ETLineTracer(void)\r
 {\r

diff --git a/PID.h b/PID.h
index 4d7d67c..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,12 +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)
 {
 }
 
@@ -38,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;
@@ -57,6 +71,13 @@ void PID<T>::SetClip(const T& clipMin, const T& 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;