first
Diff: pid.cpp
- Revision:
- 0:4ecdddd912e2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.cpp Sun Sep 24 05:23:58 2017 +0000 @@ -0,0 +1,62 @@ +#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.5*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; +} + +float PositionPid::duty_enableWidth(float duty_min, float duty_max){ + if(now > duty_min && now < duty_max){ //nowは差 + return 0.0; + }else{ + 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; +}