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?
User | Revision | Line number | New 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 | } |