aaa
CalPIDF.cpp
- Committer:
- yuki0108
- Date:
- 2021-03-18
- Revision:
- 0:76fdba038ca7
File content as of revision 0:76fdba038ca7:
#include "mbed.h" #include "CalPIDF.h" CalPIDF::CalPIDF(double kp_, double ki_, double kd_,double dt, double max) { setParameter(kp_, ki_, kd_); deviation_old = 0; integral = 0; delta_t=dt; setMaxValue(max); }; void CalPIDF::setParameter(double kp_, double ki_, double kd_) { kp = kp_; ki = ki_; kd = kd_; } void CalPIDF::setMaxValue(double max) { max_pid = max; } void CalPIDF::setDELTA_T(double delta_time) { delta_t = delta_time; } double CalPIDF::calPID(double devia_present) { double differential = (deviation_old - devia_present) / delta_t; if(value_PID!=max_pid&&value_PID!=-max_pid) {//アンチワインドアップ(積分項固定式) integral += (devia_present + deviation_old) * delta_t / 2; } value_PID = kp * devia_present + ki * integral - kd * differential; if (value_PID > max_pid) { value_PID = max_pid; } else if (value_PID < -max_pid) { value_PID = -max_pid; } deviation_old = devia_present; return value_PID; } double CalPIDF::calPD(double devia_present) { double differential = (deviation_old - devia_present) / delta_t; double value_PD = kp * devia_present - kd * differential; if (value_PD > max_pid) { value_PD = max_pid; } else if (value_PD < -max_pid) { value_PD = -max_pid; } deviation_old = devia_present; return value_PD; } double CalPIDF::calP_D(double devia_present,double diff_value) { double value_P_D = kp * devia_present - kd * diff_value; if (value_P_D > max_pid) { value_P_D = max_pid; } else if (value_P_D < -max_pid) { value_P_D = -max_pid; } return value_P_D; } void CalPIDF::resetIntegral() { integral = 0; }