Changes to get servo and encoders working

Tested on Pie robot. Added test functions for tuning turns and the IR
sensor. Motor encoder distance and turns work.