tarou yamada
Fork of
PID
by 
tarou yamada
PID_impl.hpp@4:ed6aea0299df, 2016-03-03 (annotated)
- Committer:
- inst
- Date:
- Thu Mar 03 02:46:17 2016 +0000
- Revision:
- 4:ed6aea0299df
- Parent:
- 3:b28b34b89120
- Child:
- 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 | 2:73618cad4762 | 5 | |
| inst | 4:ed6aea0299df | 6 | namespace mbed_stl { |
| inst | 4:ed6aea0299df | 7 | |
| inst | 2:73618cad4762 | 8 | template <typename T, typename K> |
| inst | 2:73618cad4762 | 9 | PID<T, K>::PID(K kp, K ki, K kd) : |
| inst | 2:73618cad4762 | 10 | kp_(kp), |
| inst | 2:73618cad4762 | 11 | ki_(ki), |
| inst | 2:73618cad4762 | 12 | kd_(kd), |
| inst | 2:73618cad4762 | 13 | integral_(), |
| inst | 2:73618cad4762 | 14 | prev_error_() {} |
| inst | 2:73618cad4762 | 15 | |
| inst | 2:73618cad4762 | 16 | template <typename T, typename K> |
| inst | 2:73618cad4762 | 17 | inline T PID<T, K>::update(T error) { |
| inst | 2:73618cad4762 | 18 | // P制御(比例) |
| inst | 3:b28b34b89120 | 19 | T mv = kp_ * error; |
| inst | 2:73618cad4762 | 20 | |
| inst | 2:73618cad4762 | 21 | // I制御(積分) |
| inst | 2:73618cad4762 | 22 | mv += ki_ * error; |
| inst | 2:73618cad4762 | 23 | integral_ += error; |
| inst | 2:73618cad4762 | 24 | |
| inst | 2:73618cad4762 | 25 | // D制御(微分) |
| inst | 2:73618cad4762 | 26 | mv += kd_ * (error - prev_error_); |
| inst | 2:73618cad4762 | 27 | |
| inst | 2:73618cad4762 | 28 | prev_error_ = error; |
| inst | 2:73618cad4762 | 29 | |
| inst | 2:73618cad4762 | 30 | return mv; |
| inst | 2:73618cad4762 | 31 | } |
| inst | 2:73618cad4762 | 32 | |
| inst | 4:ed6aea0299df | 33 | } /* namespace mbed_stl */ |
| inst | 4:ed6aea0299df | 34 | |
| inst | 2:73618cad4762 | 35 | #endif |