Revision 6:4427687e6dbe, committed 2016-04-26

Comitter:

inst

Date:

Tue Apr 26 08:25:02 2016 +0000

Parent:

5:82bc9c845760

Commit message:

Changed in this revision

diff -r 82bc9c845760 -r 4427687e6dbe PID.hpp
--- a/PID.hpp	Thu Mar 03 02:50:11 2016 +0000
+++ b/PID.hpp	Tue Apr 26 08:25:02 2016 +0000
@@ -1,26 +1,35 @@
 #ifndef INCLUDED_MBED_STL_PID_H
 #define INCLUDED_MBED_STL_PID_H
 
+#include "mbed.h"
+
 namespace mbed_stl {
 
-template <typename T, typename K = T>
+// ErrTypeは偏差の型．MVTypeは操作量の型．KTypeは係数の型．
+template <typename ErrType, typename MVType = ErrType, typename KType = ErrType>
 class PID{
 public:
-    PID(K kp, K ki, K kd);
+    PID(KType kp, KType ki, KType kd);
     
-    T update(T error);
+    MVType update(ErrType error);
     
 private:
-    const K kp_;    // 比例制御係数
-    const K ki_;    // 積分制御係数
-    const K kd_;    // 微分制御係数
+    const KType kp_;    // 比例制御係数
+    const KType ki_;    // 積分制御係数
+    const KType kd_;    // 微分制御係数
     
-    T integral_;
-    T prev_error_;
+    ErrType integral_;
+    ErrType prev_error_;
+    
+    Timer timer_;
+    float prev_time_us_;
 };
 
 } /* namespace mbed_stl */
 
 #include "PID_impl.hpp"
 
+typedef mbed_stl::PID<float> PID_f;
+typedef mbed_stl::PID<double> PID_d;
+
 #endif

diff -r 82bc9c845760 -r 4427687e6dbe PID_impl.hpp
--- a/PID_impl.hpp	Thu Mar 03 02:50:11 2016 +0000
+++ b/PID_impl.hpp	Tue Apr 26 08:25:02 2016 +0000
@@ -2,29 +2,40 @@
 #define INCLUDED_PID_IMPL_H
 
 #include "PID.hpp"
+#include "mbed.h"
 
 namespace mbed_stl {
 
-template <typename T, typename K>
-PID<T, K>::PID(K kp, K ki, K kd) :
+template <typename ErrType, typename MVType, typename KType>
+PID<ErrType, MVType, KType>::PID(KType kp, KType ki, KType kd) :
         kp_(kp),
         ki_(ki),
         kd_(kd),
         integral_(),
-        prev_error_() {}
+        prev_error_(),
+        prev_time_us_() {
+    timer_.reset();
+}
 
-template <typename T, typename K>
-inline T PID<T, K>::update(T error) {
+template <typename ErrType, typename MVType, typename KType>
+inline MVType PID<ErrType, MVType, KType>::update(ErrType error) {
+    float time_us = timer_.read_us() - prev_time_us_;
+    
     // P制御(比例)
-    T mv = kp_ * error;
+    MVType mv = kp_ * error;
     
     // I制御(積分)
     mv += ki_ * integral_;
-    integral_ += error;
+    integral_ += time_us * (error + prev_error_) / (2.0f * 1000.0f * 1000.0f);
     
     // D制御(微分)
-    mv += kd_ * (error - prev_error_);
+    if (time_us > 0.0f) {
+        mv += kd_ * (error - prev_error_) * 1000.0f * 1000.0f / time_us;
+    }
+    
     prev_error_ = error;
+    prev_time_us_ = time_us;
+    timer_.start();
     
     return mv;
 }