Kazushi Yamanobe
/
R-ciliatable_4
latest version 9/26
Diff: pid.cpp
- Revision:
- 0:0d02a451e79d
diff -r 000000000000 -r 0d02a451e79d pid.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.cpp Sun Oct 10 04:47:14 2021 +0000 @@ -0,0 +1,54 @@ +#include "pid.h" + +void PositionPid::setup(float Kp, float Ki, float Kd, float dt){ + kp = Kp; + ki = Ki; + kd = Kd; + time = dt; + frequency = 1/dt; +} + +void PositionPid::calculate(float target, float nowValue){ + old = now; + now = nowValue - target; + p = now; + i = i + (now + old) * 0.5f * time; + d = (now - old) * frequency; + result = kp*p + ki*i + kd*d; + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } +} + +float PositionPid::duty(){ + return result; +} + +void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){ + kp = Kp; + ki = Ki; + kd = Kd; + time = dt; + frequency = 1/dt; +} + +void SpeedPid::calculate(float target, float nowValue){ + e2 = e1; + e1 = e; + e = nowValue - target; + p = e - e1; + i = e*time; + d = (e-2*e1+e2)*frequency; + result = result + (kp*p+ki*i+kd*d); + if (result > 1.0f) { + result = 1.0f; + } else if (result < -1.0f) { + result = -1.0f; + } +} + +float SpeedPid::duty(){ + return result; +}