aaa
CalPIDF.cpp@0:76fdba038ca7, 2021-03-18 (annotated)
- Committer:
- yuki0108
- Date:
- Thu Mar 18 12:19:17 2021 +0000
- Revision:
- 0:76fdba038ca7
3/18
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki0108 | 0:76fdba038ca7 | 1 | #include "mbed.h" |
yuki0108 | 0:76fdba038ca7 | 2 | #include "CalPIDF.h" |
yuki0108 | 0:76fdba038ca7 | 3 | |
yuki0108 | 0:76fdba038ca7 | 4 | CalPIDF::CalPIDF(double kp_, double ki_, double kd_,double dt, double max) |
yuki0108 | 0:76fdba038ca7 | 5 | { |
yuki0108 | 0:76fdba038ca7 | 6 | setParameter(kp_, ki_, kd_); |
yuki0108 | 0:76fdba038ca7 | 7 | deviation_old = 0; |
yuki0108 | 0:76fdba038ca7 | 8 | integral = 0; |
yuki0108 | 0:76fdba038ca7 | 9 | delta_t=dt; |
yuki0108 | 0:76fdba038ca7 | 10 | setMaxValue(max); |
yuki0108 | 0:76fdba038ca7 | 11 | }; |
yuki0108 | 0:76fdba038ca7 | 12 | void CalPIDF::setParameter(double kp_, double ki_, double kd_) |
yuki0108 | 0:76fdba038ca7 | 13 | { |
yuki0108 | 0:76fdba038ca7 | 14 | kp = kp_; |
yuki0108 | 0:76fdba038ca7 | 15 | ki = ki_; |
yuki0108 | 0:76fdba038ca7 | 16 | kd = kd_; |
yuki0108 | 0:76fdba038ca7 | 17 | } |
yuki0108 | 0:76fdba038ca7 | 18 | void CalPIDF::setMaxValue(double max) |
yuki0108 | 0:76fdba038ca7 | 19 | { |
yuki0108 | 0:76fdba038ca7 | 20 | max_pid = max; |
yuki0108 | 0:76fdba038ca7 | 21 | } |
yuki0108 | 0:76fdba038ca7 | 22 | void CalPIDF::setDELTA_T(double delta_time) |
yuki0108 | 0:76fdba038ca7 | 23 | { |
yuki0108 | 0:76fdba038ca7 | 24 | delta_t = delta_time; |
yuki0108 | 0:76fdba038ca7 | 25 | } |
yuki0108 | 0:76fdba038ca7 | 26 | double CalPIDF::calPID(double devia_present) |
yuki0108 | 0:76fdba038ca7 | 27 | { |
yuki0108 | 0:76fdba038ca7 | 28 | double differential = (deviation_old - devia_present) / delta_t; |
yuki0108 | 0:76fdba038ca7 | 29 | if(value_PID!=max_pid&&value_PID!=-max_pid) {//アンチワインドアップ(積分項固定式) |
yuki0108 | 0:76fdba038ca7 | 30 | integral += (devia_present + deviation_old) * delta_t / 2; |
yuki0108 | 0:76fdba038ca7 | 31 | } |
yuki0108 | 0:76fdba038ca7 | 32 | value_PID = kp * devia_present + ki * integral - kd * differential; |
yuki0108 | 0:76fdba038ca7 | 33 | |
yuki0108 | 0:76fdba038ca7 | 34 | if (value_PID > max_pid) { |
yuki0108 | 0:76fdba038ca7 | 35 | value_PID = max_pid; |
yuki0108 | 0:76fdba038ca7 | 36 | } else if (value_PID < -max_pid) { |
yuki0108 | 0:76fdba038ca7 | 37 | value_PID = -max_pid; |
yuki0108 | 0:76fdba038ca7 | 38 | } |
yuki0108 | 0:76fdba038ca7 | 39 | |
yuki0108 | 0:76fdba038ca7 | 40 | deviation_old = devia_present; |
yuki0108 | 0:76fdba038ca7 | 41 | |
yuki0108 | 0:76fdba038ca7 | 42 | return value_PID; |
yuki0108 | 0:76fdba038ca7 | 43 | } |
yuki0108 | 0:76fdba038ca7 | 44 | double CalPIDF::calPD(double devia_present) |
yuki0108 | 0:76fdba038ca7 | 45 | { |
yuki0108 | 0:76fdba038ca7 | 46 | double differential = (deviation_old - devia_present) / delta_t; |
yuki0108 | 0:76fdba038ca7 | 47 | double value_PD = kp * devia_present - kd * differential; |
yuki0108 | 0:76fdba038ca7 | 48 | |
yuki0108 | 0:76fdba038ca7 | 49 | if (value_PD > max_pid) { |
yuki0108 | 0:76fdba038ca7 | 50 | value_PD = max_pid; |
yuki0108 | 0:76fdba038ca7 | 51 | } else if (value_PD < -max_pid) { |
yuki0108 | 0:76fdba038ca7 | 52 | value_PD = -max_pid; |
yuki0108 | 0:76fdba038ca7 | 53 | } |
yuki0108 | 0:76fdba038ca7 | 54 | |
yuki0108 | 0:76fdba038ca7 | 55 | deviation_old = devia_present; |
yuki0108 | 0:76fdba038ca7 | 56 | |
yuki0108 | 0:76fdba038ca7 | 57 | return value_PD; |
yuki0108 | 0:76fdba038ca7 | 58 | } |
yuki0108 | 0:76fdba038ca7 | 59 | double CalPIDF::calP_D(double devia_present,double diff_value) |
yuki0108 | 0:76fdba038ca7 | 60 | { |
yuki0108 | 0:76fdba038ca7 | 61 | double value_P_D = kp * devia_present - kd * diff_value; |
yuki0108 | 0:76fdba038ca7 | 62 | |
yuki0108 | 0:76fdba038ca7 | 63 | if (value_P_D > max_pid) { |
yuki0108 | 0:76fdba038ca7 | 64 | value_P_D = max_pid; |
yuki0108 | 0:76fdba038ca7 | 65 | } else if (value_P_D < -max_pid) { |
yuki0108 | 0:76fdba038ca7 | 66 | value_P_D = -max_pid; |
yuki0108 | 0:76fdba038ca7 | 67 | } |
yuki0108 | 0:76fdba038ca7 | 68 | |
yuki0108 | 0:76fdba038ca7 | 69 | return value_P_D; |
yuki0108 | 0:76fdba038ca7 | 70 | } |
yuki0108 | 0:76fdba038ca7 | 71 | void CalPIDF::resetIntegral() |
yuki0108 | 0:76fdba038ca7 | 72 | { |
yuki0108 | 0:76fdba038ca7 | 73 | integral = 0; |
yuki0108 | 0:76fdba038ca7 | 74 | } |