Added debug log function and implimented logging in motors
Fixed some debug log issues
Merge pull request #2 from WasabiFan/debugging

Add debug logging