ryo shatari / CalPID1210

aaa

CalPID.cpp@1:ce6f0674ab0d, 2021-03-18 (annotated)

Committer:
yuki0108
Date:
Thu Mar 18 12:23:00 2021 +0000
Revision:
1:ce6f0674ab0d
Parent:
CalPIDF.cpp@0:76fdba038ca7
Child:
3:634166991b10
3/18

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:76fdba038ca7 1 #include "mbed.h"
yuki0108 1:ce6f0674ab0d 2 #include "CalPID.h"
yuki0108 0:76fdba038ca7 3
yuki0108 1:ce6f0674ab0d 4 CalPID::CalPID(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 1:ce6f0674ab0d 12 void CalPID::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 1:ce6f0674ab0d 18 void CalPID::setMaxValue(double max)
yuki0108 0:76fdba038ca7 19 {
yuki0108 0:76fdba038ca7 20 max_pid = max;
yuki0108 0:76fdba038ca7 21 }
yuki0108 1:ce6f0674ab0d 22 void CalPID::setDELTA_T(double delta_time)
yuki0108 0:76fdba038ca7 23 {
yuki0108 0:76fdba038ca7 24 delta_t = delta_time;
yuki0108 0:76fdba038ca7 25 }
yuki0108 1:ce6f0674ab0d 26 double CalPID::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 1:ce6f0674ab0d 44 double CalPID::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 1:ce6f0674ab0d 59 double CalPID::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 1:ce6f0674ab0d 71 void CalPID::resetIntegral()
yuki0108 0:76fdba038ca7 72 {
yuki0108 0:76fdba038ca7 73 integral = 0;
yuki0108 0:76fdba038ca7 74 }