Motor control algorithm