Add motor scaling and debug prints