Fork of PID by
Diff: PID_impl.hpp
- Revision:
- 6:4427687e6dbe
- Parent:
- 5:82bc9c845760
--- 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; }