Takumi Nakagawara / PID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

PID.cpp@2:c8ab3e8d4c51, 2019-04-14 (annotated)

Committer:
Takkun
Date:
Sun Apr 14 14:03:46 2019 +0000
Revision:
2:c8ab3e8d4c51
Parent:
1:4705d8930670
Child:
3:c690a8974246
PID library create;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TakumiToda 0:be531c5604db 1 #include "PID.h"
TakumiToda 0:be531c5604db 2 #include "mbed.h"
TakumiToda 0:be531c5604db 3
Takkun 2:c8ab3e8d4c51 4 PID::PID(double* _Input, double* _Output, double* _Setpoint, double _Kp, double _Ki, double _Kd, PID_Mode _mode, Timer *_timer)
Takkun 2:c8ab3e8d4c51 5 {
Takkun 2:c8ab3e8d4c51 6
Takkun 2:c8ab3e8d4c51 7 Input = _Input;
Takkun 2:c8ab3e8d4c51 8 Output = _Output;
Takkun 2:c8ab3e8d4c51 9 Setpoint = _Setpoint;
TakumiToda 0:be531c5604db 10
Takkun 2:c8ab3e8d4c51 11 timer = _timer;
Takkun 2:c8ab3e8d4c51 12
Takkun 2:c8ab3e8d4c51 13 SampleTime = 100;
Takkun 2:c8ab3e8d4c51 14 lastTime = timer->read_ms();
Takkun 2:c8ab3e8d4c51 15 setOutputLimits(-1,1);
Takkun 2:c8ab3e8d4c51 16 setParameter_pid(_Kp,_Ki,_Kd);
Takkun 2:c8ab3e8d4c51 17 setPIDMode(_mode);
Takkun 2:c8ab3e8d4c51 18 Initialize();
TakumiToda 0:be531c5604db 19 }
TakumiToda 0:be531c5604db 20
Takkun 2:c8ab3e8d4c51 21 void PID::control()
Takkun 2:c8ab3e8d4c51 22 {
Takkun 2:c8ab3e8d4c51 23 //timer->start();
Takkun 2:c8ab3e8d4c51 24 double nowtime =timer->read_ms();
Takkun 2:c8ab3e8d4c51 25 double input = *Input;
Takkun 2:c8ab3e8d4c51 26
Takkun 2:c8ab3e8d4c51 27 if((nowtime - lastTime) > SampleTime)
Takkun 2:c8ab3e8d4c51 28 {
Takkun 2:c8ab3e8d4c51 29 double error = *Setpoint - input;
Takkun 2:c8ab3e8d4c51 30 double dInput = input - lastInput;
Takkun 2:c8ab3e8d4c51 31 double derror = error - lastError;
Takkun 2:c8ab3e8d4c51 32 outputSum += (Ki*error);
Takkun 2:c8ab3e8d4c51 33 double output = 0;
Takkun 2:c8ab3e8d4c51 34
Takkun 2:c8ab3e8d4c51 35 if(outputSum > outMax) outputSum= outMax;
Takkun 2:c8ab3e8d4c51 36 else if(outputSum < outMin) outputSum= outMin;
Takkun 2:c8ab3e8d4c51 37
Takkun 2:c8ab3e8d4c51 38 switch(mode)
Takkun 2:c8ab3e8d4c51 39 {
Takkun 2:c8ab3e8d4c51 40 case pos_PID:
Takkun 2:c8ab3e8d4c51 41 output = outputSum + Kp*error + Kd*derror;
Takkun 2:c8ab3e8d4c51 42 break;
Takkun 2:c8ab3e8d4c51 43
Takkun 2:c8ab3e8d4c51 44 case P_D:
Takkun 2:c8ab3e8d4c51 45 output = Kp*error - Kd*dInput;
Takkun 2:c8ab3e8d4c51 46 break;
Takkun 2:c8ab3e8d4c51 47
Takkun 2:c8ab3e8d4c51 48 case PI_D:
Takkun 2:c8ab3e8d4c51 49 output = outputSum + Kp*error - Kd*dInput;
Takkun 2:c8ab3e8d4c51 50 break;
Takkun 2:c8ab3e8d4c51 51
Takkun 2:c8ab3e8d4c51 52 case I_PD:
Takkun 2:c8ab3e8d4c51 53 output = outputSum - Kp*input - Kd*dInput;
Takkun 2:c8ab3e8d4c51 54 break;
Takkun 2:c8ab3e8d4c51 55
Takkun 2:c8ab3e8d4c51 56 }
Takkun 2:c8ab3e8d4c51 57
Takkun 2:c8ab3e8d4c51 58 *Output = output;
Takkun 2:c8ab3e8d4c51 59 lastTime = nowtime;
Takkun 2:c8ab3e8d4c51 60 lastInput = input;
Takkun 2:c8ab3e8d4c51 61 }
TakumiToda 0:be531c5604db 62 }
TakumiToda 0:be531c5604db 63
Takkun 2:c8ab3e8d4c51 64 void PID::setParameter_pid(double new_Kp, double new_Ki, double new_Kd)
Takkun 2:c8ab3e8d4c51 65 {
Takkun 2:c8ab3e8d4c51 66 Kp = new_Kp;
Takkun 2:c8ab3e8d4c51 67 Ki = new_Ki;
Takkun 2:c8ab3e8d4c51 68 Kd = new_Kd;
Takkun 2:c8ab3e8d4c51 69 }
Takkun 2:c8ab3e8d4c51 70
Takkun 2:c8ab3e8d4c51 71 void PID::setParameter_KuPu(double new_Ku, double new_Pu)
Takkun 2:c8ab3e8d4c51 72 {
Takkun 2:c8ab3e8d4c51 73 Ku = new_Ku;
Takkun 2:c8ab3e8d4c51 74 Pu = new_Pu;
Takkun 2:c8ab3e8d4c51 75 Kp = 0.60 * Ku;
Takkun 2:c8ab3e8d4c51 76 Ti = 0.50 * Pu;
Takkun 2:c8ab3e8d4c51 77 Td = 0.125 * Pu;
Takkun 2:c8ab3e8d4c51 78 Ki = (1 / Ti) * Kp;
Takkun 2:c8ab3e8d4c51 79 Kd = Td * Kp;
Takkun 2:c8ab3e8d4c51 80 }
Takkun 2:c8ab3e8d4c51 81
Takkun 2:c8ab3e8d4c51 82 void PID::setPIDMode(PID_Mode _mode)
Takkun 2:c8ab3e8d4c51 83 {
Takkun 2:c8ab3e8d4c51 84 mode = _mode;
Takkun 2:c8ab3e8d4c51 85 Initialize();
TakumiToda 0:be531c5604db 86 }
TakumiToda 0:be531c5604db 87
Takkun 2:c8ab3e8d4c51 88 void PID::setOutputLimits(double Min, double Max)
Takkun 2:c8ab3e8d4c51 89 {
Takkun 2:c8ab3e8d4c51 90 if(Min >= Max) return;
Takkun 2:c8ab3e8d4c51 91 outMin = Min;
Takkun 2:c8ab3e8d4c51 92 outMax = Max;
Takkun 2:c8ab3e8d4c51 93
Takkun 2:c8ab3e8d4c51 94 if(*Output > outMax) *Output = outMax;
Takkun 2:c8ab3e8d4c51 95 else if(*Output < outMin) *Output = outMin;
Takkun 2:c8ab3e8d4c51 96 if(outputSum > outMax) outputSum = outMax;
Takkun 2:c8ab3e8d4c51 97 else if(outputSum < outMin) outputSum = outMin;
TakumiToda 0:be531c5604db 98 }
TakumiToda 0:be531c5604db 99
Takkun 2:c8ab3e8d4c51 100 void PID::reset(double target)
Takkun 2:c8ab3e8d4c51 101 {
Takkun 2:c8ab3e8d4c51 102 outputSum = 0;
TakumiToda 0:be531c5604db 103 }
Takkun 2:c8ab3e8d4c51 104
Takkun 2:c8ab3e8d4c51 105 void PID::Initialize()
Takkun 2:c8ab3e8d4c51 106 {
Takkun 2:c8ab3e8d4c51 107 outputSum = *Output;
Takkun 2:c8ab3e8d4c51 108 lastInput = *Input;
Takkun 2:c8ab3e8d4c51 109 lastError = *Setpoint - lastInput;
Takkun 2:c8ab3e8d4c51 110 if(outputSum > outMax) outputSum = outMax;
Takkun 2:c8ab3e8d4c51 111 else if(outputSum < outMin) outputSum = outMin;
Takkun 2:c8ab3e8d4c51 112 }