Moved library files out, fixed up code to use right motor & encoder