How to integrate position error, add to speed loop