Fixed the motor control to stop with the encoder