fixed enormous bug in servo angle calculation.