Both encoders working, initial work on distance measurement