Code to use encoders

This code (theoretically) works with the encoders; servo pins probably
need to be updated appropriately.