bugfixed motorDrive func and added debug mode by ifdef preprocessor