improved motor drive function and debug code