Fixed some errors in servo control.