tarou yamada
Fork of
PID
by 
tarou yamada
PID_impl.hpp@6:4427687e6dbe, 2016-04-26 (annotated)
- Committer:
- inst
- Date:
- Tue Apr 26 08:25:02 2016 +0000
- Revision:
- 6:4427687e6dbe
- Parent:
- 5:82bc9c845760
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| inst | 2:73618cad4762 | 1 | #ifndef INCLUDED_PID_IMPL_H |
| inst | 2:73618cad4762 | 2 | #define INCLUDED_PID_IMPL_H |
| inst | 2:73618cad4762 | 3 | |
| inst | 2:73618cad4762 | 4 | #include "PID.hpp" |
| inst | 6:4427687e6dbe | 5 | #include "mbed.h" |
| inst | 2:73618cad4762 | 6 | |
| inst | 4:ed6aea0299df | 7 | namespace mbed_stl { |
| inst | 4:ed6aea0299df | 8 | |
| inst | 6:4427687e6dbe | 9 | template <typename ErrType, typename MVType, typename KType> |
| inst | 6:4427687e6dbe | 10 | PID<ErrType, MVType, KType>::PID(KType kp, KType ki, KType kd) : |
| inst | 2:73618cad4762 | 11 | kp_(kp), |
| inst | 2:73618cad4762 | 12 | ki_(ki), |
| inst | 2:73618cad4762 | 13 | kd_(kd), |
| inst | 2:73618cad4762 | 14 | integral_(), |
| inst | 6:4427687e6dbe | 15 | prev_error_(), |
| inst | 6:4427687e6dbe | 16 | prev_time_us_() { |
| inst | 6:4427687e6dbe | 17 | timer_.reset(); |
| inst | 6:4427687e6dbe | 18 | } |
| inst | 2:73618cad4762 | 19 | |
| inst | 6:4427687e6dbe | 20 | template <typename ErrType, typename MVType, typename KType> |
| inst | 6:4427687e6dbe | 21 | inline MVType PID<ErrType, MVType, KType>::update(ErrType error) { |
| inst | 6:4427687e6dbe | 22 | float time_us = timer_.read_us() - prev_time_us_; |
| inst | 6:4427687e6dbe | 23 | |
| inst | 2:73618cad4762 | 24 | // P制御(比例) |
| inst | 6:4427687e6dbe | 25 | MVType mv = kp_ * error; |
| inst | 2:73618cad4762 | 26 | |
| inst | 2:73618cad4762 | 27 | // I制御(積分) |
| inst | 5:82bc9c845760 | 28 | mv += ki_ * integral_; |
| inst | 6:4427687e6dbe | 29 | integral_ += time_us * (error + prev_error_) / (2.0f * 1000.0f * 1000.0f); |
| inst | 2:73618cad4762 | 30 | |
| inst | 2:73618cad4762 | 31 | // D制御(微分) |
| inst | 6:4427687e6dbe | 32 | if (time_us > 0.0f) { |
| inst | 6:4427687e6dbe | 33 | mv += kd_ * (error - prev_error_) * 1000.0f * 1000.0f / time_us; |
| inst | 6:4427687e6dbe | 34 | } |
| inst | 6:4427687e6dbe | 35 | |
| inst | 2:73618cad4762 | 36 | prev_error_ = error; |
| inst | 6:4427687e6dbe | 37 | prev_time_us_ = time_us; |
| inst | 6:4427687e6dbe | 38 | timer_.start(); |
| inst | 2:73618cad4762 | 39 | |
| inst | 2:73618cad4762 | 40 | return mv; |
| inst | 2:73618cad4762 | 41 | } |
| inst | 2:73618cad4762 | 42 | |
| inst | 4:ed6aea0299df | 43 | } /* namespace mbed_stl */ |
| inst | 4:ed6aea0299df | 44 | |
| inst | 2:73618cad4762 | 45 | #endif |