Updated the encoder code to make the loop faster and fixed some problems
with serial write.