Corrected compilation issue and improve stability

Corrected compilation issue
Improved stability, the robot do not behave always the same manners,
sometime it is very stable and some other not, but I suppose this is because the controllers runs asynchronously.
Do you want I try to make it more stable ?