Integral Gain Implementation

[Fabian Gerlinghaus]

Btw, just noticed something: In the mails with Torsten you said this is your formula:

IntegralForce(t) = IntegralForce(t-1) + integral_gain_ * pos_err * (t_curr - t_pre) / integral_gain_time_constt_;

If I'm not mistaken what you effectively implemented, however, is this:

IntegralForce(t) = (IntegralForce(t-1) + integral_gain_ * pos_err) * (t_curr - t_pre) / integral_gain_time_constt_;

[Samir Menon]

Thanks. You are correct.. Both actually work. Though the one I implemented decays much faster... I'll convert it to the first one.

[Samir Menon]

Fixed as of 676c9a7d0f9b43c5c72beec0f0af5482906df59f