Added motor control to code