Fixes in the direction change code and in PI controller.