Added motor controller and cleaned it up