first
pid.cpp
- Committer:
- echo_piyo
- Date:
- 2017-09-24
- Revision:
- 0:4ecdddd912e2
File content as of revision 0:4ecdddd912e2:
#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; }