ETLineTracer: 前進量もPIDで決めるようにした。

diff --git a/ETLineTracer.cpp b/ETLineTracer.cpp
index 1ab30b3..8f41aec 100644 (file)
--- a/ETLineTracer.cpp
+++ b/ETLineTracer.cpp
@@ -1,12 +1,15 @@
 #include "ETLineTracer.h"\r
 #include "LinePos.h"\r
 #include "Runner.h"\r
+#include "math.h"\r
 \r
 ETLineTracer::ETLineTracer(LinePos& linepos, Runner& runner)\r
-       : m_linepos(linepos), m_runner(runner), m_pid(0.4, 0.0015, 12.5)\r
+       : m_linepos(linepos), m_runner(runner), m_pid(0.4, 0.0015, 12.5), m_speedpid(0.3 ,0.03 ,10.0)\r
 {\r
-       m_pid.SetClip(-100,100);\r
-       m_pid.SetIntegralClip(-1000,1000);\r
+       m_pid.SetClip(-100.0,100.0);\r
+       m_pid.SetIntegralClip(-1000.0,1000.0);\r
+       m_speedpid.SetClip(0.0,50.0);\r
+       m_speedpid.SetIntegralClip(-500.0,500.0);\r
 }\r
 ETLineTracer::~ETLineTracer(void)\r
 {\r
@@ -30,9 +33,24 @@ void ETLineTracer::Reset()
 {\r
        m_runner.Reset();\r
        m_pid.ResetPrevDiff();\r
+       m_speedpid.ResetPrevDiff();\r
+}\r
+template<class T>\r
+inline static void clipmax(T& val, const T& max){\r
+       val = (val < max) ? val : max;\r
 }\r
 void ETLineTracer::CalcOutput(int speed, int linepos, int& forward, int& turn)\r
 {\r
-       forward = speed * 7 / 10;       // \82Æ\82è\82 \82¦\82¸\82Ì\8eÀ\91\95\r
-       turn =static_cast<int>(m_pid.CalcControlValue(static_cast<float>(linepos)));\r
+       float fturn = m_pid.CalcControlValue(static_cast<float>(linepos));\r
+       turn = static_cast<int>(fturn);\r
+       float fabsturn = fabs(fturn);\r
+       float fwddiff;\r
+       if(fabsturn < 5.0){\r
+               fwddiff = -1.0;\r
+       }else{\r
+               fwddiff = fabsturn;\r
+       }\r
+       clipmax(speed,90);      // \82±\82ê\88È\8fã\82Å\82Í\91\96\82ç\82È\82¢\82Ì\82Å\90§\8cÀ\r
+       float fforward = static_cast<float>(speed) * (100.0 - m_speedpid.CalcControlValue(fwddiff)) / 100.0;\r
+       forward = static_cast<int>(fforward);\r
 }\r

diff --git a/ETLineTracer.h b/ETLineTracer.h
index 49d0d59..f7b5aec 100644 (file)
--- a/ETLineTracer.h
+++ b/ETLineTracer.h
@@ -10,6 +10,7 @@ class ETLineTracer : public Robot
        LinePos& m_linepos;\r
        Runner& m_runner;\r
        PID<float> m_pid;\r
+       PID<float>m_speedpid;\r
 public:\r
        ETLineTracer(LinePos& linepos, Runner& runner);\r
        virtual ~ETLineTracer(void);\r