Added ability to calibrate servos