Motors working, test code in main