Added motor init and cleaned up commented code