ケンタ ミヤザキ
Dependents: nhk_2018_undercarry_test04 nhk_2018_undercarry_test08 nhk_2018_undercarry_test09 nhk_2018_undercarry_test10 ... more
PID.cpp@3:3bc8034f569e, 2018-03-24 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Mar 24 10:50:11 2018 +0000
- Revision:
- 3:3bc8034f569e
- Parent:
- 2:09a00a9407f8
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 | 3:3bc8034f569e | 13 | //timer.reset(); |
| kenken0721 | 3:3bc8034f569e | 14 | //timer.start(); |
| kenken0721 | 3:3bc8034f569e | 15 | preP = 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 | 3:3bc8034f569e | 22 | //dt = timer.read_ms() / 10; |
| kenken0721 | 3:3bc8034f569e | 23 | //timer.reset(); |
| kenken0721 | 0:d8b5fa590ca2 | 24 | P = Setpoint - input; |
| kenken0721 | 3:3bc8034f569e | 25 | I += P; |
| kenken0721 | 3:3bc8034f569e | 26 | D = (P - preP); |
| kenken0721 | 0:d8b5fa590ca2 | 27 | preP = P; |
| kenken0721 | 0:d8b5fa590ca2 | 28 | return (KP * P + KI * I + KD * D); |
| kenken0721 | 0:d8b5fa590ca2 | 29 | } |