Servo Diags in progress

just finished yn_to_tf, yet untested.