That code looks familiar
There is one addition on the end that is likely causing you problems.
In general everything works. If your commanded angle happens to be 0 then regardless of what your current angle is you skip the optimization. In the case of being optimized at 180 and then getting a 0 angle no rotation is required but due to the additional check it thinks a 180 degree rotation is required so it spins 180.
What problem were you trying to solve by adding the additional check?