Yuta Togashi / PID

PID用ライブラリ ※位置型PIDと速度型PIDの選択式

Dependents:   Tourobo2022_TBCMotorDriver

Pid.cpp@2:7fede27af6ca, 2019-09-10 (annotated)

Committer:
YutaTogashi
Date:
Tue Sep 10 13:55:06 2019 +0000
Revision:
2:7fede27af6ca
Parent:
1:4bc4c63ea283
Child:
3:01ae834a066d
Child:
4:3c2651359136
20190910

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YutaTogashi 0:630126b8994f 1 #include "Pid.h"
YutaTogashi 0:630126b8994f 2
YutaTogashi 0:630126b8994f 3 void Pid::setup(float Kp,float Ki,float Kd,short PidMode,float period) {
YutaTogashi 0:630126b8994f 4 KP = Kp;
YutaTogashi 0:630126b8994f 5 KI = Ki;
YutaTogashi 0:630126b8994f 6 KD = Kd;
YutaTogashi 0:630126b8994f 7 MODE = PidMode;
YutaTogashi 0:630126b8994f 8 PERIOD = period;
YutaTogashi 0:630126b8994f 9 //pid.attach(this,&Pid::calculate,period);
YutaTogashi 0:630126b8994f 10 }
YutaTogashi 0:630126b8994f 11
YutaTogashi 2:7fede27af6ca 12 void Pid::calculate(float targetValue,float nowValue,bool enableErrorIntegration) {
YutaTogashi 0:630126b8994f 13 switch(MODE) {
YutaTogashi 0:630126b8994f 14 case 0:
YutaTogashi 0:630126b8994f 15 before = now;
YutaTogashi 0:630126b8994f 16 now = targetValue - nowValue;
YutaTogashi 0:630126b8994f 17 p = now;
YutaTogashi 2:7fede27af6ca 18 if(enableErrorIntegration) i += (now + before) * PERIOD / 2; //積分
YutaTogashi 2:7fede27af6ca 19 else i = 0.0f;
YutaTogashi 2:7fede27af6ca 20 d = (now - before) / PERIOD; //微分
YutaTogashi 0:630126b8994f 21 //target_duty = p * KP + i * KI + d * KD;
YutaTogashi 0:630126b8994f 22 duty = p * KP + i * KI + d * KD;
YutaTogashi 0:630126b8994f 23 break;
YutaTogashi 0:630126b8994f 24 case 1:
YutaTogashi 0:630126b8994f 25 e2 = e1;
YutaTogashi 0:630126b8994f 26 e1 = e;
YutaTogashi 0:630126b8994f 27 e = targetValue - nowValue;
YutaTogashi 0:630126b8994f 28 //p = (e - e1) / PERIOD;
YutaTogashi 0:630126b8994f 29 p = e / PERIOD;
YutaTogashi 0:630126b8994f 30 i = e;
YutaTogashi 0:630126b8994f 31 d = (e - 2*e1 + e2)/PERIOD;
YutaTogashi 0:630126b8994f 32 duty += p * KP + i * KI + d * KD; //dutyは微分されたものなので、積分してあげる(もとに戻す)
YutaTogashi 0:630126b8994f 33 break;
YutaTogashi 0:630126b8994f 34 default:
YutaTogashi 0:630126b8994f 35 break;
YutaTogashi 0:630126b8994f 36 }
YutaTogashi 0:630126b8994f 37
YutaTogashi 1:4bc4c63ea283 38 if(duty > 1.0f) {
YutaTogashi 1:4bc4c63ea283 39 duty = 1.0f;
YutaTogashi 1:4bc4c63ea283 40 } else if (duty < -1.0f) {
YutaTogashi 1:4bc4c63ea283 41 duty = -1.0f;
YutaTogashi 0:630126b8994f 42 }
YutaTogashi 0:630126b8994f 43 };
YutaTogashi 0:630126b8994f 44
YutaTogashi 1:4bc4c63ea283 45 void Pid::reset() {
YutaTogashi 1:4bc4c63ea283 46 duty = 0.0f;
YutaTogashi 1:4bc4c63ea283 47 }
YutaTogashi 1:4bc4c63ea283 48
YutaTogashi 0:630126b8994f 49 float Pid::getDuty() {
YutaTogashi 0:630126b8994f 50 return duty;
YutaTogashi 0:630126b8994f 51 }