Implement advanced SM control algorithm with fast response (direction can be changed during motion)