Minor cleanup in servo class.

Merged two functions that were always called together.