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@3:c690a8974246, 2019-04-15 (annotated)

Committer:
Takkun
Date:
Mon Apr 15 05:23:54 2019 +0000
Revision:
3:c690a8974246
Parent:
2:c8ab3e8d4c51 
add vec_PID;

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();
Takkun 3:c690a8974246 19 derror[0]=0;
Takkun 3:c690a8974246 20 derror[1]=0;
TakumiToda 0:be531c5604db 21 }
TakumiToda 0:be531c5604db 22
Takkun 2:c8ab3e8d4c51 23 void PID::control()
Takkun 2:c8ab3e8d4c51 24 {
Takkun 2:c8ab3e8d4c51 25 //timer->start();
Takkun 2:c8ab3e8d4c51 26 double nowtime =timer->read_ms();
Takkun 2:c8ab3e8d4c51 27 double input = *Input;
Takkun 2:c8ab3e8d4c51 28
Takkun 2:c8ab3e8d4c51 29 if((nowtime - lastTime) > SampleTime)
Takkun 2:c8ab3e8d4c51 30 {
Takkun 2:c8ab3e8d4c51 31 double error = *Setpoint - input;
Takkun 2:c8ab3e8d4c51 32 double dInput = input - lastInput;
Takkun 3:c690a8974246 33 derror[0] = error - lastError;
Takkun 2:c8ab3e8d4c51 34 outputSum += (Ki*error);
Takkun 2:c8ab3e8d4c51 35 double output = 0;
Takkun 2:c8ab3e8d4c51 36
Takkun 2:c8ab3e8d4c51 37 if(outputSum > outMax) outputSum= outMax;
Takkun 2:c8ab3e8d4c51 38 else if(outputSum < outMin) outputSum= outMin;
Takkun 2:c8ab3e8d4c51 39
Takkun 2:c8ab3e8d4c51 40 switch(mode)
Takkun 2:c8ab3e8d4c51 41 {
Takkun 2:c8ab3e8d4c51 42 case pos_PID:
Takkun 3:c690a8974246 43 output = outputSum + Kp*error + Kd*derror[0];
Takkun 2:c8ab3e8d4c51 44 break;
Takkun 2:c8ab3e8d4c51 45
Takkun 3:c690a8974246 46 case vec_PID:
Takkun 3:c690a8974246 47 output = *Output + Kp*derror[0] + Kd*error + Ki*(derror[0] - derror[1]);
Takkun 3:c690a8974246 48
Takkun 2:c8ab3e8d4c51 49 case P_D:
Takkun 2:c8ab3e8d4c51 50 output = Kp*error - Kd*dInput;
Takkun 2:c8ab3e8d4c51 51 break;
Takkun 2:c8ab3e8d4c51 52
Takkun 2:c8ab3e8d4c51 53 case PI_D:
Takkun 2:c8ab3e8d4c51 54 output = outputSum + Kp*error - Kd*dInput;
Takkun 2:c8ab3e8d4c51 55 break;
Takkun 2:c8ab3e8d4c51 56
Takkun 2:c8ab3e8d4c51 57 case I_PD:
Takkun 2:c8ab3e8d4c51 58 output = outputSum - Kp*input - Kd*dInput;
Takkun 2:c8ab3e8d4c51 59 break;
Takkun 2:c8ab3e8d4c51 60
Takkun 2:c8ab3e8d4c51 61 }
Takkun 3:c690a8974246 62 derror[1] = derror[0];
Takkun 2:c8ab3e8d4c51 63 *Output = output;
Takkun 2:c8ab3e8d4c51 64 lastTime = nowtime;
Takkun 2:c8ab3e8d4c51 65 lastInput = input;
Takkun 2:c8ab3e8d4c51 66 }
TakumiToda 0:be531c5604db 67 }
TakumiToda 0:be531c5604db 68
Takkun 2:c8ab3e8d4c51 69 void PID::setParameter_pid(double new_Kp, double new_Ki, double new_Kd)
Takkun 2:c8ab3e8d4c51 70 {
Takkun 2:c8ab3e8d4c51 71 Kp = new_Kp;
Takkun 2:c8ab3e8d4c51 72 Ki = new_Ki;
Takkun 2:c8ab3e8d4c51 73 Kd = new_Kd;
Takkun 2:c8ab3e8d4c51 74 }
Takkun 2:c8ab3e8d4c51 75
Takkun 2:c8ab3e8d4c51 76 void PID::setParameter_KuPu(double new_Ku, double new_Pu)
Takkun 2:c8ab3e8d4c51 77 {
Takkun 2:c8ab3e8d4c51 78 Ku = new_Ku;
Takkun 2:c8ab3e8d4c51 79 Pu = new_Pu;
Takkun 2:c8ab3e8d4c51 80 Kp = 0.60 * Ku;
Takkun 2:c8ab3e8d4c51 81 Ti = 0.50 * Pu;
Takkun 2:c8ab3e8d4c51 82 Td = 0.125 * Pu;
Takkun 2:c8ab3e8d4c51 83 Ki = (1 / Ti) * Kp;
Takkun 2:c8ab3e8d4c51 84 Kd = Td * Kp;
Takkun 2:c8ab3e8d4c51 85 }
Takkun 2:c8ab3e8d4c51 86
Takkun 2:c8ab3e8d4c51 87 void PID::setPIDMode(PID_Mode _mode)
Takkun 2:c8ab3e8d4c51 88 {
Takkun 2:c8ab3e8d4c51 89 mode = _mode;
Takkun 2:c8ab3e8d4c51 90 Initialize();
TakumiToda 0:be531c5604db 91 }
TakumiToda 0:be531c5604db 92
Takkun 2:c8ab3e8d4c51 93 void PID::setOutputLimits(double Min, double Max)
Takkun 2:c8ab3e8d4c51 94 {
Takkun 2:c8ab3e8d4c51 95 if(Min >= Max) return;
Takkun 2:c8ab3e8d4c51 96 outMin = Min;
Takkun 2:c8ab3e8d4c51 97 outMax = Max;
Takkun 2:c8ab3e8d4c51 98
Takkun 2:c8ab3e8d4c51 99 if(*Output > outMax) *Output = outMax;
Takkun 2:c8ab3e8d4c51 100 else if(*Output < outMin) *Output = outMin;
Takkun 2:c8ab3e8d4c51 101 if(outputSum > outMax) outputSum = outMax;
Takkun 2:c8ab3e8d4c51 102 else if(outputSum < outMin) outputSum = outMin;
TakumiToda 0:be531c5604db 103 }
TakumiToda 0:be531c5604db 104
Takkun 2:c8ab3e8d4c51 105 void PID::reset(double target)
Takkun 2:c8ab3e8d4c51 106 {
Takkun 2:c8ab3e8d4c51 107 outputSum = 0;
TakumiToda 0:be531c5604db 108 }
Takkun 2:c8ab3e8d4c51 109
Takkun 2:c8ab3e8d4c51 110 void PID::Initialize()
Takkun 2:c8ab3e8d4c51 111 {
Takkun 2:c8ab3e8d4c51 112 outputSum = *Output;
Takkun 2:c8ab3e8d4c51 113 lastInput = *Input;
Takkun 2:c8ab3e8d4c51 114 lastError = *Setpoint - lastInput;
Takkun 2:c8ab3e8d4c51 115 if(outputSum > outMax) outputSum = outMax;
Takkun 2:c8ab3e8d4c51 116 else if(outputSum < outMin) outputSum = outMin;
Takkun 2:c8ab3e8d4c51 117 }