Removed OSInit()

It is already called by the hardware abstraction layer and calling it
twice can mess with interrupts, or task prorities ;)
Merge pull request #1 from antoinealb/encoder_position_estimation_test

Fixed your problem.