Rewrite of motor control sub