Diff: PID_impl.hpp

Revision:

6:4427687e6dbe

Parent:

5:82bc9c845760

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;
 }