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