Fork of PID by
Diff: PID_impl.hpp
- Revision:
- 2:73618cad4762
- Child:
- 3:b28b34b89120
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID_impl.hpp Thu Feb 25 16:22:59 2016 +0000 @@ -0,0 +1,33 @@ +#ifndef INCLUDED_PID_IMPL_H +#define INCLUDED_PID_IMPL_H + +#include "PID.hpp" + +template <typename T, typename K> +PID<T, K>::PID(K kp, K ki, K kd) : + kp_(kp), + ki_(ki), + kd_(kd), + integral_(), + prev_error_() {} + +template <typename T, typename K> +inline T PID<T, K>::update(T error) { + // P制御(比例) + //T mv = static_cast<T>(kp_ * error); + T mv; + mv += kp_ * error; + + // I制御(積分) + mv += ki_ * error; + integral_ += error; + + // D制御(微分) + mv += kd_ * (error - prev_error_); + + prev_error_ = error; + + return mv; +} + +#endif