Kazushi Yamanobe
/
R-ciliatable_4
latest version 9/26
pid.cpp
- Committer:
- Yamanobe
- Date:
- 2021-10-10
- Revision:
- 0:0d02a451e79d
File content as of revision 0:0d02a451e79d:
#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; }