Fix driving algorithm.

Signed-off-by: Kiril Nesenko <nesenko@gmail.com>