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@0:be531c5604db, 2019-01-11 (annotated)

Committer:
TakumiToda
Date:
Fri Jan 11 08:00:51 2019 +0000
Revision:
0:be531c5604db
Child:
1:4705d8930670
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
TakumiToda 0:be531c5604db 4 PID::PID(){
TakumiToda 0:be531c5604db 5 integral = 0;
TakumiToda 0:be531c5604db 6 prev_hensa = 0;
TakumiToda 0:be531c5604db 7 nowtime = 0;
TakumiToda 0:be531c5604db 8 prev_time = 0;
TakumiToda 0:be531c5604db 9 lateD = 0;
TakumiToda 0:be531c5604db 10 }
TakumiToda 0:be531c5604db 11
TakumiToda 0:be531c5604db 12 float PID::control(float target, float nowrpm, Timer *timer){
TakumiToda 0:be531c5604db 13 //timer->start();
TakumiToda 0:be531c5604db 14 nowtime =timer->read();
TakumiToda 0:be531c5604db 15 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 16 float dt = 1000000 / (nowtime - prev_time);
TakumiToda 0:be531c5604db 17 integral += (hensa + prev_hensa) / 2 * dt;
TakumiToda 0:be531c5604db 18 float differential = (hensa - prev_hensa) / dt;
TakumiToda 0:be531c5604db 19 float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
TakumiToda 0:be531c5604db 20 prev_hensa = hensa;
TakumiToda 0:be531c5604db 21 prev_time =timer->read();
TakumiToda 0:be531c5604db 22 return sousaryou;
TakumiToda 0:be531c5604db 23 }
TakumiToda 0:be531c5604db 24
TakumiToda 0:be531c5604db 25 float PID::PI_lateD(float target, float nowrpm, Timer *timer){
TakumiToda 0:be531c5604db 26 nowtime = (*timer).read();
TakumiToda 0:be531c5604db 27 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 28 float dt = nowtime - prev_time;
TakumiToda 0:be531c5604db 29 integral += (hensa + prev_hensa) / 2 *dt;
TakumiToda 0:be531c5604db 30 float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
TakumiToda 0:be531c5604db 31 prev_hensa = hensa;
TakumiToda 0:be531c5604db 32 prev_time = timer->read();
TakumiToda 0:be531c5604db 33 lateD = (hensa - prev_hensa) / dt;
TakumiToda 0:be531c5604db 34 return sousaryou;
TakumiToda 0:be531c5604db 35 }
TakumiToda 0:be531c5604db 36
TakumiToda 0:be531c5604db 37 float PID::control_P(float target, float nowrpm, float new_Kp){
TakumiToda 0:be531c5604db 38 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 39 float sousaryou = new_Kp*hensa;
TakumiToda 0:be531c5604db 40 return sousaryou;
TakumiToda 0:be531c5604db 41 }
TakumiToda 0:be531c5604db 42
TakumiToda 0:be531c5604db 43 float PID::control_PI(float target, float nowrpm, Timer *timer){
TakumiToda 0:be531c5604db 44 Kp = 0.45 * Ku;
TakumiToda 0:be531c5604db 45 Ti = 0.83 * Pu;
TakumiToda 0:be531c5604db 46 Ki = (1 / Ti) * Kp;
TakumiToda 0:be531c5604db 47 nowtime = timer->read();
TakumiToda 0:be531c5604db 48 float hensa = target - nowrpm;
TakumiToda 0:be531c5604db 49 float dt = nowtime -prev_time;
TakumiToda 0:be531c5604db 50 integral += (hensa + prev_hensa) / 2 * dt;
TakumiToda 0:be531c5604db 51 float sousaryou = Kp*hensa + Ki*integral;
TakumiToda 0:be531c5604db 52 prev_hensa = hensa;
TakumiToda 0:be531c5604db 53 prev_time = timer->read();
TakumiToda 0:be531c5604db 54 return sousaryou;
TakumiToda 0:be531c5604db 55 }
TakumiToda 0:be531c5604db 56
TakumiToda 0:be531c5604db 57 void PID::setParameter_pid(float new_Kp, float new_Ki, float new_Kd){
TakumiToda 0:be531c5604db 58 Kp = new_Kp;
TakumiToda 0:be531c5604db 59 Ki = new_Ki;
TakumiToda 0:be531c5604db 60 Kd = new_Kd;
TakumiToda 0:be531c5604db 61 }
TakumiToda 0:be531c5604db 62
TakumiToda 0:be531c5604db 63 void PID::setParameter_KuPu(float new_Ku, float new_Pu){
TakumiToda 0:be531c5604db 64 Ku = new_Ku;
TakumiToda 0:be531c5604db 65 Pu = new_Pu;
TakumiToda 0:be531c5604db 66 Kp = 0.60 * Ku;
TakumiToda 0:be531c5604db 67 Ti = 0.50 * Pu;
TakumiToda 0:be531c5604db 68 Td = 0.125 * Pu;
TakumiToda 0:be531c5604db 69 Ki = (1 / Ti) * Kp;
TakumiToda 0:be531c5604db 70 Kd = Td * Kp;
TakumiToda 0:be531c5604db 71 }
TakumiToda 0:be531c5604db 72
TakumiToda 0:be531c5604db 73 void PID::reset(float target){
TakumiToda 0:be531c5604db 74 integral = 0;
TakumiToda 0:be531c5604db 75 prev_hensa = target;
TakumiToda 0:be531c5604db 76 prev_time = timer->read();
TakumiToda 0:be531c5604db 77 }