Kazushi Yamanobe
/
R-ciliatable_4
latest version 9/26
pid.cpp@0:0d02a451e79d, 2021-10-10 (annotated)
- Committer:
- Yamanobe
- Date:
- Sun Oct 10 04:47:14 2021 +0000
- Revision:
- 0:0d02a451e79d
slave program (at 9/26)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yamanobe | 0:0d02a451e79d | 1 | #include "pid.h" |
Yamanobe | 0:0d02a451e79d | 2 | |
Yamanobe | 0:0d02a451e79d | 3 | void PositionPid::setup(float Kp, float Ki, float Kd, float dt){ |
Yamanobe | 0:0d02a451e79d | 4 | kp = Kp; |
Yamanobe | 0:0d02a451e79d | 5 | ki = Ki; |
Yamanobe | 0:0d02a451e79d | 6 | kd = Kd; |
Yamanobe | 0:0d02a451e79d | 7 | time = dt; |
Yamanobe | 0:0d02a451e79d | 8 | frequency = 1/dt; |
Yamanobe | 0:0d02a451e79d | 9 | } |
Yamanobe | 0:0d02a451e79d | 10 | |
Yamanobe | 0:0d02a451e79d | 11 | void PositionPid::calculate(float target, float nowValue){ |
Yamanobe | 0:0d02a451e79d | 12 | old = now; |
Yamanobe | 0:0d02a451e79d | 13 | now = nowValue - target; |
Yamanobe | 0:0d02a451e79d | 14 | p = now; |
Yamanobe | 0:0d02a451e79d | 15 | i = i + (now + old) * 0.5f * time; |
Yamanobe | 0:0d02a451e79d | 16 | d = (now - old) * frequency; |
Yamanobe | 0:0d02a451e79d | 17 | result = kp*p + ki*i + kd*d; |
Yamanobe | 0:0d02a451e79d | 18 | if (result > 1.0f) { |
Yamanobe | 0:0d02a451e79d | 19 | result = 1.0f; |
Yamanobe | 0:0d02a451e79d | 20 | } else if (result < -1.0f) { |
Yamanobe | 0:0d02a451e79d | 21 | result = -1.0f; |
Yamanobe | 0:0d02a451e79d | 22 | } |
Yamanobe | 0:0d02a451e79d | 23 | } |
Yamanobe | 0:0d02a451e79d | 24 | |
Yamanobe | 0:0d02a451e79d | 25 | float PositionPid::duty(){ |
Yamanobe | 0:0d02a451e79d | 26 | return result; |
Yamanobe | 0:0d02a451e79d | 27 | } |
Yamanobe | 0:0d02a451e79d | 28 | |
Yamanobe | 0:0d02a451e79d | 29 | void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){ |
Yamanobe | 0:0d02a451e79d | 30 | kp = Kp; |
Yamanobe | 0:0d02a451e79d | 31 | ki = Ki; |
Yamanobe | 0:0d02a451e79d | 32 | kd = Kd; |
Yamanobe | 0:0d02a451e79d | 33 | time = dt; |
Yamanobe | 0:0d02a451e79d | 34 | frequency = 1/dt; |
Yamanobe | 0:0d02a451e79d | 35 | } |
Yamanobe | 0:0d02a451e79d | 36 | |
Yamanobe | 0:0d02a451e79d | 37 | void SpeedPid::calculate(float target, float nowValue){ |
Yamanobe | 0:0d02a451e79d | 38 | e2 = e1; |
Yamanobe | 0:0d02a451e79d | 39 | e1 = e; |
Yamanobe | 0:0d02a451e79d | 40 | e = nowValue - target; |
Yamanobe | 0:0d02a451e79d | 41 | p = e - e1; |
Yamanobe | 0:0d02a451e79d | 42 | i = e*time; |
Yamanobe | 0:0d02a451e79d | 43 | d = (e-2*e1+e2)*frequency; |
Yamanobe | 0:0d02a451e79d | 44 | result = result + (kp*p+ki*i+kd*d); |
Yamanobe | 0:0d02a451e79d | 45 | if (result > 1.0f) { |
Yamanobe | 0:0d02a451e79d | 46 | result = 1.0f; |
Yamanobe | 0:0d02a451e79d | 47 | } else if (result < -1.0f) { |
Yamanobe | 0:0d02a451e79d | 48 | result = -1.0f; |
Yamanobe | 0:0d02a451e79d | 49 | } |
Yamanobe | 0:0d02a451e79d | 50 | } |
Yamanobe | 0:0d02a451e79d | 51 | |
Yamanobe | 0:0d02a451e79d | 52 | float SpeedPid::duty(){ |
Yamanobe | 0:0d02a451e79d | 53 | return result; |
Yamanobe | 0:0d02a451e79d | 54 | } |