tarou yamada
Fork of
PID
by 
tarou yamada
PID_impl.hpp@2:73618cad4762, 2016-02-25 (annotated)
- Committer:
- inst
- Date:
- Thu Feb 25 16:22:59 2016 +0000
- Revision:
- 2:73618cad4762
- Child:
- 3:b28b34b89120
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 | 2:73618cad4762 | 17 | //T mv = static_cast<T>(kp_ * error); |
| inst | 2:73618cad4762 | 18 | T mv; |
| inst | 2:73618cad4762 | 19 | 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 | 2:73618cad4762 | 33 | #endif |