I found small problem in your encoder stream read function routine which lead to encoder reading bug when changing direction. and that is don't executing until next step after changing direction which ...