More error-checking, explicit motor reset func
Cleaned up some comments