aaa
Diff: CalPIDF.cpp
- Revision:
- 0:76fdba038ca7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CalPIDF.cpp Thu Mar 18 12:19:17 2021 +0000 @@ -0,0 +1,74 @@ +#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; +} \ No newline at end of file