Dependents: nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
PID.cpp@2:09a00a9407f8, 2018-03-21 (annotated)
- Committer:
- kenken0721
- Date:
- Wed Mar 21 06:51:29 2018 +0000
- Revision:
- 2:09a00a9407f8
- Parent:
- 1:90e1008a1bf9
- Child:
- 3:3bc8034f569e
?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenken0721 | 0:d8b5fa590ca2 | 1 | #include "PID.h" |
kenken0721 | 0:d8b5fa590ca2 | 2 | #include "mbed.h" |
kenken0721 | 0:d8b5fa590ca2 | 3 | |
kenken0721 | 0:d8b5fa590ca2 | 4 | PID::PID(double kp, double ki, double kd, double setpoint){ |
kenken0721 | 0:d8b5fa590ca2 | 5 | KP = kp; |
kenken0721 | 0:d8b5fa590ca2 | 6 | KI = ki; |
kenken0721 | 0:d8b5fa590ca2 | 7 | KD = kd; |
kenken0721 | 0:d8b5fa590ca2 | 8 | Setpoint = setpoint; |
kenken0721 | 2:09a00a9407f8 | 9 | //init(); |
kenken0721 | 0:d8b5fa590ca2 | 10 | } |
kenken0721 | 0:d8b5fa590ca2 | 11 | |
kenken0721 | 0:d8b5fa590ca2 | 12 | void PID::init(){ |
kenken0721 | 0:d8b5fa590ca2 | 13 | timer.reset(); |
kenken0721 | 0:d8b5fa590ca2 | 14 | timer.start(); |
kenken0721 | 0:d8b5fa590ca2 | 15 | preTime = 0; |
kenken0721 | 0:d8b5fa590ca2 | 16 | P = 0; |
kenken0721 | 0:d8b5fa590ca2 | 17 | I = 0; |
kenken0721 | 0:d8b5fa590ca2 | 18 | D = 0; |
kenken0721 | 0:d8b5fa590ca2 | 19 | } |
kenken0721 | 0:d8b5fa590ca2 | 20 | |
kenken0721 | 0:d8b5fa590ca2 | 21 | double PID::compute(double input){ |
kenken0721 | 1:90e1008a1bf9 | 22 | dt = (timer.read_us() - preTime) / 10000; |
kenken0721 | 1:90e1008a1bf9 | 23 | preTime = timer.read_us(); |
kenken0721 | 0:d8b5fa590ca2 | 24 | P = Setpoint - input; |
kenken0721 | 0:d8b5fa590ca2 | 25 | I += P * dt; |
kenken0721 | 0:d8b5fa590ca2 | 26 | D = (P - preP) / dt; |
kenken0721 | 0:d8b5fa590ca2 | 27 | preP = P; |
kenken0721 | 0:d8b5fa590ca2 | 28 | return (KP * P + KI * I + KD * D); |
kenken0721 | 0:d8b5fa590ca2 | 29 | } |