ryo shatari / CalPID1210

aaa

CalPID.cpp@3:634166991b10, 2021-12-10 (annotated)

Committer:
shatari
Date:
Fri Dec 10 14:02:27 2021 +0000
Revision:
3:634166991b10
Parent:
1:ce6f0674ab0d 
a

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
shatari 3:634166991b10 42 value_PID = present_parameter;
yuki0108 0:76fdba038ca7 43 return value_PID;
yuki0108 0:76fdba038ca7 44 }
shatari 3:634166991b10 45
yuki0108 1:ce6f0674ab0d 46 double CalPID::calPD(double devia_present)
yuki0108 0:76fdba038ca7 47 {
yuki0108 0:76fdba038ca7 48 double differential = (deviation_old - devia_present) / delta_t;
yuki0108 0:76fdba038ca7 49 double value_PD = kp * devia_present - kd * differential;
yuki0108 0:76fdba038ca7 50
yuki0108 0:76fdba038ca7 51 if (value_PD > max_pid) {
yuki0108 0:76fdba038ca7 52 value_PD = max_pid;
yuki0108 0:76fdba038ca7 53 } else if (value_PD < -max_pid) {
yuki0108 0:76fdba038ca7 54 value_PD = -max_pid;
yuki0108 0:76fdba038ca7 55 }
yuki0108 0:76fdba038ca7 56
yuki0108 0:76fdba038ca7 57 deviation_old = devia_present;
yuki0108 0:76fdba038ca7 58
yuki0108 0:76fdba038ca7 59 return value_PD;
yuki0108 0:76fdba038ca7 60 }
yuki0108 1:ce6f0674ab0d 61 double CalPID::calP_D(double devia_present,double diff_value)
yuki0108 0:76fdba038ca7 62 {
yuki0108 0:76fdba038ca7 63 double value_P_D = kp * devia_present - kd * diff_value;
yuki0108 0:76fdba038ca7 64
yuki0108 0:76fdba038ca7 65 if (value_P_D > max_pid) {
yuki0108 0:76fdba038ca7 66 value_P_D = max_pid;
yuki0108 0:76fdba038ca7 67 } else if (value_P_D < -max_pid) {
yuki0108 0:76fdba038ca7 68 value_P_D = -max_pid;
yuki0108 0:76fdba038ca7 69 }
yuki0108 0:76fdba038ca7 70
yuki0108 0:76fdba038ca7 71 return value_P_D;
yuki0108 0:76fdba038ca7 72 }
yuki0108 1:ce6f0674ab0d 73 void CalPID::resetIntegral()
yuki0108 0:76fdba038ca7 74 {
yuki0108 0:76fdba038ca7 75 integral = 0;
yuki0108 0:76fdba038ca7 76 }