ichinoseki_Bteam_2019 / 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.

Dependents:   MR_example 2019_AR_Itsuki

PID.cpp@4:64ae90ccf8d8, 2018-09-11 (annotated)

Committer:
soyooo
Date:
Tue Sep 11 01:33:20 2018 +0000
Revision:
4:64ae90ccf8d8
Parent:
2:493370c3e206
Child:
5:ceb7cbed26f3
????????????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
soyooo 0:da83c97448bf 1 #include "mbed.h"
soyooo 0:da83c97448bf 2 #include "PID.h"
soyooo 0:da83c97448bf 3
soyooo 0:da83c97448bf 4 /*p,i,d:ゲイン設定　t:制御ループ時間 max:ourputの最大値*/
soyooo 2:493370c3e206 5 PID::PID(float p, float i, float d, float t, float max, Timer *T)
soyooo 0:da83c97448bf 6 {
soyooo 2:493370c3e206 7 timer = T;
soyooo 0:da83c97448bf 8 kp = p; ki = i; kd = d; delta_t = t;
soyooo 0:da83c97448bf 9 abs_max_output = max;
soyooo 0:da83c97448bf 10 integral = 0;
soyooo 0:da83c97448bf 11 pid_type = PID_LOCATE;
soyooo 2:493370c3e206 12 allowable_error = 0;
soyooo 2:493370c3e206 13
soyooo 0:da83c97448bf 14 }
soyooo 0:da83c97448bf 15 void PID::setParameter(float p, float i, float d, float t, float max)
soyooo 0:da83c97448bf 16 {
soyooo 0:da83c97448bf 17 kp = p; ki = i; kd = d; delta_t = t;
soyooo 0:da83c97448bf 18 abs_max_output = max;
soyooo 0:da83c97448bf 19 integral = 0;
soyooo 0:da83c97448bf 20 }
soyooo 0:da83c97448bf 21
soyooo 0:da83c97448bf 22 /*計算のための割り込み開始*/
soyooo 0:da83c97448bf 23 void PID::start()
soyooo 0:da83c97448bf 24 {
soyooo 0:da83c97448bf 25 pidTimer.attach(this, &PID::_compute, delta_t);
soyooo 0:da83c97448bf 26 }
soyooo 0:da83c97448bf 27
soyooo 0:da83c97448bf 28 /*計算のための割り込み停止*/
soyooo 0:da83c97448bf 29 void PID::stop()
soyooo 0:da83c97448bf 30 {
soyooo 1:4906167d1263 31 reset();
soyooo 0:da83c97448bf 32 pidTimer.detach();
soyooo 0:da83c97448bf 33 }
soyooo 0:da83c97448bf 34
soyooo 0:da83c97448bf 35 /*現在保持している計算データをリセット*/
soyooo 0:da83c97448bf 36 void PID::reset()
soyooo 0:da83c97448bf 37 {
soyooo 0:da83c97448bf 38 integral = 0;
soyooo 0:da83c97448bf 39 error[0] = 0;
soyooo 0:da83c97448bf 40 error[1] = 0;
soyooo 0:da83c97448bf 41 }
soyooo 0:da83c97448bf 42
soyooo 0:da83c97448bf 43 /*計算する　タイマーで回される*/
soyooo 0:da83c97448bf 44 void PID::_compute()
soyooo 0:da83c97448bf 45 {
soyooo 0:da83c97448bf 46 float proportion, differential;
soyooo 0:da83c97448bf 47 static float _output = 0;
soyooo 0:da83c97448bf 48
soyooo 0:da83c97448bf 49 error[0] = *target - *sensor;
soyooo 0:da83c97448bf 50
soyooo 0:da83c97448bf 51 proportion = kp * error[0];
soyooo 0:da83c97448bf 52 integral += ki * error[0] * delta_t;
soyooo 0:da83c97448bf 53 differential = kd * (error[0] - error[1]) / delta_t;
soyooo 0:da83c97448bf 54
soyooo 0:da83c97448bf 55 error[1] = error[0];
soyooo 0:da83c97448bf 56
soyooo 0:da83c97448bf 57 integral = _gurd(integral, abs_max_output);
soyooo 2:493370c3e206 58
soyooo 0:da83c97448bf 59 if(pid_type == PID_LOCATE)
soyooo 0:da83c97448bf 60 _output = proportion + integral + differential;
soyooo 0:da83c97448bf 61 else if(pid_type == PID_SPEED)
soyooo 0:da83c97448bf 62 _output += proportion + integral + differential;
soyooo 0:da83c97448bf 63 else _output = 0;
soyooo 0:da83c97448bf 64
soyooo 0:da83c97448bf 65 _output = _gurd(_output, abs_max_output);
soyooo 0:da83c97448bf 66 output = _output;
soyooo 2:493370c3e206 67
soyooo 4:64ae90ccf8d8 68 last_target = *target;
soyooo 4:64ae90ccf8d8 69
soyooo 2:493370c3e206 70 if(_abs(error[0]) <= allowable_error)
soyooo 2:493370c3e206 71 timer->start();
soyooo 2:493370c3e206 72 else start_time = timer->read();
soyooo 0:da83c97448bf 73 }
soyooo 0:da83c97448bf 74
soyooo 0:da83c97448bf 75 float PID::_gurd(float val, float abs_max)
soyooo 0:da83c97448bf 76 {
soyooo 0:da83c97448bf 77 if(val > abs_max)
soyooo 0:da83c97448bf 78 return abs_max;
soyooo 0:da83c97448bf 79 else if(val < -abs_max)
soyooo 0:da83c97448bf 80 return -abs_max;
soyooo 0:da83c97448bf 81 else return val;
soyooo 0:da83c97448bf 82 }
soyooo 2:493370c3e206 83 float PID::_abs(float val)
soyooo 2:493370c3e206 84 {
soyooo 2:493370c3e206 85 if(val >= 0)
soyooo 2:493370c3e206 86 return val;
soyooo 2:493370c3e206 87 else return -val;
soyooo 2:493370c3e206 88 }
soyooo 2:493370c3e206 89
soyooo 2:493370c3e206 90 bool PID::isConvergence(float time_range)
soyooo 4:64ae90ccf8d8 91 {
soyooo 4:64ae90ccf8d8 92 if(last_target != *target)
soyooo 4:64ae90ccf8d8 93 return 0;
soyooo 2:493370c3e206 94 if(timer->read() - start_time > time_range)
soyooo 2:493370c3e206 95 return 1;
soyooo 2:493370c3e206 96 else return 0;
soyooo 2:493370c3e206 97 }