Servo calibration. Rewritten & untested -- pushing so it can be looked at.

Calibration uses the same base code as normal operation, so some of that is in here.