Yuta Togashi / PID

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

Dependents:   Tourobo2022_TBCMotorDriver

Pid.cpp@6:4074aded9b9d, 2021-12-30 (annotated)

Committer:
YutaTogashi
Date:
Thu Dec 30 16:07:37 2021 +0000
Revision:
6:4074aded9b9d
Parent:
5:a3aa705d9023 
20211231 add resetFunction now=0.0f,before=0.0f

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 4:3c2651359136 9 upperLimit = 1.0f;
YutaTogashi 5:a3aa705d9023 10 fallLimit = -1.0f;
YutaTogashi 0:630126b8994f 11 //pid.attach(this,&Pid::calculate,period);
YutaTogashi 0:630126b8994f 12 }
YutaTogashi 0:630126b8994f 13
YutaTogashi 4:3c2651359136 14 void Pid::setupLimit(float UpperLimit,float FallLimit) {
YutaTogashi 4:3c2651359136 15 upperLimit = UpperLimit;
YutaTogashi 4:3c2651359136 16 fallLimit = FallLimit;
YutaTogashi 4:3c2651359136 17 }
YutaTogashi 4:3c2651359136 18
YutaTogashi 2:7fede27af6ca 19 void Pid::calculate(float targetValue,float nowValue,bool enableErrorIntegration) {
YutaTogashi 0:630126b8994f 20 switch(MODE) {
YutaTogashi 0:630126b8994f 21 case 0:
YutaTogashi 0:630126b8994f 22 before = now;
YutaTogashi 0:630126b8994f 23 now = targetValue - nowValue;
YutaTogashi 0:630126b8994f 24 p = now;
YutaTogashi 2:7fede27af6ca 25 if(enableErrorIntegration) i += (now + before) * PERIOD / 2; //積分
YutaTogashi 2:7fede27af6ca 26 else i = 0.0f;
YutaTogashi 2:7fede27af6ca 27 d = (now - before) / PERIOD; //微分
YutaTogashi 0:630126b8994f 28 //target_duty = p * KP + i * KI + d * KD;
YutaTogashi 0:630126b8994f 29 duty = p * KP + i * KI + d * KD;
YutaTogashi 0:630126b8994f 30 break;
YutaTogashi 0:630126b8994f 31 case 1:
YutaTogashi 0:630126b8994f 32 e2 = e1;
YutaTogashi 0:630126b8994f 33 e1 = e;
YutaTogashi 0:630126b8994f 34 e = targetValue - nowValue;
YutaTogashi 0:630126b8994f 35 //p = (e - e1) / PERIOD;
YutaTogashi 0:630126b8994f 36 p = e / PERIOD;
YutaTogashi 0:630126b8994f 37 i = e;
YutaTogashi 0:630126b8994f 38 d = (e - 2*e1 + e2)/PERIOD;
YutaTogashi 0:630126b8994f 39 duty += p * KP + i * KI + d * KD; //dutyは微分されたものなので、積分してあげる(もとに戻す)
YutaTogashi 0:630126b8994f 40 break;
YutaTogashi 0:630126b8994f 41 default:
YutaTogashi 0:630126b8994f 42 break;
YutaTogashi 0:630126b8994f 43 }
YutaTogashi 0:630126b8994f 44
YutaTogashi 4:3c2651359136 45 if(duty > upperLimit) {
YutaTogashi 4:3c2651359136 46 duty = upperLimit;
YutaTogashi 4:3c2651359136 47 } else if (duty < fallLimit) {
YutaTogashi 4:3c2651359136 48 duty = fallLimit;
YutaTogashi 0:630126b8994f 49 }
YutaTogashi 0:630126b8994f 50 };
YutaTogashi 0:630126b8994f 51
YutaTogashi 1:4bc4c63ea283 52 void Pid::reset() {
YutaTogashi 1:4bc4c63ea283 53 duty = 0.0f;
YutaTogashi 6:4074aded9b9d 54 now = 0.0f;
YutaTogashi 6:4074aded9b9d 55 before = 0.0f;
YutaTogashi 1:4bc4c63ea283 56 }
YutaTogashi 1:4bc4c63ea283 57
YutaTogashi 0:630126b8994f 58 float Pid::getDuty() {
YutaTogashi 0:630126b8994f 59 return duty;
YutaTogashi 0:630126b8994f 60 }