![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
verwerking/pid.cpp@0:bec310bde899, 2011-09-27 (annotated)
- Committer:
- pd0wm
- Date:
- Tue Sep 27 19:46:30 2011 +0000
- Revision:
- 0:bec310bde899
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pd0wm | 0:bec310bde899 | 1 | #include "pid.h" |
pd0wm | 0:bec310bde899 | 2 | |
pd0wm | 0:bec310bde899 | 3 | Pid::Pid(Goto * p, Kompas * k, Roer * r){ |
pd0wm | 0:bec310bde899 | 4 | sp = p; |
pd0wm | 0:bec310bde899 | 5 | pv = k; |
pd0wm | 0:bec310bde899 | 6 | output = r; |
pd0wm | 0:bec310bde899 | 7 | dt = 0.1; |
pd0wm | 0:bec310bde899 | 8 | |
pd0wm | 0:bec310bde899 | 9 | kp = 1.2; |
pd0wm | 0:bec310bde899 | 10 | ki = 1; |
pd0wm | 0:bec310bde899 | 11 | kd = 0.1; |
pd0wm | 0:bec310bde899 | 12 | |
pd0wm | 0:bec310bde899 | 13 | prev_error = 0; |
pd0wm | 0:bec310bde899 | 14 | } |
pd0wm | 0:bec310bde899 | 15 | |
pd0wm | 0:bec310bde899 | 16 | void Pid::update(void){ |
pd0wm | 0:bec310bde899 | 17 | double error = verschil(pv->get(),sp->get()); |
pd0wm | 0:bec310bde899 | 18 | |
pd0wm | 0:bec310bde899 | 19 | p = error * kp; |
pd0wm | 0:bec310bde899 | 20 | i = i + error * dt; |
pd0wm | 0:bec310bde899 | 21 | if (i > 25) |
pd0wm | 0:bec310bde899 | 22 | i = 25; |
pd0wm | 0:bec310bde899 | 23 | if (i < -25) |
pd0wm | 0:bec310bde899 | 24 | i = -25; |
pd0wm | 0:bec310bde899 | 25 | i *= ki; |
pd0wm | 0:bec310bde899 | 26 | |
pd0wm | 0:bec310bde899 | 27 | d = (error - prev_error)/dt * kd; |
pd0wm | 0:bec310bde899 | 28 | o = p+i+d + 50; |
pd0wm | 0:bec310bde899 | 29 | |
pd0wm | 0:bec310bde899 | 30 | if (o > 100) |
pd0wm | 0:bec310bde899 | 31 | o = 100; |
pd0wm | 0:bec310bde899 | 32 | if (0 < 0) |
pd0wm | 0:bec310bde899 | 33 | o = 0; |
pd0wm | 0:bec310bde899 | 34 | |
pd0wm | 0:bec310bde899 | 35 | |
pd0wm | 0:bec310bde899 | 36 | output->set(o); |
pd0wm | 0:bec310bde899 | 37 | prev_error = error; |
pd0wm | 0:bec310bde899 | 38 | |
pd0wm | 0:bec310bde899 | 39 | |
pd0wm | 0:bec310bde899 | 40 | } |