Corrected derivative equation for pid control of motors

Hi Georg,

I have checked your latest lpzrobots with foot contact sensors. It works fine.
We just found out that derivative eq in pid.cpp is wrong. I have corrected it and here send you pull request.
You could check it. One of my student he uses this function for his motor control that why we found this error.
KOH