![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Diff: verwerking/pid.cpp
- Revision:
- 0:bec310bde899
diff -r 000000000000 -r bec310bde899 verwerking/pid.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/pid.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,40 @@ +#include "pid.h" + +Pid::Pid(Goto * p, Kompas * k, Roer * r){ + sp = p; + pv = k; + output = r; + dt = 0.1; + + kp = 1.2; + ki = 1; + kd = 0.1; + + prev_error = 0; +} + +void Pid::update(void){ + double error = verschil(pv->get(),sp->get()); + + p = error * kp; + i = i + error * dt; + if (i > 25) + i = 25; + if (i < -25) + i = -25; + i *= ki; + + d = (error - prev_error)/dt * kd; + o = p+i+d + 50; + + if (o > 100) + o = 100; + if (0 < 0) + o = 0; + + + output->set(o); + prev_error = error; + + +}